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