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  public FlywheelSim(DCMotor gearbox, double gearing, double jKgMetersSquared) {
062    super(LinearSystemId.createFlywheelSystem(gearbox, jKgMetersSquared, gearing));
063    m_gearbox = gearbox;
064    m_gearing = gearing;
065  }
066
067  /**
068   * Creates a simulated flywheel mechanism.
069   *
070   * @param gearbox The type of and number of motors in the flywheel gearbox.
071   * @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions).
072   * @param jKgMetersSquared The moment of inertia of the flywheel. If this is unknown, use the
073   *     {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)} constructor.
074   * @param measurementStdDevs The standard deviations of the measurements.
075   */
076  public FlywheelSim(
077      DCMotor gearbox, double gearing, double jKgMetersSquared, Matrix<N1, N1> measurementStdDevs) {
078    super(
079        LinearSystemId.createFlywheelSystem(gearbox, jKgMetersSquared, gearing),
080        measurementStdDevs);
081    m_gearbox = gearbox;
082    m_gearing = gearing;
083  }
084
085  /**
086   * Returns the flywheel velocity.
087   *
088   * @return The flywheel velocity.
089   */
090  public double getAngularVelocityRadPerSec() {
091    return getOutput(0);
092  }
093
094  /**
095   * Returns the flywheel velocity in RPM.
096   *
097   * @return The flywheel velocity in RPM.
098   */
099  public double getAngularVelocityRPM() {
100    return Units.radiansPerSecondToRotationsPerMinute(getOutput(0));
101  }
102
103  /**
104   * Returns the flywheel current draw.
105   *
106   * @return The flywheel current draw.
107   */
108  @Override
109  public double getCurrentDrawAmps() {
110    // I = V / R - omega / (Kv * R)
111    // Reductions are output over input, so a reduction of 2:1 means the motor is spinning
112    // 2x faster than the flywheel
113    return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0))
114        * Math.signum(m_u.get(0, 0));
115  }
116
117  /**
118   * Sets the input voltage for the flywheel.
119   *
120   * @param volts The input voltage.
121   */
122  public void setInputVoltage(double volts) {
123    setInput(volts);
124  }
125}