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}