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