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 minHeight The min allowable height of the elevator in meters.
040   * @param maxHeight The max allowable height of the elevator in meters.
041   * @param simulateGravity Whether gravity should be simulated or not.
042   * @param startingHeight The starting height of the elevator in meters.
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 minHeight,
051      double maxHeight,
052      boolean simulateGravity,
053      double startingHeight,
054      double... measurementStdDevs) {
055    super(plant, measurementStdDevs);
056    m_gearbox = gearbox;
057    m_minHeight = minHeight;
058    m_maxHeight = maxHeight;
059    m_simulateGravity = simulateGravity;
060
061    setState(startingHeight, 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 minHeight The min allowable height of the elevator in meters.
071   * @param maxHeight The max allowable height of the elevator in meters.
072   * @param simulateGravity Whether gravity should be simulated or not.
073   * @param startingHeight The starting height of the elevator in meters.
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 minHeight,
082      double maxHeight,
083      boolean simulateGravity,
084      double startingHeight,
085      double... measurementStdDevs) {
086    this(
087        LinearSystemId.identifyPositionSystem(kV, kA),
088        gearbox,
089        minHeight,
090        maxHeight,
091        simulateGravity,
092        startingHeight,
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 carriageMass The mass of the elevator carriage in kg.
102   * @param drumRadius The radius of the drum that the elevator spool is wrapped around in meters.
103   * @param minHeight The min allowable height of the elevator in meters.
104   * @param maxHeight The max allowable height of the elevator in meters.
105   * @param simulateGravity Whether gravity should be simulated or not.
106   * @param startingHeight The starting height of the elevator in meters.
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 carriageMass,
114      double drumRadius,
115      double minHeight,
116      double maxHeight,
117      boolean simulateGravity,
118      double startingHeight,
119      double... measurementStdDevs) {
120    this(
121        LinearSystemId.createElevatorSystem(gearbox, carriageMass, drumRadius, gearing),
122        gearbox,
123        minHeight,
124        maxHeight,
125        simulateGravity,
126        startingHeight,
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 position The new position in meters.
135   * @param velocity New velocity in meters per second.
136   */
137  public final void setState(double position, double velocity) {
138    setState(VecBuilder.fill(MathUtil.clamp(position, m_minHeight, m_maxHeight), velocity));
139  }
140
141  /**
142   * Returns whether the elevator would hit the lower limit.
143   *
144   * @param elevatorHeight The elevator height in meters.
145   * @return Whether the elevator would hit the lower limit.
146   */
147  public boolean wouldHitLowerLimit(double elevatorHeight) {
148    return elevatorHeight <= this.m_minHeight;
149  }
150
151  /**
152   * Returns whether the elevator would hit the upper limit.
153   *
154   * @param elevatorHeight The elevator height in meters.
155   * @return Whether the elevator would hit the upper limit.
156   */
157  public boolean wouldHitUpperLimit(double elevatorHeight) {
158    return elevatorHeight >= this.m_maxHeight;
159  }
160
161  /**
162   * Returns whether the elevator has hit the lower limit.
163   *
164   * @return Whether the elevator has hit the lower limit.
165   */
166  public boolean hasHitLowerLimit() {
167    return wouldHitLowerLimit(getPosition());
168  }
169
170  /**
171   * Returns whether the elevator has hit the upper limit.
172   *
173   * @return Whether the elevator has hit the upper limit.
174   */
175  public boolean hasHitUpperLimit() {
176    return wouldHitUpperLimit(getPosition());
177  }
178
179  /**
180   * Returns the position of the elevator.
181   *
182   * @return The position of the elevator in meters.
183   */
184  public double getPosition() {
185    return getOutput(0);
186  }
187
188  /**
189   * Returns the velocity of the elevator.
190   *
191   * @return The velocity of the elevator in meters per second.
192   */
193  public double getVelocity() {
194    return getOutput(1);
195  }
196
197  /**
198   * Returns the elevator current draw.
199   *
200   * @return The elevator current draw in amps.
201   */
202  public double getCurrentDraw() {
203    // I = V / R - omega / (Kv * R)
204    // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
205    // spinning 10x faster than the output
206    // v = r w, so w = v/r
207    double kA = 1 / m_plant.getB().get(1, 0);
208    double kV = -m_plant.getA().get(1, 1) * kA;
209    double motorVelocity = m_x.get(1, 0) * kV * m_gearbox.Kv;
210    var appliedVoltage = m_u.get(0, 0);
211    return m_gearbox.getCurrent(motorVelocity, appliedVoltage) * Math.signum(appliedVoltage);
212  }
213
214  /**
215   * Sets the input voltage for the elevator.
216   *
217   * @param volts The input voltage.
218   */
219  public void setInputVoltage(double volts) {
220    setInput(volts);
221    clampInput(RobotController.getBatteryVoltage());
222  }
223
224  /**
225   * Updates the state of the elevator.
226   *
227   * @param currentXhat The current state estimate.
228   * @param u The system inputs (voltage).
229   * @param dt The time difference between controller updates in seconds.
230   */
231  @Override
232  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dt) {
233    // Calculate updated x-hat from Runge-Kutta.
234    var updatedXhat =
235        NumericalIntegration.rkdp(
236            (x, _u) -> {
237              Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
238              if (m_simulateGravity) {
239                xdot = xdot.plus(VecBuilder.fill(0, -9.8));
240              }
241              return xdot;
242            },
243            currentXhat,
244            u,
245            dt);
246
247    // We check for collisions after updating x-hat.
248    if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
249      return VecBuilder.fill(m_minHeight, 0);
250    }
251    if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
252      return VecBuilder.fill(m_maxHeight, 0);
253    }
254    return updatedXhat;
255  }
256}