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.system.LinearSystem;
010import edu.wpi.first.math.system.plant.DCMotor;
011import edu.wpi.first.math.system.plant.LinearSystemId;
012import edu.wpi.first.wpilibj.RobotController;
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  // The moment of inertia for the flywheel mechanism.
023  private final double m_j;
024
025  /**
026   * Creates a simulated flywheel mechanism.
027   *
028   * @param plant The linear system that represents the flywheel. Use either {@link
029   *     LinearSystemId#createFlywheelSystem(DCMotor, double, double)} if using physical constants
030   *     or {@link LinearSystemId#identifyVelocitySystem(double, double)} if using system
031   *     characterization.
032   * @param gearbox The type of and number of motors in the flywheel gearbox.
033   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
034   *     noise is desired. If present must have 1 element for velocity.
035   */
036  public FlywheelSim(
037      LinearSystem<N1, N1, N1> plant, DCMotor gearbox, double... measurementStdDevs) {
038    super(plant, measurementStdDevs);
039    m_gearbox = gearbox;
040
041    // By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf,
042    // the flywheel state-space model is:
043    //
044    //   dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u
045    //   A = -G²Kₜ/(KᵥRJ)
046    //   B = GKₜ/(RJ)
047    //
048    // Solve for G.
049    //
050    //   A/B = -G/Kᵥ
051    //   G = -KᵥA/B
052    //
053    // Solve for J.
054    //
055    //   B = GKₜ/(RJ)
056    //   J = GKₜ/(RB)
057    m_gearing = -gearbox.Kv * plant.getA(0, 0) / plant.getB(0, 0);
058    m_j = m_gearing * gearbox.Kt / (gearbox.R * plant.getB(0, 0));
059  }
060
061  /**
062   * Sets the flywheel's angular velocity.
063   *
064   * @param velocity The new velocity in radians per second.
065   */
066  public void setAngularVelocity(double velocity) {
067    setState(VecBuilder.fill(velocity));
068  }
069
070  /**
071   * Returns the gear ratio of the flywheel.
072   *
073   * @return the flywheel's gear ratio.
074   */
075  public double getGearing() {
076    return m_gearing;
077  }
078
079  /**
080   * Returns the moment of inertia.
081   *
082   * @return The flywheel's moment of inertia in kg-m².
083   */
084  public double getJ() {
085    return m_j;
086  }
087
088  /**
089   * Returns the gearbox for the flywheel.
090   *
091   * @return The flywheel's gearbox.
092   */
093  public DCMotor getGearbox() {
094    return m_gearbox;
095  }
096
097  /**
098   * Returns the flywheel's velocity.
099   *
100   * @return The flywheel's velocity in rad/s.
101   */
102  public double getAngularVelocity() {
103    return getOutput(0);
104  }
105
106  /**
107   * Returns the flywheel's acceleration.
108   *
109   * @return The flywheel's acceleration in rad/s².
110   */
111  public double getAngularAcceleration() {
112    var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
113    return acceleration.get(0, 0);
114  }
115
116  /**
117   * Returns the flywheel's torque.
118   *
119   * @return The flywheel's torque in Newton-meters.
120   */
121  public double getTorque() {
122    return getAngularAcceleration() * m_j;
123  }
124
125  /**
126   * Returns the flywheel's current draw.
127   *
128   * @return The flywheel's current draw in amps.
129   */
130  public double getCurrentDraw() {
131    // I = V / R - omega / (Kv * R)
132    // Reductions are output over input, so a reduction of 2:1 means the motor is spinning
133    // 2x faster than the flywheel
134    return m_gearbox.getCurrent(m_x.get(0, 0) * m_gearing, m_u.get(0, 0))
135        * Math.signum(m_u.get(0, 0));
136  }
137
138  /**
139   * Gets the input voltage for the flywheel.
140   *
141   * @return The flywheel's input voltage.
142   */
143  public double getInputVoltage() {
144    return getInput(0);
145  }
146
147  /**
148   * Sets the input voltage for the flywheel.
149   *
150   * @param volts The input voltage.
151   */
152  public void setInputVoltage(double volts) {
153    setInput(volts);
154    clampInput(RobotController.getBatteryVoltage());
155  }
156}