001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj.simulation;
006
007import edu.wpi.first.math.Matrix;
008import edu.wpi.first.math.VecBuilder;
009import edu.wpi.first.math.numbers.N1;
010import edu.wpi.first.math.numbers.N2;
011import edu.wpi.first.math.system.LinearSystem;
012import edu.wpi.first.math.system.NumericalIntegration;
013import edu.wpi.first.math.system.plant.DCMotor;
014import edu.wpi.first.math.system.plant.LinearSystemId;
015
016/** Represents a simulated elevator mechanism. */
017public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
018  // Gearbox for the elevator.
019  private final DCMotor m_gearbox;
020
021  // Gearing between the motors and the output.
022  private final double m_gearing;
023
024  // The radius of the drum that the elevator spool is wrapped around.
025  private final double m_drumRadius;
026
027  // The min allowable height for the elevator.
028  private final double m_minHeight;
029
030  // The max allowable height for the elevator.
031  private final double m_maxHeight;
032
033  /**
034   * Creates a simulated elevator mechanism.
035   *
036   * @param plant The linear system that represents the elevator.
037   * @param gearbox The type of and number of motors in the elevator gearbox.
038   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
039   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
040   * @param minHeightMeters The min allowable height of the elevator.
041   * @param maxHeightMeters The max allowable height of the elevator.
042   */
043  public ElevatorSim(
044      LinearSystem<N2, N1, N1> plant,
045      DCMotor gearbox,
046      double gearing,
047      double drumRadiusMeters,
048      double minHeightMeters,
049      double maxHeightMeters) {
050    this(plant, gearbox, gearing, drumRadiusMeters, minHeightMeters, maxHeightMeters, null);
051  }
052
053  /**
054   * Creates a simulated elevator mechanism.
055   *
056   * @param plant The linear system that represents the elevator.
057   * @param gearbox The type of and number of motors in the elevator gearbox.
058   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
059   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
060   * @param minHeightMeters The min allowable height of the elevator.
061   * @param maxHeightMeters The max allowable height of the elevator.
062   * @param measurementStdDevs The standard deviations of the measurements.
063   */
064  public ElevatorSim(
065      LinearSystem<N2, N1, N1> plant,
066      DCMotor gearbox,
067      double gearing,
068      double drumRadiusMeters,
069      double minHeightMeters,
070      double maxHeightMeters,
071      Matrix<N1, N1> measurementStdDevs) {
072    super(plant, measurementStdDevs);
073    m_gearbox = gearbox;
074    m_gearing = gearing;
075    m_drumRadius = drumRadiusMeters;
076    m_minHeight = minHeightMeters;
077    m_maxHeight = maxHeightMeters;
078  }
079
080  /**
081   * Creates a simulated elevator mechanism.
082   *
083   * @param gearbox The type of and number of motors in the elevator gearbox.
084   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
085   * @param carriageMassKg The mass of the elevator carriage.
086   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
087   * @param minHeightMeters The min allowable height of the elevator.
088   * @param maxHeightMeters The max allowable height of the elevator.
089   */
090  public ElevatorSim(
091      DCMotor gearbox,
092      double gearing,
093      double carriageMassKg,
094      double drumRadiusMeters,
095      double minHeightMeters,
096      double maxHeightMeters) {
097    this(
098        gearbox, gearing, carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters, null);
099  }
100
101  /**
102   * Creates a simulated elevator mechanism.
103   *
104   * @param gearbox The type of and number of motors in the elevator gearbox.
105   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
106   * @param carriageMassKg The mass of the elevator carriage.
107   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
108   * @param minHeightMeters The min allowable height of the elevator.
109   * @param maxHeightMeters The max allowable height of the elevator.
110   * @param measurementStdDevs The standard deviations of the measurements.
111   */
112  public ElevatorSim(
113      DCMotor gearbox,
114      double gearing,
115      double carriageMassKg,
116      double drumRadiusMeters,
117      double minHeightMeters,
118      double maxHeightMeters,
119      Matrix<N1, N1> measurementStdDevs) {
120    super(
121        LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
122        measurementStdDevs);
123    m_gearbox = gearbox;
124    m_gearing = gearing;
125    m_drumRadius = drumRadiusMeters;
126    m_minHeight = minHeightMeters;
127    m_maxHeight = maxHeightMeters;
128  }
129
130  /**
131   * Returns whether the elevator would hit the lower limit.
132   *
133   * @param elevatorHeightMeters The elevator height.
134   * @return Whether the elevator would hit the lower limit.
135   */
136  public boolean wouldHitLowerLimit(double elevatorHeightMeters) {
137    return elevatorHeightMeters < this.m_minHeight;
138  }
139
140  /**
141   * Returns whether the elevator would hit the upper limit.
142   *
143   * @param elevatorHeightMeters The elevator height.
144   * @return Whether the elevator would hit the upper limit.
145   */
146  public boolean wouldHitUpperLimit(double elevatorHeightMeters) {
147    return elevatorHeightMeters > this.m_maxHeight;
148  }
149
150  /**
151   * Returns whether the elevator has hit the lower limit.
152   *
153   * @return Whether the elevator has hit the lower limit.
154   */
155  public boolean hasHitLowerLimit() {
156    return wouldHitLowerLimit(getPositionMeters());
157  }
158
159  /**
160   * Returns whether the elevator has hit the upper limit.
161   *
162   * @return Whether the elevator has hit the upper limit.
163   */
164  public boolean hasHitUpperLimit() {
165    return wouldHitUpperLimit(getPositionMeters());
166  }
167
168  /**
169   * Returns the position of the elevator.
170   *
171   * @return The position of the elevator.
172   */
173  public double getPositionMeters() {
174    return getOutput(0);
175  }
176
177  /**
178   * Returns the velocity of the elevator.
179   *
180   * @return The velocity of the elevator.
181   */
182  public double getVelocityMetersPerSecond() {
183    return m_x.get(1, 0);
184  }
185
186  /**
187   * Returns the elevator current draw.
188   *
189   * @return The elevator current draw.
190   */
191  @Override
192  public double getCurrentDrawAmps() {
193    // I = V / R - omega / (Kv * R)
194    // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
195    // spinning 10x faster than the output
196    // v = r w, so w = v/r
197    double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadius * m_gearing;
198    var appliedVoltage = m_u.get(0, 0);
199    return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage)
200        * Math.signum(appliedVoltage);
201  }
202
203  /**
204   * Sets the input voltage for the elevator.
205   *
206   * @param volts The input voltage.
207   */
208  public void setInputVoltage(double volts) {
209    setInput(volts);
210  }
211
212  /**
213   * Updates the state of the elevator.
214   *
215   * @param currentXhat The current state estimate.
216   * @param u The system inputs (voltage).
217   * @param dtSeconds The time difference between controller updates.
218   */
219  @SuppressWarnings({"ParameterName", "LambdaParameterName"})
220  @Override
221  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
222    // Calculate updated x-hat from Runge-Kutta.
223    var updatedXhat =
224        NumericalIntegration.rkdp(
225            (x, u_) ->
226                (m_plant.getA().times(x))
227                    .plus(m_plant.getB().times(u_))
228                    .plus(VecBuilder.fill(0, -9.8)),
229            currentXhat,
230            u,
231            dtSeconds);
232
233    // We check for collisions after updating x-hat.
234    if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
235      return VecBuilder.fill(m_minHeight, 0);
236    }
237    if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
238      return VecBuilder.fill(m_maxHeight, 0);
239    }
240    return updatedXhat;
241  }
242}