From ab315e24c88e690e6db940819899e360f04fc31f Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Wed, 15 May 2024 13:40:30 -0400 Subject: [PATCH] [wpimath] LinearSystemSim Constructor and method cleanup (#6502) Modified Java constructors to take a variable number of measurement std devs argument with checks in place to make sure the right amount (or none) are passed into the constructor. All changes passed down to classes utilizing LinearSystemSim. Removed excess constructors Removed Java and C++ CurrentDrawAmps method as it doesn't belong in a generic (non electrical) linear system. Kept a non override version in all derived electrical classes. Also LinearSystemSim has now been made agnostic to electrical systems. Inputs don't have to be voltage. BatteryVoltage clamp function has been pushed down to electrical subclasses. Co-authored-by: Tyler Veness --- .../main/native/cpp/simulation/DCMotorSim.cpp | 1 + .../native/cpp/simulation/ElevatorSim.cpp | 1 + .../native/cpp/simulation/FlywheelSim.cpp | 1 + .../cpp/simulation/SingleJointedArmSim.cpp | 1 + .../include/frc/simulation/DCMotorSim.h | 2 +- .../include/frc/simulation/ElevatorSim.h | 2 +- .../include/frc/simulation/FlywheelSim.h | 2 +- .../include/frc/simulation/LinearSystemSim.h | 23 +--- .../frc/simulation/SingleJointedArmSim.h | 2 +- .../first/wpilibj/simulation/DCMotorSim.java | 47 ++------ .../first/wpilibj/simulation/ElevatorSim.java | 110 ++---------------- .../first/wpilibj/simulation/FlywheelSim.java | 45 ++----- .../wpilibj/simulation/LinearSystemSim.java | 53 ++++----- .../simulation/SingleJointedArmSim.java | 81 ++----------- .../wpilibj/simulation/ElevatorSimTest.java | 6 +- .../armsimulation/subsystems/Arm.java | 5 +- .../subsystems/Elevator.java | 4 +- .../subsystems/Elevator.java | 4 +- .../PotentiometerPIDTest.java | 3 +- 19 files changed, 83 insertions(+), 310 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp index 7023a218d4..f14564449a 100644 --- a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp @@ -48,4 +48,5 @@ units::ampere_t DCMotorSim::GetCurrentDraw() const { void DCMotorSim::SetInputVoltage(units::volt_t voltage) { SetInput(Vectord<1>{voltage.value()}); + ClampInput(frc::RobotController::GetBatteryVoltage().value()); } diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 911d828171..13b3d2e4a3 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -98,6 +98,7 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const { void ElevatorSim::SetInputVoltage(units::volt_t voltage) { SetInput(Vectord<1>{voltage.value()}); + ClampInput(frc::RobotController::GetBatteryVoltage().value()); } Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat, diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp index 1eb529b3c8..31024c3ae8 100644 --- a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp @@ -43,4 +43,5 @@ units::ampere_t FlywheelSim::GetCurrentDraw() const { void FlywheelSim::SetInputVoltage(units::volt_t voltage) { SetInput(Vectord<1>{voltage.value()}); + ClampInput(frc::RobotController::GetBatteryVoltage().value()); } diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index ab68cbcc70..945decc320 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -81,6 +81,7 @@ units::ampere_t SingleJointedArmSim::GetCurrentDraw() const { void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) { SetInput(Vectord<1>{voltage.value()}); + ClampInput(frc::RobotController::GetBatteryVoltage().value()); } Vectord<2> SingleJointedArmSim::UpdateX(const Vectord<2>& currentXhat, diff --git a/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h index be3a3259c5..c8019a36f9 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h @@ -78,7 +78,7 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> { * * @return The DC motor current draw. */ - units::ampere_t GetCurrentDraw() const override; + units::ampere_t GetCurrentDraw() const; /** * Sets the input voltage for the DC motor. diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index fd883bb295..03cc070cee 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -150,7 +150,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> { * * @return The elevator current draw. */ - units::ampere_t GetCurrentDraw() const override; + units::ampere_t GetCurrentDraw() const; /** * Sets the input voltage for the elevator. diff --git a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h index cc0eca9ae0..32c8aa3311 100644 --- a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h @@ -68,7 +68,7 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> { * * @return The flywheel current draw. */ - units::ampere_t GetCurrentDraw() const override; + units::ampere_t GetCurrentDraw() const; /** * Sets the input voltage for the flywheel. diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h index b44aee5626..8685435394 100644 --- a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h @@ -86,7 +86,7 @@ class LinearSystemSim { * * @param u The system inputs. */ - void SetInput(const Vectord& u) { m_u = ClampInput(u); } + void SetInput(const Vectord& u) { m_u = u; } /** * Sets the system inputs. @@ -94,10 +94,7 @@ class LinearSystemSim { * @param row The row in the input matrix to set. * @param value The value to set the row to. */ - void SetInput(int row, double value) { - m_u(row, 0) = value; - ClampInput(m_u); - } + void SetInput(int row, double value) { m_u(row, 0) = value; } /** * Returns the current input of the plant. @@ -121,14 +118,6 @@ class LinearSystemSim { */ void SetState(const Vectord& state) { m_x = state; } - /** - * Returns the current drawn by this simulated system. Override this method to - * add a custom current calculation. - * - * @return The current drawn by this simulated mechanism. - */ - virtual units::ampere_t GetCurrentDraw() const { return 0_A; } - protected: /** * Updates the state estimate of the system. @@ -147,12 +136,10 @@ class LinearSystemSim { * Clamp the input vector such that no element exceeds the given voltage. If * any does, the relative magnitudes of the input will be maintained. * - * @param u The input vector. - * @return The normalized input. + * @param maxInput The maximum magnitude of the input vector after clamping. */ - Vectord ClampInput(Vectord u) { - return frc::DesaturateInputVector( - u, frc::RobotController::GetInputVoltage()); + void ClampInput(double maxInput) { + m_u = frc::DesaturateInputVector(m_u, maxInput); } /// The plant that represents the linear system. diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h index 2476cea67c..36924f8d0f 100644 --- a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h @@ -126,7 +126,7 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> { * * @return The arm current draw. */ - units::ampere_t GetCurrentDraw() const override; + units::ampere_t GetCurrentDraw() const; /** * Sets the input voltage for the arm. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java index 67962a9626..0a86c11f68 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj.simulation; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; @@ -12,6 +11,7 @@ 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.math.util.Units; +import edu.wpi.first.wpilibj.RobotController; /** Represents a simulated DC motor mechanism. */ public class DCMotorSim extends LinearSystemSim { @@ -25,30 +25,17 @@ public class DCMotorSim extends LinearSystemSim { * Creates a simulated DC motor mechanism. * * @param plant The linear system representing the DC motor. This system can be created with - * {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(DCMotor, double, - * double)}. * @param gearbox The type of and number of motors in the DC motor gearbox. * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions). - */ - public DCMotorSim(LinearSystem plant, DCMotor gearbox, double gearing) { - super(plant); - m_gearbox = gearbox; - m_gearing = gearing; - } - - /** - * Creates a simulated DC motor mechanism. - * - * @param plant The linear system representing the DC motor. This system can be created with - * @param gearbox The type of and number of motors in the DC motor gearbox. - * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions). - * @param measurementStdDevs The standard deviations of the measurements. + * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no + * noise is desired. If present must have 2 elements. The first element is for position. The + * second element is for velocity. */ public DCMotorSim( LinearSystem plant, DCMotor gearbox, double gearing, - Matrix measurementStdDevs) { + double... measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; m_gearing = gearing; @@ -60,25 +47,13 @@ public class DCMotorSim extends LinearSystemSim { * @param gearbox The type of and number of motors in the DC motor gearbox. * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions). * @param jKgMetersSquared The moment of inertia of the DC motor. If this is unknown, use the - * {@link #DCMotorSim(LinearSystem, DCMotor, double, Matrix)} constructor. - */ - public DCMotorSim(DCMotor gearbox, double gearing, double jKgMetersSquared) { - super(LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing)); - m_gearbox = gearbox; - m_gearing = gearing; - } - - /** - * Creates a simulated DC motor mechanism. - * - * @param gearbox The type of and number of motors in the DC motor gearbox. - * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions). - * @param jKgMetersSquared The moment of inertia of the DC motor. If this is unknown, use the - * {@link #DCMotorSim(LinearSystem, DCMotor, double, Matrix)} constructor. - * @param measurementStdDevs The standard deviations of the measurements. + * {@link #DCMotorSim(LinearSystem, DCMotor, double, double...)} constructor. + * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no + * noise is desired. If present must have 2 elements. The first element is for position. The + * second element is for velocity. */ public DCMotorSim( - DCMotor gearbox, double gearing, double jKgMetersSquared, Matrix measurementStdDevs) { + DCMotor gearbox, double gearing, double jKgMetersSquared, double... measurementStdDevs) { super( LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing), measurementStdDevs); m_gearbox = gearbox; @@ -136,7 +111,6 @@ public class DCMotorSim extends LinearSystemSim { * * @return The DC motor current draw. */ - @Override 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 @@ -152,5 +126,6 @@ public class DCMotorSim extends LinearSystemSim { */ public void setInputVoltage(double volts) { setInput(volts); + clampInput(RobotController.getBatteryVoltage()); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 2f3bf83e97..2f7ce3e768 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.NumericalIntegration; 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 elevator mechanism. */ public class ElevatorSim extends LinearSystemSim { @@ -39,7 +40,8 @@ public class ElevatorSim extends LinearSystemSim { * @param maxHeightMeters The max allowable height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. - * @param measurementStdDevs The standard deviations of the measurements. + * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no + * noise is desired. If present must have 1 element for position. */ @SuppressWarnings("this-escape") public ElevatorSim( @@ -49,7 +51,7 @@ public class ElevatorSim extends LinearSystemSim { double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, - Matrix measurementStdDevs) { + double... measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; m_minHeight = minHeightMeters; @@ -59,35 +61,6 @@ public class ElevatorSim extends LinearSystemSim { setState(startingHeightMeters, 0); } - /** - * Creates a simulated elevator mechanism. - * - * @param plant The linear system that represents the elevator. This system can be created with - * {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, - * double, double)}. - * @param gearbox The type of and number of motors in the elevator gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param startingHeightMeters The starting height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. - */ - public ElevatorSim( - LinearSystem plant, - DCMotor gearbox, - double minHeightMeters, - double maxHeightMeters, - boolean simulateGravity, - double startingHeightMeters) { - this( - plant, - gearbox, - minHeightMeters, - maxHeightMeters, - simulateGravity, - startingHeightMeters, - null); - } - /** * Creates a simulated elevator mechanism. * @@ -98,37 +71,8 @@ public class ElevatorSim extends LinearSystemSim { * @param maxHeightMeters The max allowable height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. - */ - public ElevatorSim( - double kV, - double kA, - DCMotor gearbox, - double minHeightMeters, - double maxHeightMeters, - boolean simulateGravity, - double startingHeightMeters) { - this( - kV, - kA, - gearbox, - minHeightMeters, - maxHeightMeters, - simulateGravity, - startingHeightMeters, - null); - } - - /** - * Creates a simulated elevator mechanism. - * - * @param kV The velocity gain. - * @param kA The acceleration gain. - * @param gearbox The type of and number of motors in the elevator gearbox. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. - * @param startingHeightMeters The starting height of the elevator. - * @param measurementStdDevs The standard deviations of the measurements. + * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no + * noise is desired. If present must have 1 element for position. */ public ElevatorSim( double kV, @@ -138,7 +82,7 @@ public class ElevatorSim extends LinearSystemSim { double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, - Matrix measurementStdDevs) { + double... measurementStdDevs) { this( LinearSystemId.identifyPositionSystem(kV, kA), gearbox, @@ -160,7 +104,8 @@ public class ElevatorSim extends LinearSystemSim { * @param maxHeightMeters The max allowable height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. * @param startingHeightMeters The starting height of the elevator. - * @param measurementStdDevs The standard deviations of the measurements. + * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no + * noise is desired. If present must have 1 element for position. */ public ElevatorSim( DCMotor gearbox, @@ -171,7 +116,7 @@ public class ElevatorSim extends LinearSystemSim { double maxHeightMeters, boolean simulateGravity, double startingHeightMeters, - Matrix measurementStdDevs) { + double... measurementStdDevs) { this( LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing), gearbox, @@ -182,39 +127,6 @@ public class ElevatorSim extends LinearSystemSim { measurementStdDevs); } - /** - * Creates a simulated elevator mechanism. - * - * @param gearbox The type of and number of motors in the elevator gearbox. - * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). - * @param carriageMassKg The mass of the elevator carriage. - * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. - * @param minHeightMeters The min allowable height of the elevator. - * @param maxHeightMeters The max allowable height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. - * @param startingHeightMeters The starting height of the elevator. - */ - public ElevatorSim( - DCMotor gearbox, - double gearing, - double carriageMassKg, - double drumRadiusMeters, - double minHeightMeters, - double maxHeightMeters, - boolean simulateGravity, - double startingHeightMeters) { - this( - gearbox, - gearing, - carriageMassKg, - drumRadiusMeters, - minHeightMeters, - maxHeightMeters, - simulateGravity, - startingHeightMeters, - null); - } - /** * Sets the elevator's state. The new position will be limited between the minimum and maximum * allowed heights. @@ -289,7 +201,6 @@ public class ElevatorSim extends LinearSystemSim { * * @return The elevator current draw. */ - @Override public double getCurrentDrawAmps() { // I = V / R - omega / (Kv * R) // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is @@ -310,6 +221,7 @@ public class ElevatorSim extends LinearSystemSim { */ public void setInputVoltage(double volts) { setInput(volts); + clampInput(RobotController.getBatteryVoltage()); } /** 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 9c224880fd..eb3d8e6390 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 @@ -4,13 +4,13 @@ package edu.wpi.first.wpilibj.simulation; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; 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.math.util.Units; +import edu.wpi.first.wpilibj.RobotController; /** Represents a simulated flywheel mechanism. */ public class FlywheelSim extends LinearSystemSim { @@ -20,34 +20,20 @@ public class FlywheelSim extends LinearSystemSim { // The gearing from the motors to the output. private final double m_gearing; - /** - * Creates a simulated flywheel mechanism. - * - * @param plant The linear system that represents the flywheel. This system can be created with - * {@link edu.wpi.first.math.system.plant.LinearSystemId#createFlywheelSystem(DCMotor, double, - * double)}. - * @param gearbox The type of and number of motors in the flywheel gearbox. - * @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions). - */ - public FlywheelSim(LinearSystem plant, DCMotor gearbox, double gearing) { - super(plant); - m_gearbox = gearbox; - m_gearing = gearing; - } - /** * Creates a simulated flywheel mechanism. * * @param plant The linear system that represents the flywheel. * @param gearbox The type of and number of motors in the flywheel gearbox. * @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions). - * @param measurementStdDevs The standard deviations of the measurements. + * @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 gearing, - Matrix measurementStdDevs) { + double... measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; m_gearing = gearing; @@ -59,25 +45,12 @@ public class FlywheelSim extends LinearSystemSim { * @param gearbox The type of and number of motors in the flywheel gearbox. * @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions). * @param jKgMetersSquared The moment of inertia of the flywheel. If this is unknown, use the - * {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)} constructor. - */ - public FlywheelSim(DCMotor gearbox, double gearing, double jKgMetersSquared) { - super(LinearSystemId.createFlywheelSystem(gearbox, jKgMetersSquared, gearing)); - m_gearbox = gearbox; - m_gearing = gearing; - } - - /** - * Creates a simulated flywheel mechanism. - * - * @param gearbox The type of and number of motors in the flywheel gearbox. - * @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions). - * @param jKgMetersSquared The moment of inertia of the flywheel. If this is unknown, use the - * {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)} constructor. - * @param measurementStdDevs The standard deviations of the measurements. + * {@link #FlywheelSim(LinearSystem, DCMotor, double, double...)} constructor. + * @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( - DCMotor gearbox, double gearing, double jKgMetersSquared, Matrix measurementStdDevs) { + DCMotor gearbox, double gearing, double jKgMetersSquared, double... measurementStdDevs) { super( LinearSystemId.createFlywheelSystem(gearbox, jKgMetersSquared, gearing), measurementStdDevs); @@ -117,7 +90,6 @@ public class FlywheelSim extends LinearSystemSim { * * @return The flywheel current draw. */ - @Override 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 @@ -133,5 +105,6 @@ public class FlywheelSim extends LinearSystemSim { */ public void setInputVoltage(double volts) { setInput(volts); + clampInput(RobotController.getBatteryVoltage()); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java index 8207bb556f..ec296fde3d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java @@ -9,7 +9,6 @@ import edu.wpi.first.math.Num; import edu.wpi.first.math.StateSpaceUtil; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.system.LinearSystem; -import edu.wpi.first.wpilibj.RobotController; import org.ejml.MatrixDimensionException; import org.ejml.simple.SimpleMatrix; @@ -43,27 +42,28 @@ public class LinearSystemSim m_measurementStdDevs; - /** - * Creates a simulated generic linear system. - * - * @param system The system to simulate. - */ - public LinearSystemSim(LinearSystem system) { - this(system, null); - } - /** * Creates a simulated generic linear system with measurement noise. * * @param system The system being controlled. - * @param measurementStdDevs Standard deviations of measurements. Can be null if no noise is - * desired. + * @param measurementStdDevs Standard deviations of measurements. Can be empty if no noise is + * desired. If present must have same number of items as Outputs */ public LinearSystemSim( - LinearSystem system, Matrix measurementStdDevs) { + LinearSystem system, double... measurementStdDevs) { + if (measurementStdDevs.length != 0 && measurementStdDevs.length != system.getC().getNumRows()) { + throw new MatrixDimensionException( + "Malformed measurementStdDevs! Got " + + measurementStdDevs.length + + " elements instead of " + + system.getC().getNumRows()); + } this.m_plant = system; - this.m_measurementStdDevs = measurementStdDevs; - + if (measurementStdDevs.length == 0) { + m_measurementStdDevs = new Matrix<>(new SimpleMatrix(system.getC().getNumRows(), 1)); + } else { + m_measurementStdDevs = new Matrix<>(new SimpleMatrix(measurementStdDevs)); + } m_x = new Matrix<>(new SimpleMatrix(system.getA().getNumRows(), 1)); m_u = new Matrix<>(new SimpleMatrix(system.getB().getNumCols(), 1)); m_y = new Matrix<>(new SimpleMatrix(system.getC().getNumRows(), 1)); @@ -112,7 +112,7 @@ public class LinearSystemSim u) { - this.m_u = clampInput(u); + this.m_u = u; } /** @@ -123,7 +123,6 @@ public class LinearSystemSim(new SimpleMatrix(m_u.getNumRows(), 1, true, u)); - m_u = clampInput(m_u); } /** @@ -168,16 +166,6 @@ public class LinearSystemSim clampInput(Matrix u) { - return StateSpaceUtil.desaturateInputVector(u, RobotController.getBatteryVoltage()); + protected void clampInput(double maxInput) { + m_u = StateSpaceUtil.desaturateInputVector(m_u, maxInput); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java index b2f155771d..ba0442f782 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.NumericalIntegration; 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 single jointed arm mechanism. */ public class SingleJointedArmSim extends LinearSystemSim { @@ -47,7 +48,8 @@ public class SingleJointedArmSim extends LinearSystemSim { * @param maxAngleRads The maximum angle that the arm is capable of. * @param simulateGravity Whether gravity should be simulated or not. * @param startingAngleRads The initial position of the Arm simulation in radians. - * @param measurementStdDevs The standard deviations of the measurements. + * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no + * noise is desired. If present must have 1 element for position. */ @SuppressWarnings("this-escape") public SingleJointedArmSim( @@ -59,7 +61,7 @@ public class SingleJointedArmSim extends LinearSystemSim { double maxAngleRads, boolean simulateGravity, double startingAngleRads, - Matrix measurementStdDevs) { + double... measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; m_gearing = gearing; @@ -71,74 +73,6 @@ public class SingleJointedArmSim extends LinearSystemSim { setState(startingAngleRads, 0.0); } - /** - * Creates a simulated arm mechanism. - * - * @param plant The linear system that represents the arm. This system can be created with {@link - * edu.wpi.first.math.system.plant.LinearSystemId#createSingleJointedArmSystem(DCMotor, - * double, double)}. - * @param gearbox The type of and number of motors in the arm gearbox. - * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). - * @param armLengthMeters The length of the arm. - * @param minAngleRads The minimum angle that the arm is capable of. - * @param maxAngleRads The maximum angle that the arm is capable of. - * @param simulateGravity Whether gravity should be simulated or not. - * @param startingAngleRads The initial position of the Arm simulation in radians. - */ - public SingleJointedArmSim( - LinearSystem plant, - DCMotor gearbox, - double gearing, - double armLengthMeters, - double minAngleRads, - double maxAngleRads, - boolean simulateGravity, - double startingAngleRads) { - this( - plant, - gearbox, - gearing, - armLengthMeters, - minAngleRads, - maxAngleRads, - simulateGravity, - startingAngleRads, - null); - } - - /** - * Creates a simulated arm mechanism. - * - * @param gearbox The type of and number of motors in the arm gearbox. - * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). - * @param jKgMetersSquared The moment of inertia of the arm, can be calculated from CAD software. - * @param armLengthMeters The length of the arm. - * @param minAngleRads The minimum angle that the arm is capable of. - * @param maxAngleRads The maximum angle that the arm is capable of. - * @param simulateGravity Whether gravity should be simulated or not. - * @param startingAngleRads The initial position of the Arm simulation in radians. - */ - public SingleJointedArmSim( - DCMotor gearbox, - double gearing, - double jKgMetersSquared, - double armLengthMeters, - double minAngleRads, - double maxAngleRads, - boolean simulateGravity, - double startingAngleRads) { - this( - gearbox, - gearing, - jKgMetersSquared, - armLengthMeters, - minAngleRads, - maxAngleRads, - simulateGravity, - startingAngleRads, - null); - } - /** * Creates a simulated arm mechanism. * @@ -150,7 +84,8 @@ public class SingleJointedArmSim extends LinearSystemSim { * @param maxAngleRads The maximum angle that the arm is capable of. * @param simulateGravity Whether gravity should be simulated or not. * @param startingAngleRads The initial position of the Arm simulation in radians. - * @param measurementStdDevs The standard deviations of the measurements. + * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no + * noise is desired. If present must have 1 element for position. */ public SingleJointedArmSim( DCMotor gearbox, @@ -161,7 +96,7 @@ public class SingleJointedArmSim extends LinearSystemSim { double maxAngleRads, boolean simulateGravity, double startingAngleRads, - Matrix measurementStdDevs) { + double... measurementStdDevs) { this( LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing), gearbox, @@ -247,7 +182,6 @@ public class SingleJointedArmSim extends LinearSystemSim { * * @return The aram current draw. */ - @Override public double getCurrentDrawAmps() { // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is // spinning 10x faster than the output @@ -262,6 +196,7 @@ public class SingleJointedArmSim extends LinearSystemSim { */ public void setInputVoltage(double volts) { setInput(volts); + clampInput(RobotController.getBatteryVoltage()); } /** diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java index 73a28100ce..deade245fa 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java @@ -35,7 +35,8 @@ class ElevatorSimTest { 3.0, true, 0.0, - VecBuilder.fill(0.01, 0.00)); + 0.01, + 0.0); try (var motor = new PWMVictorSPX(0); var encoder = new Encoder(0, 1)) { @@ -74,7 +75,8 @@ class ElevatorSimTest { 1.0, true, 0.0, - VecBuilder.fill(0.01, 0.00)); + 0.01, + 0.0); for (int i = 0; i < 100; i++) { sim.setInput(VecBuilder.fill(0)); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java index fcf53d5f50..3c92370019 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj.examples.armsimulation.subsystems; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.util.Units; @@ -51,8 +50,8 @@ public class Arm implements AutoCloseable { Constants.kMaxAngleRads, true, 0, - VecBuilder.fill( - Constants.kArmEncoderDistPerPulse, 0.0) // Add noise with a std-dev of 1 tick + Constants.kArmEncoderDistPerPulse, + 0.0 // Add noise with a std-dev of 1 tick ); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java index 66fb6ec58d..ac2423f434 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialsimulation/subsystems/Elevator.java @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj.examples.elevatorexponentialsimulation.subsystems; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.system.plant.DCMotor; @@ -60,7 +59,8 @@ public class Elevator implements AutoCloseable { Constants.kMaxElevatorHeightMeters, true, 0, - VecBuilder.fill(0.005, 0.0)); + 0.005, + 0.0); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); private final PWMSim m_motorSim = new PWMSim(m_motor); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java index 8c0b0f6bbc..d4f04fed26 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj.examples.elevatorsimulation.subsystems; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.system.plant.DCMotor; @@ -55,7 +54,8 @@ public class Elevator implements AutoCloseable { Constants.kMaxElevatorHeightMeters, true, 0, - VecBuilder.fill(0.01, 0.0)); + 0.01, + 0.0); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); private final PWMSim m_motorSim = new PWMSim(m_motor); diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java index 0da5e3d930..8a3d92b8e6 100644 --- a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java @@ -55,8 +55,7 @@ class PotentiometerPIDTest { 0.0, Robot.kFullHeightMeters, true, - 0, - null); + 0); m_analogSim = new AnalogInputSim(Robot.kPotChannel); m_motorSim = new PWMSim(Robot.kMotorChannel); m_joystickSim = new JoystickSim(Robot.kJoystickChannel);