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