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.math.util.Units;
013import edu.wpi.first.wpilibj.RobotController;
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.
027   * @param gearbox The type of and number of motors in the flywheel gearbox.
028   * @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions).
029   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
030   *     noise is desired. If present must have 1 element for velocity.
031   */
032  public FlywheelSim(
033      LinearSystem<N1, N1, N1> plant,
034      DCMotor gearbox,
035      double gearing,
036      double... measurementStdDevs) {
037    super(plant, measurementStdDevs);
038    m_gearbox = gearbox;
039    m_gearing = gearing;
040  }
041
042  /**
043   * Creates a simulated flywheel mechanism.
044   *
045   * @param gearbox The type of and number of motors in the flywheel gearbox.
046   * @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions).
047   * @param jKgMetersSquared The moment of inertia of the flywheel. If this is unknown, use the
048   *     {@link #FlywheelSim(LinearSystem, DCMotor, double, double...)} constructor.
049   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
050   *     noise is desired. If present must have 1 element for velocity.
051   */
052  public FlywheelSim(
053      DCMotor gearbox, double gearing, double jKgMetersSquared, double... measurementStdDevs) {
054    super(
055        LinearSystemId.createFlywheelSystem(gearbox, jKgMetersSquared, gearing),
056        measurementStdDevs);
057    m_gearbox = gearbox;
058    m_gearing = gearing;
059  }
060
061  /**
062   * Sets the flywheel's state.
063   *
064   * @param velocityRadPerSec The new velocity in radians per second.
065   */
066  public void setState(double velocityRadPerSec) {
067    setState(VecBuilder.fill(velocityRadPerSec));
068  }
069
070  /**
071   * Returns the flywheel velocity.
072   *
073   * @return The flywheel velocity.
074   */
075  public double getAngularVelocityRadPerSec() {
076    return getOutput(0);
077  }
078
079  /**
080   * Returns the flywheel velocity in RPM.
081   *
082   * @return The flywheel velocity in RPM.
083   */
084  public double getAngularVelocityRPM() {
085    return Units.radiansPerSecondToRotationsPerMinute(getOutput(0));
086  }
087
088  /**
089   * Returns the flywheel current draw.
090   *
091   * @return The flywheel current draw.
092   */
093  public double getCurrentDrawAmps() {
094    // I = V / R - omega / (Kv * R)
095    // Reductions are output over input, so a reduction of 2:1 means the motor is spinning
096    // 2x faster than the flywheel
097    return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0))
098        * Math.signum(m_u.get(0, 0));
099  }
100
101  /**
102   * Sets the input voltage for the flywheel.
103   *
104   * @param volts The input voltage.
105   */
106  public void setInputVoltage(double volts) {
107    setInput(volts);
108    clampInput(RobotController.getBatteryVoltage());
109  }
110}