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.plant.DCMotor;
013import edu.wpi.first.math.system.plant.LinearSystemId;
014import edu.wpi.first.math.util.Units;
015
016/** Represents a simulated DC motor mechanism. */
017public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
018  // Gearbox for the DC motor.
019  private final DCMotor m_gearbox;
020
021  // The gearing from the motors to the output.
022  private final double m_gearing;
023
024  /**
025   * Creates a simulated DC motor mechanism.
026   *
027   * @param plant The linear system representing the DC motor. This system can be created with
028   *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(DCMotor, double,
029   *     double)}.
030   * @param gearbox The type of and number of motors in the DC motor gearbox.
031   * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
032   */
033  public DCMotorSim(LinearSystem<N2, N1, N2> plant, DCMotor gearbox, double gearing) {
034    super(plant);
035    m_gearbox = gearbox;
036    m_gearing = gearing;
037  }
038
039  /**
040   * Creates a simulated DC motor mechanism.
041   *
042   * @param plant The linear system representing the DC motor. This system can be created with
043   * @param gearbox The type of and number of motors in the DC motor gearbox.
044   * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
045   * @param measurementStdDevs The standard deviations of the measurements.
046   */
047  public DCMotorSim(
048      LinearSystem<N2, N1, N2> plant,
049      DCMotor gearbox,
050      double gearing,
051      Matrix<N2, N1> measurementStdDevs) {
052    super(plant, measurementStdDevs);
053    m_gearbox = gearbox;
054    m_gearing = gearing;
055  }
056
057  /**
058   * Creates a simulated DC motor mechanism.
059   *
060   * @param gearbox The type of and number of motors in the DC motor gearbox.
061   * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
062   * @param jKgMetersSquared The moment of inertia of the DC motor. If this is unknown, use the
063   *     {@link #DCMotorSim(LinearSystem, DCMotor, double, Matrix)} constructor.
064   */
065  public DCMotorSim(DCMotor gearbox, double gearing, double jKgMetersSquared) {
066    super(LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing));
067    m_gearbox = gearbox;
068    m_gearing = gearing;
069  }
070
071  /**
072   * Creates a simulated DC motor mechanism.
073   *
074   * @param gearbox The type of and number of motors in the DC motor gearbox.
075   * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
076   * @param jKgMetersSquared The moment of inertia of the DC motor. If this is unknown, use the
077   *     {@link #DCMotorSim(LinearSystem, DCMotor, double, Matrix)} constructor.
078   * @param measurementStdDevs The standard deviations of the measurements.
079   */
080  public DCMotorSim(
081      DCMotor gearbox, double gearing, double jKgMetersSquared, Matrix<N2, N1> measurementStdDevs) {
082    super(
083        LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing), measurementStdDevs);
084    m_gearbox = gearbox;
085    m_gearing = gearing;
086  }
087
088  /**
089   * Sets the state of the DC motor.
090   *
091   * @param angularPositionRad The new position in radians.
092   * @param angularVelocityRadPerSec The new velocity in radians per second.
093   */
094  public void setState(double angularPositionRad, double angularVelocityRadPerSec) {
095    setState(VecBuilder.fill(angularPositionRad, angularVelocityRadPerSec));
096  }
097
098  /**
099   * Returns the DC motor position.
100   *
101   * @return The DC motor position.
102   */
103  public double getAngularPositionRad() {
104    return getOutput(0);
105  }
106
107  /**
108   * Returns the DC motor position in rotations.
109   *
110   * @return The DC motor position in rotations.
111   */
112  public double getAngularPositionRotations() {
113    return Units.radiansToRotations(getAngularPositionRad());
114  }
115
116  /**
117   * Returns the DC motor velocity.
118   *
119   * @return The DC motor velocity.
120   */
121  public double getAngularVelocityRadPerSec() {
122    return getOutput(1);
123  }
124
125  /**
126   * Returns the DC motor velocity in RPM.
127   *
128   * @return The DC motor velocity in RPM.
129   */
130  public double getAngularVelocityRPM() {
131    return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec());
132  }
133
134  /**
135   * Returns the DC motor current draw.
136   *
137   * @return The DC motor current draw.
138   */
139  @Override
140  public double getCurrentDrawAmps() {
141    // I = V / R - omega / (Kv * R)
142    // Reductions are output over input, so a reduction of 2:1 means the motor is spinning
143    // 2x faster than the output
144    return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0))
145        * Math.signum(m_u.get(0, 0));
146  }
147
148  /**
149   * Sets the input voltage for the DC motor.
150   *
151   * @param volts The input voltage.
152   */
153  public void setInputVoltage(double volts) {
154    setInput(volts);
155  }
156}