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