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