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.VecBuilder;
008import edu.wpi.first.math.numbers.N1;
009import edu.wpi.first.math.numbers.N2;
010import edu.wpi.first.math.system.LinearSystem;
011import edu.wpi.first.math.system.plant.DCMotor;
012import edu.wpi.first.wpilibj.RobotController;
013
014/** Represents a simulated DC motor mechanism. */
015public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
016  // Gearbox for the DC motor.
017  private final DCMotor m_gearbox;
018
019  // The gearing from the motors to the output.
020  private final double m_gearing;
021
022  // The moment of inertia for the DC motor mechanism in kg-m².
023  private final double m_j;
024
025  /**
026   * Creates a simulated DC motor mechanism.
027   *
028   * @param plant The linear system representing the DC motor. This system can be created with
029   *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(DCMotor, double,
030   *     double)} or {@link
031   *     edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, double)}. If
032   *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, double)}
033   *     is used, the distance unit must be radians.
034   * @param gearbox The type of and number of motors in the DC motor gearbox.
035   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
036   *     noise is desired. If present must have 2 elements. The first element is for position. The
037   *     second element is for velocity.
038   */
039  public DCMotorSim(LinearSystem<N2, N1, N2> plant, DCMotor gearbox, double... measurementStdDevs) {
040    super(plant, measurementStdDevs);
041    m_gearbox = gearbox;
042
043    // By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf,
044    // the DC motor state-space model is:
045    //
046    //   dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u
047    //   A = -G²Kₜ/(KᵥRJ)
048    //   B = GKₜ/(RJ)
049    //
050    // Solve for G.
051    //
052    //   A/B = -G/Kᵥ
053    //   G = -KᵥA/B
054    //
055    // Solve for J.
056    //
057    //   B = GKₜ/(RJ)
058    //   J = GKₜ/(RB)
059    m_gearing = -gearbox.Kv * plant.getA(1, 1) / plant.getB(1, 0);
060    m_j = m_gearing * gearbox.Kt / (gearbox.R * plant.getB(1, 0));
061  }
062
063  /**
064   * Sets the state of the DC motor.
065   *
066   * @param angularPosition The new position in radians.
067   * @param angularVelocity The new velocity in radians per second.
068   */
069  public void setState(double angularPosition, double angularVelocity) {
070    setState(VecBuilder.fill(angularPosition, angularVelocity));
071  }
072
073  /**
074   * Sets the DC motor's angular position.
075   *
076   * @param angularPosition The new position in radians.
077   */
078  public void setAngle(double angularPosition) {
079    setState(angularPosition, getAngularVelocity());
080  }
081
082  /**
083   * Sets the DC motor's angular velocity.
084   *
085   * @param angularVelocity The new velocity in radians per second.
086   */
087  public void setAngularVelocity(double angularVelocity) {
088    setState(getAngularPosition(), angularVelocity);
089  }
090
091  /**
092   * Returns the gear ratio of the DC motor.
093   *
094   * @return the DC motor's gear ratio.
095   */
096  public double getGearing() {
097    return m_gearing;
098  }
099
100  /**
101   * Returns the moment of inertia of the DC motor.
102   *
103   * @return The DC motor's moment of inertia in kg-m².
104   */
105  public double getJ() {
106    return m_j;
107  }
108
109  /**
110   * Returns the gearbox for the DC motor.
111   *
112   * @return The DC motor's gearbox.
113   */
114  public DCMotor getGearbox() {
115    return m_gearbox;
116  }
117
118  /**
119   * Returns the DC motor's position.
120   *
121   * @return The DC motor's position in radians.
122   */
123  public double getAngularPosition() {
124    return getOutput(0);
125  }
126
127  /**
128   * Returns the DC motor's velocity.
129   *
130   * @return The DC motor's velocity in radians per second.
131   */
132  public double getAngularVelocity() {
133    return getOutput(1);
134  }
135
136  /**
137   * Returns the DC motor's acceleration.
138   *
139   * @return The DC motor's acceleration in rad/s².
140   */
141  public double getAngularAcceleration() {
142    var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
143    return acceleration.get(1, 0);
144  }
145
146  /**
147   * Returns the DC motor's torque in.
148   *
149   * @return The DC motor's torque in Newton-meters.
150   */
151  public double getTorque() {
152    return getAngularAcceleration() * m_j;
153  }
154
155  /**
156   * Returns the DC motor's current draw.
157   *
158   * @return The DC motor's current draw in amps.
159   */
160  public double getCurrentDraw() {
161    // I = V / R - omega / (Kv * R)
162    // Reductions are output over input, so a reduction of 2:1 means the motor is spinning
163    // 2x faster than the output
164    return m_gearbox.getCurrent(m_x.get(1, 0) * m_gearing, m_u.get(0, 0))
165        * Math.signum(m_u.get(0, 0));
166  }
167
168  /**
169   * Gets the input voltage for the DC motor.
170   *
171   * @return The DC motor's input voltage.
172   */
173  public double getInputVoltage() {
174    return getInput(0);
175  }
176
177  /**
178   * Sets the input voltage for the DC motor.
179   *
180   * @param volts The input voltage.
181   */
182  public void setInputVoltage(double volts) {
183    setInput(volts);
184    clampInput(RobotController.getBatteryVoltage());
185  }
186}