From 1dff657b8c1d6720d1ce7dc20c9e6df3115ae706 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Thu, 3 Oct 2024 14:48:56 -0400 Subject: [PATCH] Initial file setup with FlywheelSim --- .../first/wpilibj/simulation/FlywheelSim.java | 73 +++++++++---------- .../wpilibj/simulation/FlywheelSimTorque.java | 51 +++++++++++++ .../simulation/FlywheelSimVoltage.java | 69 ++++++++++++++++++ 3 files changed, 154 insertions(+), 39 deletions(-) create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimTorque.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimVoltage.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java index 372dec74560..22b3e4c3a82 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java @@ -17,10 +17,9 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.MutAngularAcceleration; import edu.wpi.first.units.measure.MutAngularVelocity; -import edu.wpi.first.wpilibj.RobotController; /** Represents a simulated flywheel mechanism. */ -public class FlywheelSim extends LinearSystemSim { +public abstract class FlywheelSim extends LinearSystemSim { // Gearbox for the flywheel. private final DCMotor m_gearbox; @@ -28,7 +27,7 @@ public class FlywheelSim extends LinearSystemSim { private final double m_gearing; // The moment of inertia for the flywheel mechanism. - private final double m_jKgMetersSquared; + final double m_jKgMetersSquared; // The angular velocity of the system. private final MutAngularVelocity m_angularVelocity = RadiansPerSecond.mutable(0); @@ -39,35 +38,43 @@ public class FlywheelSim extends LinearSystemSim { /** * Creates a simulated flywheel mechanism. * - * @param plant The linear system that represents the flywheel. Use either {@link - * LinearSystemId#createFlywheelSystem(DCMotor, double, double)} if using physical constants - * or {@link LinearSystemId#identifyVelocitySystem(double, double)} if using system - * characterization. - * @param gearbox The type of and number of motors in the flywheel gearbox. - * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no - * noise is desired. If present must have 1 element for velocity. + * @param plant The linear system that represents the flywheel. Use + * either {@link + * LinearSystemId#createFlywheelSystem(DCMotor, double, double)} + * if using physical constants + * or + * {@link LinearSystemId#identifyVelocitySystem(double, double)} + * if using system + * characterization. + * @param gearbox The type of and number of motors in the flywheel + * gearbox. + * @param measurementStdDevs The standard deviations of the measurements. Can be + * omitted if no + * noise is desired. If present must have 1 element + * for velocity. */ public FlywheelSim( LinearSystem plant, DCMotor gearbox, double... measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; - // By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf, + // By theorem 6.10.1 of + // https://file.tavsys.net/control/controls-engineering-in-frc.pdf, // the flywheel state-space model is: // - // dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u - // A = -G²Kₜ/(KᵥRJ) - // B = GKₜ/(RJ) + // dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u + // A = -G²Kₜ/(KᵥRJ) + // B = GKₜ/(RJ) // // Solve for G. // - // A/B = -G/Kᵥ - // G = -KᵥA/B + // A/B = -G/Kᵥ + // G = -KᵥA/B // // Solve for J. // - // B = GKₜ/(RJ) - // J = GKₜ/(RB) + // B = GKₜ/(RJ) + // J = GKₜ/(RB) m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(0, 0) / plant.getB(0, 0); m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(0, 0)); } @@ -156,15 +163,6 @@ public AngularAcceleration getAngularAcceleration() { return m_angularAcceleration; } - /** - * Returns the flywheel's torque in Newton-Meters. - * - * @return The flywheel's torque in Newton-Meters. - */ - public double getTorqueNewtonMeters() { - return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared; - } - /** * Returns the flywheel's current draw. * @@ -172,28 +170,25 @@ public double getTorqueNewtonMeters() { */ public double getCurrentDrawAmps() { // I = V / R - omega / (Kv * R) - // Reductions are output over input, so a reduction of 2:1 means the motor is spinning + // Reductions are output over input, so a reduction of 2:1 means the motor is + // spinning // 2x faster than the flywheel return m_gearbox.getCurrent(m_x.get(0, 0) * m_gearing, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0)); } /** - * Gets the input voltage for the flywheel. + * Gets the voltage of the flywheel. * - * @return The flywheel input voltage. + * @return The flywheel's voltage. */ - public double getInputVoltage() { - return getInput(0); - } + public abstract double getVoltage(); /** - * Sets the input voltage for the flywheel. + * Gets the torque on the flywheel. * - * @param volts The input voltage. + * @return The flywheel's torque in Newton-Meters. */ - public void setInputVoltage(double volts) { - setInput(volts); - clampInput(RobotController.getBatteryVoltage()); - } + public abstract double getTorqueNewtonMeters(); + } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimTorque.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimTorque.java new file mode 100644 index 00000000000..2f3d044bdb5 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimTorque.java @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; + +/** Represents a simulated flywheel mechanism. */ +public class FlywheelSimTorque extends FlywheelSim { + + /** + * Creates a simulated flywheel mechanism. + * + * @param plant The linear system that represents the flywheel. Use either {@link + * LinearSystemId#createFlywheelSystem(DCMotor, double, double)} if using physical constants + * or {@link LinearSystemId#identifyVelocitySystem(double, double)} if using system + * characterization. + * @param gearbox The type of and number of motors in the flywheel gearbox. + * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no + * noise is desired. If present must have 1 element for velocity. + */ + public FlywheelSimTorque( + LinearSystem plant, DCMotor gearbox, double... measurementStdDevs) { + super(plant, gearbox, measurementStdDevs); + } + + /** + * Sets the input torque for the flywheel. + * + * @param volts The input torque. + */ + public void setInputTorque(double torqueNM) { + setInput(torqueNM); + } + + @Override + public double getVoltage() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getVoltage'"); + } + + @Override + public double getTorqueNewtonMeters() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getTorqueNewtonMeters'"); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimVoltage.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimVoltage.java new file mode 100644 index 00000000000..37d13f1f498 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSimVoltage.java @@ -0,0 +1,69 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.RobotController; + +/** Represents a simulated flywheel mechanism. */ +public class FlywheelSimVoltage extends FlywheelSim { + + /** + * Creates a simulated flywheel mechanism. + * + * @param plant The linear system that represents the flywheel. Use + * either {@link + * LinearSystemId#createFlywheelSystem(DCMotor, double, double)} + * if using physical constants + * or + * {@link LinearSystemId#identifyVelocitySystem(double, double)} + * if using system + * characterization. + * @param gearbox The type of and number of motors in the flywheel + * gearbox. + * @param measurementStdDevs The standard deviations of the measurements. Can be + * omitted if no + * noise is desired. If present must have 1 element + * for velocity. + */ + public FlywheelSimVoltage( + LinearSystem plant, DCMotor gearbox, double... measurementStdDevs) { + super(plant, gearbox, measurementStdDevs); + + } + + /** + * Sets the input voltage for the flywheel. + * + * @param volts The input voltage. + */ + public void setInputVoltage(double volts) { + setInput(volts); + clampInput(RobotController.getBatteryVoltage()); + } + + /** + * Gets the input voltage for the flywheel. + * + * @return The flywheel input voltage. + */ + @Override + public double getVoltage() { + return getInput(0); + } + + /** + * Gets the torque on the flywheel. + * + * @return The flywheel's torque in Newton-Meters. + */ + @Override + public double getTorqueNewtonMeters() { + return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared; + } +}