Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] LinearSystemSim Constructor and method cleanup (#6502)

Browse Source 
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 <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-05-15 13:40:30 -04:00
committed by GitHub
parent 0f45fe9486
commit ab315e24c8
19 changed files with 83 additions and 310 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java
View File

@@ -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<N2, N1, N2> {
@@ -25,30 +25,17 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
* 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<N2, N1, N2> 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<N2, N1, N2> plant,
DCMotor gearbox,
double gearing,
Matrix<N2, N1> measurementStdDevs) {
double... measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
@@ -60,25 +47,13 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
* @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<N2, N1> 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<N2, N1, N2> {
*
* @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<N2, N1, N2> {
*/
public void setInputVoltage(double volts) {
setInput(volts);
clampInput(RobotController.getBatteryVoltage());
}
}

110
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -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<N2, N1, N2> {
@@ -39,7 +40,8 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* @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<N2, N1, N2> {
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N2, N1> measurementStdDevs) {
double... measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_minHeight = minHeightMeters;
@@ -59,35 +61,6 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
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<N2, N1, N2> 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<N2, N1, N2> {
* @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<N2, N1, N2> {
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N2, N1> measurementStdDevs) {
double... measurementStdDevs) {
this(
LinearSystemId.identifyPositionSystem(kV, kA),
gearbox,
@@ -160,7 +104,8 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* @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<N2, N1, N2> {
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N2, N1> measurementStdDevs) {
double... measurementStdDevs) {
this(
LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
gearbox,
@@ -182,39 +127,6 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
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<N2, N1, N2> {
*
* @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<N2, N1, N2> {
*/
public void setInputVoltage(double volts) {
setInput(volts);
clampInput(RobotController.getBatteryVoltage());
}
/**

45
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
View File

@@ -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<N1, N1, N1> {
@@ -20,34 +20,20 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
// 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<N1, N1, N1> 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<N1, N1, N1> plant,
DCMotor gearbox,
double gearing,
Matrix<N1, N1> measurementStdDevs) {
double... measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
@@ -59,25 +45,12 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
* @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<N1, N1> 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<N1, N1, N1> {
*
* @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<N1, N1, N1> {
*/
public void setInputVoltage(double volts) {
setInput(volts);
clampInput(RobotController.getBatteryVoltage());
}
}

53
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
View File

@@ -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<States extends Num, Inputs extends Num, Outputs ext
/** The standard deviations of measurements, used for adding noise to the measurements. */
protected final Matrix<Outputs, N1> m_measurementStdDevs;
/**
* Creates a simulated generic linear system.
*
* @param system The system to simulate.
*/
public LinearSystemSim(LinearSystem<States, Inputs, Outputs> 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<States, Inputs, Outputs> system, Matrix<Outputs, N1> measurementStdDevs) {
LinearSystem<States, Inputs, Outputs> 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<States extends Num, Inputs extends Num, Outputs ext
* @param u The system inputs.
*/
public void setInput(Matrix<Inputs, N1> u) {
this.m_u = clampInput(u);
this.m_u = u;
}
/**
@@ -123,7 +123,6 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
*/
public void setInput(int row, double value) {
m_u.set(row, 0, value);
m_u = clampInput(m_u);
}
/**
@@ -137,7 +136,6 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
"Malformed input! Got " + u.length + " elements instead of " + m_u.getNumRows());
}
m_u = new Matrix<>(new SimpleMatrix(m_u.getNumRows(), 1, true, u));
m_u = clampInput(m_u);
}
/**
@@ -168,16 +166,6 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
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.
*/
public double getCurrentDrawAmps() {
return 0.0;
}
/**
* Updates the state estimate of the system.
*
@@ -192,13 +180,12 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
}
/**
* Clamp the input vector such that no element exceeds the given voltage. If any does, the
* Clamp the input vector such that no element exceeds the maximum allowed value. 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.
*/
protected Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> u) {
return StateSpaceUtil.desaturateInputVector(u, RobotController.getBatteryVoltage());
protected void clampInput(double maxInput) {
m_u = StateSpaceUtil.desaturateInputVector(m_u, maxInput);
}
}

81
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
View File

@@ -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<N2, N1, N2> {
@@ -47,7 +48,8 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
* @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<N2, N1, N2> {
double maxAngleRads,
boolean simulateGravity,
double startingAngleRads,
Matrix<N2, N1> measurementStdDevs) {
double... measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
@@ -71,74 +73,6 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
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<N2, N1, N2> 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<N2, N1, N2> {
* @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<N2, N1, N2> {
double maxAngleRads,
boolean simulateGravity,
double startingAngleRads,
Matrix<N2, N1> measurementStdDevs) {
double... measurementStdDevs) {
this(
LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
gearbox,
@@ -247,7 +182,6 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
*
* @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<N2, N1, N2> {
*/
public void setInputVoltage(double volts) {
setInput(volts);
clampInput(RobotController.getBatteryVoltage());
}
/**