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