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.MathUtil;
008import edu.wpi.first.math.Matrix;
009import edu.wpi.first.math.VecBuilder;
010import edu.wpi.first.math.numbers.N1;
011import edu.wpi.first.math.numbers.N2;
012import edu.wpi.first.math.system.LinearSystem;
013import edu.wpi.first.math.system.NumericalIntegration;
014import edu.wpi.first.math.system.plant.DCMotor;
015import edu.wpi.first.math.system.plant.LinearSystemId;
016import edu.wpi.first.wpilibj.RobotController;
017
018/** Represents a simulated elevator mechanism. */
019public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
020  // Gearbox for the elevator.
021  private final DCMotor m_gearbox;
022
023  // The min allowable height for the elevator.
024  private final double m_minHeight;
025
026  // The max allowable height for the elevator.
027  private final double m_maxHeight;
028
029  // Whether the simulator should simulate gravity.
030  private final boolean m_simulateGravity;
031
032  /**
033   * Creates a simulated elevator mechanism.
034   *
035   * @param plant The linear system that represents the elevator. This system can be created with
036   *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double,
037   *     double, double)}.
038   * @param gearbox The type of and number of motors in the elevator gearbox.
039   * @param minHeightMeters The min allowable height of the elevator.
040   * @param maxHeightMeters The max allowable height of the elevator.
041   * @param simulateGravity Whether gravity should be simulated or not.
042   * @param startingHeightMeters The starting height of the elevator.
043   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
044   *     noise is desired. If present must have 1 element for position.
045   */
046  @SuppressWarnings("this-escape")
047  public ElevatorSim(
048      LinearSystem<N2, N1, N2> plant,
049      DCMotor gearbox,
050      double minHeightMeters,
051      double maxHeightMeters,
052      boolean simulateGravity,
053      double startingHeightMeters,
054      double... measurementStdDevs) {
055    super(plant, measurementStdDevs);
056    m_gearbox = gearbox;
057    m_minHeight = minHeightMeters;
058    m_maxHeight = maxHeightMeters;
059    m_simulateGravity = simulateGravity;
060
061    setState(startingHeightMeters, 0);
062  }
063
064  /**
065   * Creates a simulated elevator mechanism.
066   *
067   * @param kV The velocity gain.
068   * @param kA The acceleration gain.
069   * @param gearbox The type of and number of motors in the elevator gearbox.
070   * @param minHeightMeters The min allowable height of the elevator.
071   * @param maxHeightMeters The max allowable height of the elevator.
072   * @param simulateGravity Whether gravity should be simulated or not.
073   * @param startingHeightMeters The starting height of the elevator.
074   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
075   *     noise is desired. If present must have 1 element for position.
076   */
077  public ElevatorSim(
078      double kV,
079      double kA,
080      DCMotor gearbox,
081      double minHeightMeters,
082      double maxHeightMeters,
083      boolean simulateGravity,
084      double startingHeightMeters,
085      double... measurementStdDevs) {
086    this(
087        LinearSystemId.identifyPositionSystem(kV, kA),
088        gearbox,
089        minHeightMeters,
090        maxHeightMeters,
091        simulateGravity,
092        startingHeightMeters,
093        measurementStdDevs);
094  }
095
096  /**
097   * Creates a simulated elevator mechanism.
098   *
099   * @param gearbox The type of and number of motors in the elevator gearbox.
100   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
101   * @param carriageMassKg The mass of the elevator carriage.
102   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
103   * @param minHeightMeters The min allowable height of the elevator.
104   * @param maxHeightMeters The max allowable height of the elevator.
105   * @param simulateGravity Whether gravity should be simulated or not.
106   * @param startingHeightMeters The starting height of the elevator.
107   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
108   *     noise is desired. If present must have 1 element for position.
109   */
110  public ElevatorSim(
111      DCMotor gearbox,
112      double gearing,
113      double carriageMassKg,
114      double drumRadiusMeters,
115      double minHeightMeters,
116      double maxHeightMeters,
117      boolean simulateGravity,
118      double startingHeightMeters,
119      double... measurementStdDevs) {
120    this(
121        LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
122        gearbox,
123        minHeightMeters,
124        maxHeightMeters,
125        simulateGravity,
126        startingHeightMeters,
127        measurementStdDevs);
128  }
129
130  /**
131   * Sets the elevator's state. The new position will be limited between the minimum and maximum
132   * allowed heights.
133   *
134   * @param positionMeters The new position in meters.
135   * @param velocityMetersPerSecond New velocity in meters per second.
136   */
137  public final void setState(double positionMeters, double velocityMetersPerSecond) {
138    setState(
139        VecBuilder.fill(
140            MathUtil.clamp(positionMeters, m_minHeight, m_maxHeight), velocityMetersPerSecond));
141  }
142
143  /**
144   * Returns whether the elevator would hit the lower limit.
145   *
146   * @param elevatorHeightMeters The elevator height.
147   * @return Whether the elevator would hit the lower limit.
148   */
149  public boolean wouldHitLowerLimit(double elevatorHeightMeters) {
150    return elevatorHeightMeters <= this.m_minHeight;
151  }
152
153  /**
154   * Returns whether the elevator would hit the upper limit.
155   *
156   * @param elevatorHeightMeters The elevator height.
157   * @return Whether the elevator would hit the upper limit.
158   */
159  public boolean wouldHitUpperLimit(double elevatorHeightMeters) {
160    return elevatorHeightMeters >= this.m_maxHeight;
161  }
162
163  /**
164   * Returns whether the elevator has hit the lower limit.
165   *
166   * @return Whether the elevator has hit the lower limit.
167   */
168  public boolean hasHitLowerLimit() {
169    return wouldHitLowerLimit(getPositionMeters());
170  }
171
172  /**
173   * Returns whether the elevator has hit the upper limit.
174   *
175   * @return Whether the elevator has hit the upper limit.
176   */
177  public boolean hasHitUpperLimit() {
178    return wouldHitUpperLimit(getPositionMeters());
179  }
180
181  /**
182   * Returns the position of the elevator.
183   *
184   * @return The position of the elevator.
185   */
186  public double getPositionMeters() {
187    return getOutput(0);
188  }
189
190  /**
191   * Returns the velocity of the elevator.
192   *
193   * @return The velocity of the elevator.
194   */
195  public double getVelocityMetersPerSecond() {
196    return getOutput(1);
197  }
198
199  /**
200   * Returns the elevator current draw.
201   *
202   * @return The elevator current draw.
203   */
204  public double getCurrentDrawAmps() {
205    // I = V / R - omega / (Kv * R)
206    // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
207    // spinning 10x faster than the output
208    // v = r w, so w = v/r
209    double kA = 1 / m_plant.getB().get(1, 0);
210    double kV = -m_plant.getA().get(1, 1) * kA;
211    double motorVelocityRadPerSec = m_x.get(1, 0) * kV * m_gearbox.KvRadPerSecPerVolt;
212    var appliedVoltage = m_u.get(0, 0);
213    return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage)
214        * Math.signum(appliedVoltage);
215  }
216
217  /**
218   * Sets the input voltage for the elevator.
219   *
220   * @param volts The input voltage.
221   */
222  public void setInputVoltage(double volts) {
223    setInput(volts);
224    clampInput(RobotController.getBatteryVoltage());
225  }
226
227  /**
228   * Updates the state of the elevator.
229   *
230   * @param currentXhat The current state estimate.
231   * @param u The system inputs (voltage).
232   * @param dtSeconds The time difference between controller updates.
233   */
234  @Override
235  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
236    // Calculate updated x-hat from Runge-Kutta.
237    var updatedXhat =
238        NumericalIntegration.rkdp(
239            (x, _u) -> {
240              Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
241              if (m_simulateGravity) {
242                xdot = xdot.plus(VecBuilder.fill(0, -9.8));
243              }
244              return xdot;
245            },
246            currentXhat,
247            u,
248            dtSeconds);
249
250    // We check for collisions after updating x-hat.
251    if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
252      return VecBuilder.fill(m_minHeight, 0);
253    }
254    if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
255      return VecBuilder.fill(m_maxHeight, 0);
256    }
257    return updatedXhat;
258  }
259}