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);