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;
016
017/** Represents a simulated elevator mechanism. */
018public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
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 minHeightMeters The min allowable height of the elevator.
039   * @param maxHeightMeters The max allowable height of the elevator.
040   * @param simulateGravity Whether gravity should be simulated or not.
041   * @param startingHeightMeters The starting height of the elevator.
042   * @param measurementStdDevs The standard deviations of the measurements.
043   */
044  @SuppressWarnings("this-escape")
045  public ElevatorSim(
046      LinearSystem<N2, N1, N1> plant,
047      DCMotor gearbox,
048      double minHeightMeters,
049      double maxHeightMeters,
050      boolean simulateGravity,
051      double startingHeightMeters,
052      Matrix<N1, N1> measurementStdDevs) {
053    super(plant, measurementStdDevs);
054    m_gearbox = gearbox;
055    m_minHeight = minHeightMeters;
056    m_maxHeight = maxHeightMeters;
057    m_simulateGravity = simulateGravity;
058
059    setState(startingHeightMeters, 0);
060  }
061
062  /**
063   * Creates a simulated elevator mechanism.
064   *
065   * @param plant The linear system that represents the elevator. This system can be created with
066   *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double,
067   *     double, double)}.
068   * @param gearbox The type of and number of motors in the elevator gearbox.
069   * @param minHeightMeters The min allowable height of the elevator.
070   * @param maxHeightMeters The max allowable height of the elevator.
071   * @param startingHeightMeters The starting height of the elevator.
072   * @param simulateGravity Whether gravity should be simulated or not.
073   */
074  public ElevatorSim(
075      LinearSystem<N2, N1, N1> plant,
076      DCMotor gearbox,
077      double minHeightMeters,
078      double maxHeightMeters,
079      boolean simulateGravity,
080      double startingHeightMeters) {
081    this(
082        plant,
083        gearbox,
084        minHeightMeters,
085        maxHeightMeters,
086        simulateGravity,
087        startingHeightMeters,
088        null);
089  }
090
091  /**
092   * Creates a simulated elevator mechanism.
093   *
094   * @param kV The velocity gain.
095   * @param kA The acceleration gain.
096   * @param gearbox The type of and number of motors in the elevator gearbox.
097   * @param minHeightMeters The min allowable height of the elevator.
098   * @param maxHeightMeters The max allowable height of the elevator.
099   * @param simulateGravity Whether gravity should be simulated or not.
100   * @param startingHeightMeters The starting height of the elevator.
101   */
102  public ElevatorSim(
103      double kV,
104      double kA,
105      DCMotor gearbox,
106      double minHeightMeters,
107      double maxHeightMeters,
108      boolean simulateGravity,
109      double startingHeightMeters) {
110    this(
111        kV,
112        kA,
113        gearbox,
114        minHeightMeters,
115        maxHeightMeters,
116        simulateGravity,
117        startingHeightMeters,
118        null);
119  }
120
121  /**
122   * Creates a simulated elevator mechanism.
123   *
124   * @param kV The velocity gain.
125   * @param kA The acceleration gain.
126   * @param gearbox The type of and number of motors in the elevator gearbox.
127   * @param minHeightMeters The min allowable height of the elevator.
128   * @param maxHeightMeters The max allowable height of the elevator.
129   * @param simulateGravity Whether gravity should be simulated or not.
130   * @param startingHeightMeters The starting height of the elevator.
131   * @param measurementStdDevs The standard deviations of the measurements.
132   */
133  public ElevatorSim(
134      double kV,
135      double kA,
136      DCMotor gearbox,
137      double minHeightMeters,
138      double maxHeightMeters,
139      boolean simulateGravity,
140      double startingHeightMeters,
141      Matrix<N1, N1> measurementStdDevs) {
142    this(
143        LinearSystemId.identifyPositionSystem(kV, kA),
144        gearbox,
145        minHeightMeters,
146        maxHeightMeters,
147        simulateGravity,
148        startingHeightMeters,
149        measurementStdDevs);
150  }
151
152  /**
153   * Creates a simulated elevator mechanism.
154   *
155   * @param gearbox The type of and number of motors in the elevator gearbox.
156   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
157   * @param carriageMassKg The mass of the elevator carriage.
158   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
159   * @param minHeightMeters The min allowable height of the elevator.
160   * @param maxHeightMeters The max allowable height of the elevator.
161   * @param simulateGravity Whether gravity should be simulated or not.
162   * @param startingHeightMeters The starting height of the elevator.
163   * @param measurementStdDevs The standard deviations of the measurements.
164   */
165  public ElevatorSim(
166      DCMotor gearbox,
167      double gearing,
168      double carriageMassKg,
169      double drumRadiusMeters,
170      double minHeightMeters,
171      double maxHeightMeters,
172      boolean simulateGravity,
173      double startingHeightMeters,
174      Matrix<N1, N1> measurementStdDevs) {
175    this(
176        LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
177        gearbox,
178        minHeightMeters,
179        maxHeightMeters,
180        simulateGravity,
181        startingHeightMeters,
182        measurementStdDevs);
183  }
184
185  /**
186   * Creates a simulated elevator mechanism.
187   *
188   * @param gearbox The type of and number of motors in the elevator gearbox.
189   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
190   * @param carriageMassKg The mass of the elevator carriage.
191   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
192   * @param minHeightMeters The min allowable height of the elevator.
193   * @param maxHeightMeters The max allowable height of the elevator.
194   * @param simulateGravity Whether gravity should be simulated or not.
195   * @param startingHeightMeters The starting height of the elevator.
196   */
197  public ElevatorSim(
198      DCMotor gearbox,
199      double gearing,
200      double carriageMassKg,
201      double drumRadiusMeters,
202      double minHeightMeters,
203      double maxHeightMeters,
204      boolean simulateGravity,
205      double startingHeightMeters) {
206    this(
207        gearbox,
208        gearing,
209        carriageMassKg,
210        drumRadiusMeters,
211        minHeightMeters,
212        maxHeightMeters,
213        simulateGravity,
214        startingHeightMeters,
215        null);
216  }
217
218  /**
219   * Sets the elevator's state. The new position will be limited between the minimum and maximum
220   * allowed heights.
221   *
222   * @param positionMeters The new position in meters.
223   * @param velocityMetersPerSecond New velocity in meters per second.
224   */
225  public final void setState(double positionMeters, double velocityMetersPerSecond) {
226    setState(
227        VecBuilder.fill(
228            MathUtil.clamp(positionMeters, m_minHeight, m_maxHeight), velocityMetersPerSecond));
229  }
230
231  /**
232   * Returns whether the elevator would hit the lower limit.
233   *
234   * @param elevatorHeightMeters The elevator height.
235   * @return Whether the elevator would hit the lower limit.
236   */
237  public boolean wouldHitLowerLimit(double elevatorHeightMeters) {
238    return elevatorHeightMeters <= this.m_minHeight;
239  }
240
241  /**
242   * Returns whether the elevator would hit the upper limit.
243   *
244   * @param elevatorHeightMeters The elevator height.
245   * @return Whether the elevator would hit the upper limit.
246   */
247  public boolean wouldHitUpperLimit(double elevatorHeightMeters) {
248    return elevatorHeightMeters >= this.m_maxHeight;
249  }
250
251  /**
252   * Returns whether the elevator has hit the lower limit.
253   *
254   * @return Whether the elevator has hit the lower limit.
255   */
256  public boolean hasHitLowerLimit() {
257    return wouldHitLowerLimit(getPositionMeters());
258  }
259
260  /**
261   * Returns whether the elevator has hit the upper limit.
262   *
263   * @return Whether the elevator has hit the upper limit.
264   */
265  public boolean hasHitUpperLimit() {
266    return wouldHitUpperLimit(getPositionMeters());
267  }
268
269  /**
270   * Returns the position of the elevator.
271   *
272   * @return The position of the elevator.
273   */
274  public double getPositionMeters() {
275    return getOutput(0);
276  }
277
278  /**
279   * Returns the velocity of the elevator.
280   *
281   * @return The velocity of the elevator.
282   */
283  public double getVelocityMetersPerSecond() {
284    return m_x.get(1, 0);
285  }
286
287  /**
288   * Returns the elevator current draw.
289   *
290   * @return The elevator current draw.
291   */
292  @Override
293  public double getCurrentDrawAmps() {
294    // I = V / R - omega / (Kv * R)
295    // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
296    // spinning 10x faster than the output
297    // v = r w, so w = v/r
298    double kA = 1 / m_plant.getB().get(1, 0);
299    double kV = -m_plant.getA().get(1, 1) * kA;
300    double motorVelocityRadPerSec =
301        getVelocityMetersPerSecond() * kV * m_gearbox.KvRadPerSecPerVolt;
302    var appliedVoltage = m_u.get(0, 0);
303    return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage)
304        * Math.signum(appliedVoltage);
305  }
306
307  /**
308   * Sets the input voltage for the elevator.
309   *
310   * @param volts The input voltage.
311   */
312  public void setInputVoltage(double volts) {
313    setInput(volts);
314  }
315
316  /**
317   * Updates the state of the elevator.
318   *
319   * @param currentXhat The current state estimate.
320   * @param u The system inputs (voltage).
321   * @param dtSeconds The time difference between controller updates.
322   */
323  @Override
324  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
325    // Calculate updated x-hat from Runge-Kutta.
326    var updatedXhat =
327        NumericalIntegration.rkdp(
328            (x, _u) -> {
329              Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
330              if (m_simulateGravity) {
331                xdot = xdot.plus(VecBuilder.fill(0, -9.8));
332              }
333              return xdot;
334            },
335            currentXhat,
336            u,
337            dtSeconds);
338
339    // We check for collisions after updating x-hat.
340    if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
341      return VecBuilder.fill(m_minHeight, 0);
342    }
343    if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
344      return VecBuilder.fill(m_maxHeight, 0);
345    }
346    return updatedXhat;
347  }
348}