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  // Whether the simulator should simulate gravity.
034  private final boolean m_simulateGravity;
035
036  /**
037   * Creates a simulated elevator mechanism.
038   *
039   * @param plant The linear system that represents the elevator.
040   * @param gearbox The type of and number of motors in the elevator gearbox.
041   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
042   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
043   * @param minHeightMeters The min allowable height of the elevator.
044   * @param maxHeightMeters The max allowable height of the elevator.
045   * @param simulateGravity Whether gravity should be simulated or not.
046   */
047  public ElevatorSim(
048      LinearSystem<N2, N1, N1> plant,
049      DCMotor gearbox,
050      double gearing,
051      double drumRadiusMeters,
052      double minHeightMeters,
053      double maxHeightMeters,
054      boolean simulateGravity) {
055    this(
056        plant,
057        gearbox,
058        gearing,
059        drumRadiusMeters,
060        minHeightMeters,
061        maxHeightMeters,
062        simulateGravity,
063        null);
064  }
065
066  /**
067   * Creates a simulated elevator mechanism.
068   *
069   * @param plant The linear system that represents the elevator.
070   * @param gearbox The type of and number of motors in the elevator gearbox.
071   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
072   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
073   * @param minHeightMeters The min allowable height of the elevator.
074   * @param maxHeightMeters The max allowable height of the elevator.
075   * @param simulateGravity Whether gravity should be simulated or not.
076   * @param measurementStdDevs The standard deviations of the measurements.
077   */
078  public ElevatorSim(
079      LinearSystem<N2, N1, N1> plant,
080      DCMotor gearbox,
081      double gearing,
082      double drumRadiusMeters,
083      double minHeightMeters,
084      double maxHeightMeters,
085      boolean simulateGravity,
086      Matrix<N1, N1> measurementStdDevs) {
087    super(plant, measurementStdDevs);
088    m_gearbox = gearbox;
089    m_gearing = gearing;
090    m_drumRadius = drumRadiusMeters;
091    m_minHeight = minHeightMeters;
092    m_maxHeight = maxHeightMeters;
093    m_simulateGravity = simulateGravity;
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   */
107  public ElevatorSim(
108      DCMotor gearbox,
109      double gearing,
110      double carriageMassKg,
111      double drumRadiusMeters,
112      double minHeightMeters,
113      double maxHeightMeters,
114      boolean simulateGravity) {
115    this(
116        gearbox,
117        gearing,
118        carriageMassKg,
119        drumRadiusMeters,
120        minHeightMeters,
121        maxHeightMeters,
122        simulateGravity,
123        null);
124  }
125
126  /**
127   * Creates a simulated elevator mechanism.
128   *
129   * @param gearbox The type of and number of motors in the elevator gearbox.
130   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
131   * @param carriageMassKg The mass of the elevator carriage.
132   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
133   * @param minHeightMeters The min allowable height of the elevator.
134   * @param maxHeightMeters The max allowable height of the elevator.
135   * @param simulateGravity Whether gravity should be simulated or not.
136   * @param measurementStdDevs The standard deviations of the measurements.
137   */
138  public ElevatorSim(
139      DCMotor gearbox,
140      double gearing,
141      double carriageMassKg,
142      double drumRadiusMeters,
143      double minHeightMeters,
144      double maxHeightMeters,
145      boolean simulateGravity,
146      Matrix<N1, N1> measurementStdDevs) {
147    super(
148        LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
149        measurementStdDevs);
150    m_gearbox = gearbox;
151    m_gearing = gearing;
152    m_drumRadius = drumRadiusMeters;
153    m_minHeight = minHeightMeters;
154    m_maxHeight = maxHeightMeters;
155    m_simulateGravity = simulateGravity;
156  }
157
158  /**
159   * Returns whether the elevator would hit the lower limit.
160   *
161   * @param elevatorHeightMeters The elevator height.
162   * @return Whether the elevator would hit the lower limit.
163   */
164  public boolean wouldHitLowerLimit(double elevatorHeightMeters) {
165    return elevatorHeightMeters <= this.m_minHeight;
166  }
167
168  /**
169   * Returns whether the elevator would hit the upper limit.
170   *
171   * @param elevatorHeightMeters The elevator height.
172   * @return Whether the elevator would hit the upper limit.
173   */
174  public boolean wouldHitUpperLimit(double elevatorHeightMeters) {
175    return elevatorHeightMeters >= this.m_maxHeight;
176  }
177
178  /**
179   * Returns whether the elevator has hit the lower limit.
180   *
181   * @return Whether the elevator has hit the lower limit.
182   */
183  public boolean hasHitLowerLimit() {
184    return wouldHitLowerLimit(getPositionMeters());
185  }
186
187  /**
188   * Returns whether the elevator has hit the upper limit.
189   *
190   * @return Whether the elevator has hit the upper limit.
191   */
192  public boolean hasHitUpperLimit() {
193    return wouldHitUpperLimit(getPositionMeters());
194  }
195
196  /**
197   * Returns the position of the elevator.
198   *
199   * @return The position of the elevator.
200   */
201  public double getPositionMeters() {
202    return getOutput(0);
203  }
204
205  /**
206   * Returns the velocity of the elevator.
207   *
208   * @return The velocity of the elevator.
209   */
210  public double getVelocityMetersPerSecond() {
211    return m_x.get(1, 0);
212  }
213
214  /**
215   * Returns the elevator current draw.
216   *
217   * @return The elevator current draw.
218   */
219  @Override
220  public double getCurrentDrawAmps() {
221    // I = V / R - omega / (Kv * R)
222    // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
223    // spinning 10x faster than the output
224    // v = r w, so w = v/r
225    double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadius * m_gearing;
226    var appliedVoltage = m_u.get(0, 0);
227    return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage)
228        * Math.signum(appliedVoltage);
229  }
230
231  /**
232   * Sets the input voltage for the elevator.
233   *
234   * @param volts The input voltage.
235   */
236  public void setInputVoltage(double volts) {
237    setInput(volts);
238  }
239
240  /**
241   * Updates the state of the elevator.
242   *
243   * @param currentXhat The current state estimate.
244   * @param u The system inputs (voltage).
245   * @param dtSeconds The time difference between controller updates.
246   */
247  @Override
248  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
249    // Calculate updated x-hat from Runge-Kutta.
250    var updatedXhat =
251        NumericalIntegration.rkdp(
252            (x, _u) -> {
253              Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
254              if (m_simulateGravity) {
255                xdot = xdot.plus(VecBuilder.fill(0, -9.8));
256              }
257              return xdot;
258            },
259            currentXhat,
260            u,
261            dtSeconds);
262
263    // We check for collisions after updating x-hat.
264    if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
265      return VecBuilder.fill(m_minHeight, 0);
266    }
267    if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
268      return VecBuilder.fill(m_maxHeight, 0);
269    }
270    return updatedXhat;
271  }
272}