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 static edu.wpi.first.units.Units.Radians;
008import static edu.wpi.first.units.Units.RadiansPerSecond;
009import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
010
011import edu.wpi.first.math.VecBuilder;
012import edu.wpi.first.math.numbers.N1;
013import edu.wpi.first.math.numbers.N2;
014import edu.wpi.first.math.system.LinearSystem;
015import edu.wpi.first.math.system.plant.DCMotor;
016import edu.wpi.first.math.util.Units;
017import edu.wpi.first.units.measure.Angle;
018import edu.wpi.first.units.measure.AngularAcceleration;
019import edu.wpi.first.units.measure.AngularVelocity;
020import edu.wpi.first.wpilibj.RobotController;
021
022/** Represents a simulated DC motor mechanism. */
023public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
024  // Gearbox for the DC motor.
025  private final DCMotor m_gearbox;
026
027  // The gearing from the motors to the output.
028  private final double m_gearing;
029
030  // The moment of inertia for the DC motor mechanism.
031  private final double m_jKgMetersSquared;
032
033  /**
034   * Creates a simulated DC motor mechanism.
035   *
036   * @param plant The linear system representing the DC motor. This system can be created with
037   *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(DCMotor, double,
038   *     double)} or {@link
039   *     edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, double)}. If
040   *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, double)}
041   *     is used, the distance unit must be radians.
042   * @param gearbox The type of and number of motors in the DC motor gearbox.
043   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
044   *     noise is desired. If present must have 2 elements. The first element is for position. The
045   *     second element is for velocity.
046   */
047  public DCMotorSim(LinearSystem<N2, N1, N2> plant, DCMotor gearbox, double... measurementStdDevs) {
048    super(plant, measurementStdDevs);
049    m_gearbox = gearbox;
050
051    // By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf,
052    // the DC motor state-space model is:
053    //
054    //   dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u
055    //   A = -G²Kₜ/(KᵥRJ)
056    //   B = GKₜ/(RJ)
057    //
058    // Solve for G.
059    //
060    //   A/B = -G/Kᵥ
061    //   G = -KᵥA/B
062    //
063    // Solve for J.
064    //
065    //   B = GKₜ/(RJ)
066    //   J = GKₜ/(RB)
067    m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(1, 1) / plant.getB(1, 0);
068    m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(1, 0));
069  }
070
071  /**
072   * Sets the state of the DC motor.
073   *
074   * @param angularPositionRad The new position in radians.
075   * @param angularVelocityRadPerSec The new velocity in radians per second.
076   */
077  public void setState(double angularPositionRad, double angularVelocityRadPerSec) {
078    setState(VecBuilder.fill(angularPositionRad, angularVelocityRadPerSec));
079  }
080
081  /**
082   * Sets the DC motor's angular position.
083   *
084   * @param angularPositionRad The new position in radians.
085   */
086  public void setAngle(double angularPositionRad) {
087    setState(angularPositionRad, getAngularVelocityRadPerSec());
088  }
089
090  /**
091   * Sets the DC motor's angular velocity.
092   *
093   * @param angularVelocityRadPerSec The new velocity in radians per second.
094   */
095  public void setAngularVelocity(double angularVelocityRadPerSec) {
096    setState(getAngularPositionRad(), angularVelocityRadPerSec);
097  }
098
099  /**
100   * Returns the gear ratio of the DC motor.
101   *
102   * @return the DC motor's gear ratio.
103   */
104  public double getGearing() {
105    return m_gearing;
106  }
107
108  /**
109   * Returns the moment of inertia of the DC motor.
110   *
111   * @return The DC motor's moment of inertia.
112   */
113  public double getJKgMetersSquared() {
114    return m_jKgMetersSquared;
115  }
116
117  /**
118   * Returns the gearbox for the DC motor.
119   *
120   * @return The DC motor's gearbox.
121   */
122  public DCMotor getGearbox() {
123    return m_gearbox;
124  }
125
126  /**
127   * Returns the DC motor's position.
128   *
129   * @return The DC motor's position.
130   */
131  public double getAngularPositionRad() {
132    return getOutput(0);
133  }
134
135  /**
136   * Returns the DC motor's position in rotations.
137   *
138   * @return The DC motor's position in rotations.
139   */
140  public double getAngularPositionRotations() {
141    return Units.radiansToRotations(getAngularPositionRad());
142  }
143
144  /**
145   * Returns the DC motor's position.
146   *
147   * @return The DC motor's position
148   */
149  public Angle getAngularPosition() {
150    return Radians.of(getAngularPositionRad());
151  }
152
153  /**
154   * Returns the DC motor's velocity.
155   *
156   * @return The DC motor's velocity.
157   */
158  public double getAngularVelocityRadPerSec() {
159    return getOutput(1);
160  }
161
162  /**
163   * Returns the DC motor's velocity in RPM.
164   *
165   * @return The DC motor's velocity in RPM.
166   */
167  public double getAngularVelocityRPM() {
168    return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec());
169  }
170
171  /**
172   * Returns the DC motor's velocity.
173   *
174   * @return The DC motor's velocity
175   */
176  public AngularVelocity getAngularVelocity() {
177    return RadiansPerSecond.of(getAngularVelocityRadPerSec());
178  }
179
180  /**
181   * Returns the DC motor's acceleration in Radians Per Second Squared.
182   *
183   * @return The DC motor's acceleration in Radians Per Second Squared.
184   */
185  public double getAngularAccelerationRadPerSecSq() {
186    var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
187    return acceleration.get(1, 0);
188  }
189
190  /**
191   * Returns the DC motor's acceleration.
192   *
193   * @return The DC motor's acceleration.
194   */
195  public AngularAcceleration getAngularAcceleration() {
196    return RadiansPerSecondPerSecond.of(getAngularAccelerationRadPerSecSq());
197  }
198
199  /**
200   * Returns the DC motor's torque in Newton-Meters.
201   *
202   * @return The DC motor's torque in Newton-Meters.
203   */
204  public double getTorqueNewtonMeters() {
205    return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared;
206  }
207
208  /**
209   * Returns the DC motor's current draw.
210   *
211   * @return The DC motor's current draw.
212   */
213  public double getCurrentDrawAmps() {
214    // I = V / R - omega / (Kv * R)
215    // Reductions are output over input, so a reduction of 2:1 means the motor is spinning
216    // 2x faster than the output
217    return m_gearbox.getCurrent(m_x.get(1, 0) * m_gearing, m_u.get(0, 0))
218        * Math.signum(m_u.get(0, 0));
219  }
220
221  /**
222   * Gets the input voltage for the DC motor.
223   *
224   * @return The DC motor's input voltage.
225   */
226  public double getInputVoltage() {
227    return getInput(0);
228  }
229
230  /**
231   * Sets the input voltage for the DC motor.
232   *
233   * @param volts The input voltage.
234   */
235  public void setInputVoltage(double volts) {
236    setInput(volts);
237    clampInput(RobotController.getBatteryVoltage());
238  }
239}