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.math.system.plant.LinearSystemId;
013import edu.wpi.first.math.util.Units;
014import edu.wpi.first.wpilibj.RobotController;
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   * @param gearbox The type of and number of motors in the DC motor gearbox.
029   * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
030   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
031   *     noise is desired. If present must have 2 elements. The first element is for position. The
032   *     second element is for velocity.
033   */
034  public DCMotorSim(
035      LinearSystem<N2, N1, N2> plant,
036      DCMotor gearbox,
037      double gearing,
038      double... measurementStdDevs) {
039    super(plant, measurementStdDevs);
040    m_gearbox = gearbox;
041    m_gearing = gearing;
042  }
043
044  /**
045   * Creates a simulated DC motor mechanism.
046   *
047   * @param gearbox The type of and number of motors in the DC motor gearbox.
048   * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
049   * @param jKgMetersSquared The moment of inertia of the DC motor. If this is unknown, use the
050   *     {@link #DCMotorSim(LinearSystem, DCMotor, double, double...)} constructor.
051   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
052   *     noise is desired. If present must have 2 elements. The first element is for position. The
053   *     second element is for velocity.
054   */
055  public DCMotorSim(
056      DCMotor gearbox, double gearing, double jKgMetersSquared, double... measurementStdDevs) {
057    super(
058        LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing), measurementStdDevs);
059    m_gearbox = gearbox;
060    m_gearing = gearing;
061  }
062
063  /**
064   * Sets the state of the DC motor.
065   *
066   * @param angularPositionRad The new position in radians.
067   * @param angularVelocityRadPerSec The new velocity in radians per second.
068   */
069  public void setState(double angularPositionRad, double angularVelocityRadPerSec) {
070    setState(VecBuilder.fill(angularPositionRad, angularVelocityRadPerSec));
071  }
072
073  /**
074   * Returns the DC motor position.
075   *
076   * @return The DC motor position.
077   */
078  public double getAngularPositionRad() {
079    return getOutput(0);
080  }
081
082  /**
083   * Returns the DC motor position in rotations.
084   *
085   * @return The DC motor position in rotations.
086   */
087  public double getAngularPositionRotations() {
088    return Units.radiansToRotations(getAngularPositionRad());
089  }
090
091  /**
092   * Returns the DC motor velocity.
093   *
094   * @return The DC motor velocity.
095   */
096  public double getAngularVelocityRadPerSec() {
097    return getOutput(1);
098  }
099
100  /**
101   * Returns the DC motor velocity in RPM.
102   *
103   * @return The DC motor velocity in RPM.
104   */
105  public double getAngularVelocityRPM() {
106    return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec());
107  }
108
109  /**
110   * Returns the DC motor current draw.
111   *
112   * @return The DC motor current draw.
113   */
114  public double getCurrentDrawAmps() {
115    // I = V / R - omega / (Kv * R)
116    // Reductions are output over input, so a reduction of 2:1 means the motor is spinning
117    // 2x faster than the output
118    return m_gearbox.getCurrent(m_x.get(1, 0) * m_gearing, m_u.get(0, 0))
119        * Math.signum(m_u.get(0, 0));
120  }
121
122  /**
123   * Sets the input voltage for the DC motor.
124   *
125   * @param volts The input voltage.
126   */
127  public void setInputVoltage(double volts) {
128    setInput(volts);
129    clampInput(RobotController.getBatteryVoltage());
130  }
131}