mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpilib] Simulation: Add ctor parameter to set starting state of mechanism sims (#5288)
- Add a constructor parameter to configure the initial angle of the arm - Also reorganizes cascading constructors for Java
This commit is contained in:
@@ -16,6 +16,7 @@ ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
|
||||
const DCMotor& gearbox, double gearing,
|
||||
units::meter_t drumRadius, units::meter_t minHeight,
|
||||
units::meter_t maxHeight, bool simulateGravity,
|
||||
units::meter_t startingHeight,
|
||||
const std::array<double, 1>& measurementStdDevs)
|
||||
: LinearSystemSim(plant, measurementStdDevs),
|
||||
m_gearbox(gearbox),
|
||||
@@ -23,22 +24,21 @@ ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
|
||||
m_minHeight(minHeight),
|
||||
m_maxHeight(maxHeight),
|
||||
m_gearing(gearing),
|
||||
m_simulateGravity(simulateGravity) {}
|
||||
m_simulateGravity(simulateGravity) {
|
||||
SetState(
|
||||
frc::Vectord<2>{std::clamp(startingHeight, minHeight, maxHeight), 0.0});
|
||||
}
|
||||
|
||||
ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
|
||||
units::kilogram_t carriageMass,
|
||||
units::meter_t drumRadius, units::meter_t minHeight,
|
||||
units::meter_t maxHeight, bool simulateGravity,
|
||||
units::meter_t startingHeight,
|
||||
const std::array<double, 1>& measurementStdDevs)
|
||||
: LinearSystemSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
|
||||
drumRadius, gearing),
|
||||
measurementStdDevs),
|
||||
m_gearbox(gearbox),
|
||||
m_drumRadius(drumRadius),
|
||||
m_minHeight(minHeight),
|
||||
m_maxHeight(maxHeight),
|
||||
m_gearing(gearing),
|
||||
m_simulateGravity(simulateGravity) {}
|
||||
: ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
|
||||
drumRadius, gearing),
|
||||
gearbox, gearing, drumRadius, minHeight, maxHeight,
|
||||
simulateGravity, startingHeight, measurementStdDevs) {}
|
||||
|
||||
bool ElevatorSim::WouldHitLowerLimit(units::meter_t elevatorHeight) const {
|
||||
return elevatorHeight <= m_minHeight;
|
||||
|
||||
@@ -19,6 +19,7 @@ SingleJointedArmSim::SingleJointedArmSim(
|
||||
const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing,
|
||||
units::meter_t armLength, units::radian_t minAngle,
|
||||
units::radian_t maxAngle, bool simulateGravity,
|
||||
units::radian_t startingAngle,
|
||||
const std::array<double, 1>& measurementStdDevs)
|
||||
: LinearSystemSim<2, 1, 1>(system, measurementStdDevs),
|
||||
m_armLen(armLength),
|
||||
@@ -26,17 +27,20 @@ SingleJointedArmSim::SingleJointedArmSim(
|
||||
m_maxAngle(maxAngle),
|
||||
m_gearbox(gearbox),
|
||||
m_gearing(gearing),
|
||||
m_simulateGravity(simulateGravity) {}
|
||||
m_simulateGravity(simulateGravity) {
|
||||
SetState(frc::Vectord<2>{startingAngle, 0.0});
|
||||
}
|
||||
|
||||
SingleJointedArmSim::SingleJointedArmSim(
|
||||
const DCMotor& gearbox, double gearing, units::kilogram_square_meter_t moi,
|
||||
units::meter_t armLength, units::radian_t minAngle,
|
||||
units::radian_t maxAngle, bool simulateGravity,
|
||||
units::radian_t startingAngle,
|
||||
const std::array<double, 1>& measurementStdDevs)
|
||||
: SingleJointedArmSim(
|
||||
LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing),
|
||||
gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity,
|
||||
measurementStdDevs) {}
|
||||
startingAngle, measurementStdDevs) {}
|
||||
|
||||
bool SingleJointedArmSim::WouldHitLowerLimit(units::radian_t armAngle) const {
|
||||
return armAngle <= m_minAngle;
|
||||
|
||||
@@ -32,12 +32,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
|
||||
* @param minHeight The minimum allowed height of the elevator.
|
||||
* @param maxHeight The maximum allowed height of the elevator.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingHeight The starting height of the elevator.
|
||||
* @param measurementStdDevs The standard deviation of the measurements.
|
||||
*/
|
||||
ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox,
|
||||
double gearing, units::meter_t drumRadius,
|
||||
units::meter_t minHeight, units::meter_t maxHeight,
|
||||
bool simulateGravity,
|
||||
bool simulateGravity, units::meter_t startingHeight,
|
||||
const std::array<double, 1>& measurementStdDevs = {0.0});
|
||||
|
||||
/**
|
||||
@@ -53,12 +54,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
|
||||
* @param minHeight The minimum allowed height of the elevator.
|
||||
* @param maxHeight The maximum allowed height of the elevator.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingHeight The starting height of the elevator.
|
||||
* @param measurementStdDevs The standard deviation of the measurements.
|
||||
*/
|
||||
ElevatorSim(const DCMotor& gearbox, double gearing,
|
||||
units::kilogram_t carriageMass, units::meter_t drumRadius,
|
||||
units::meter_t minHeight, units::meter_t maxHeight,
|
||||
bool simulateGravity,
|
||||
bool simulateGravity, units::meter_t startingHeight,
|
||||
const std::array<double, 1>& measurementStdDevs = {0.0});
|
||||
|
||||
/**
|
||||
|
||||
@@ -31,12 +31,14 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
|
||||
* @param minAngle The minimum angle that the arm is capable of.
|
||||
* @param maxAngle The maximum angle that the arm is capable of.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingAngle The initial position of the arm.
|
||||
* @param measurementStdDevs The standard deviations of the measurements.
|
||||
*/
|
||||
SingleJointedArmSim(const LinearSystem<2, 1, 1>& system,
|
||||
const DCMotor& gearbox, double gearing,
|
||||
units::meter_t armLength, units::radian_t minAngle,
|
||||
units::radian_t maxAngle, bool simulateGravity,
|
||||
units::radian_t startingAngle,
|
||||
const std::array<double, 1>& measurementStdDevs = {0.0});
|
||||
/**
|
||||
* Creates a simulated arm mechanism.
|
||||
@@ -50,12 +52,14 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
|
||||
* @param minAngle The minimum angle that the arm is capable of.
|
||||
* @param maxAngle The maximum angle that the arm is capable of.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingAngle The initial position of the arm.
|
||||
* @param measurementStdDevs The standard deviation of the measurement noise.
|
||||
*/
|
||||
SingleJointedArmSim(const DCMotor& gearbox, double gearing,
|
||||
units::kilogram_square_meter_t moi,
|
||||
units::meter_t armLength, units::radian_t minAngle,
|
||||
units::radian_t maxAngle, bool simulateGravity,
|
||||
units::radian_t startingAngle,
|
||||
const std::array<double, 1>& measurementStdDevs = {0.0});
|
||||
|
||||
/**
|
||||
|
||||
@@ -21,7 +21,7 @@
|
||||
|
||||
TEST(ElevatorSimTest, StateSpaceSim) {
|
||||
frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, 0.75_in,
|
||||
0_m, 3_m, true, {0.01});
|
||||
0_m, 3_m, true, 0_m, {0.01});
|
||||
frc2::PIDController controller(10, 0.0, 0.0);
|
||||
|
||||
frc::PWMVictorSPX motor(0);
|
||||
@@ -46,7 +46,7 @@ TEST(ElevatorSimTest, StateSpaceSim) {
|
||||
|
||||
TEST(ElevatorSimTest, MinMax) {
|
||||
frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, 0.75_in,
|
||||
0_m, 1_m, true, {0.01});
|
||||
0_m, 1_m, true, 0_m, {0.01});
|
||||
for (size_t i = 0; i < 100; ++i) {
|
||||
sim.SetInput(frc::Vectord<1>{0.0});
|
||||
sim.Update(20_ms);
|
||||
@@ -66,7 +66,7 @@ TEST(ElevatorSimTest, MinMax) {
|
||||
|
||||
TEST(ElevatorSimTest, Stability) {
|
||||
frc::sim::ElevatorSim sim{
|
||||
frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, false};
|
||||
frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, false, 0_m};
|
||||
|
||||
sim.SetState(frc::Vectord<2>{0.0, 0.0});
|
||||
sim.SetInput(frc::Vectord<1>{12.0});
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
|
||||
TEST(SingleJointedArmTest, Disabled) {
|
||||
frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 300, 3_kg_sq_m,
|
||||
30_in, -180_deg, 0_deg, true);
|
||||
30_in, -180_deg, 0_deg, true, 90_deg);
|
||||
sim.SetState(frc::Vectord<2>{0.0, 0.0});
|
||||
|
||||
for (size_t i = 0; i < 12 / 0.02; ++i) {
|
||||
|
||||
@@ -53,6 +53,7 @@ class Arm {
|
||||
kMinAngle,
|
||||
kMaxAngle,
|
||||
true,
|
||||
0_deg,
|
||||
{kArmEncoderDistPerPulse}};
|
||||
frc::sim::EncoderSim m_encoderSim{m_encoder};
|
||||
|
||||
|
||||
@@ -56,6 +56,7 @@ class Elevator {
|
||||
Constants::kMinElevatorHeight,
|
||||
Constants::kMaxElevatorHeight,
|
||||
true,
|
||||
0_m,
|
||||
{0.01}};
|
||||
frc::sim::EncoderSim m_encoderSim{m_encoder};
|
||||
|
||||
|
||||
@@ -38,7 +38,8 @@ class PotentiometerPIDTest : public testing::Test {
|
||||
kElevatorDrumRadius,
|
||||
0.0_m,
|
||||
Robot::kFullHeight,
|
||||
true};
|
||||
true,
|
||||
0.0_m};
|
||||
frc::sim::PWMSim m_motorSim{Robot::kMotorChannel};
|
||||
frc::sim::AnalogInputSim m_analogSim{Robot::kPotChannel};
|
||||
frc::sim::JoystickSim m_joystickSim{Robot::kJoystickChannel};
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
|
||||
package edu.wpi.first.wpilibj.simulation;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
@@ -43,6 +44,8 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
|
||||
* @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.
|
||||
*/
|
||||
public ElevatorSim(
|
||||
LinearSystem<N2, N1, N1> plant,
|
||||
@@ -51,16 +54,19 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
|
||||
double drumRadiusMeters,
|
||||
double minHeightMeters,
|
||||
double maxHeightMeters,
|
||||
boolean simulateGravity) {
|
||||
this(
|
||||
plant,
|
||||
gearbox,
|
||||
gearing,
|
||||
drumRadiusMeters,
|
||||
minHeightMeters,
|
||||
maxHeightMeters,
|
||||
simulateGravity,
|
||||
null);
|
||||
boolean simulateGravity,
|
||||
double startingHeightMeters,
|
||||
Matrix<N1, N1> measurementStdDevs) {
|
||||
super(plant, measurementStdDevs);
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
m_drumRadius = drumRadiusMeters;
|
||||
m_minHeight = minHeightMeters;
|
||||
m_maxHeight = maxHeightMeters;
|
||||
m_simulateGravity = simulateGravity;
|
||||
|
||||
setState(
|
||||
VecBuilder.fill(MathUtil.clamp(startingHeightMeters, minHeightMeters, maxHeightMeters), 0));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -72,8 +78,8 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
|
||||
* @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 startingHeightMeters The starting height of the elevator.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param measurementStdDevs The standard deviations of the measurements.
|
||||
*/
|
||||
public ElevatorSim(
|
||||
LinearSystem<N2, N1, N1> plant,
|
||||
@@ -83,43 +89,16 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
|
||||
double minHeightMeters,
|
||||
double maxHeightMeters,
|
||||
boolean simulateGravity,
|
||||
Matrix<N1, N1> measurementStdDevs) {
|
||||
super(plant, measurementStdDevs);
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
m_drumRadius = drumRadiusMeters;
|
||||
m_minHeight = minHeightMeters;
|
||||
m_maxHeight = maxHeightMeters;
|
||||
m_simulateGravity = simulateGravity;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
public ElevatorSim(
|
||||
DCMotor gearbox,
|
||||
double gearing,
|
||||
double carriageMassKg,
|
||||
double drumRadiusMeters,
|
||||
double minHeightMeters,
|
||||
double maxHeightMeters,
|
||||
boolean simulateGravity) {
|
||||
double startingHeightMeters) {
|
||||
this(
|
||||
plant,
|
||||
gearbox,
|
||||
gearing,
|
||||
carriageMassKg,
|
||||
drumRadiusMeters,
|
||||
minHeightMeters,
|
||||
maxHeightMeters,
|
||||
simulateGravity,
|
||||
startingHeightMeters,
|
||||
null);
|
||||
}
|
||||
|
||||
@@ -133,6 +112,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
|
||||
* @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.
|
||||
*/
|
||||
public ElevatorSim(
|
||||
@@ -143,16 +123,51 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
|
||||
double minHeightMeters,
|
||||
double maxHeightMeters,
|
||||
boolean simulateGravity,
|
||||
double startingHeightMeters,
|
||||
Matrix<N1, N1> measurementStdDevs) {
|
||||
super(
|
||||
this(
|
||||
LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
|
||||
gearbox,
|
||||
gearing,
|
||||
drumRadiusMeters,
|
||||
minHeightMeters,
|
||||
maxHeightMeters,
|
||||
simulateGravity,
|
||||
startingHeightMeters,
|
||||
measurementStdDevs);
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
m_drumRadius = drumRadiusMeters;
|
||||
m_minHeight = minHeightMeters;
|
||||
m_maxHeight = maxHeightMeters;
|
||||
m_simulateGravity = simulateGravity;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -43,6 +43,8 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
|
||||
* @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.
|
||||
* @param measurementStdDevs The standard deviations of the measurements.
|
||||
*/
|
||||
public SingleJointedArmSim(
|
||||
LinearSystem<N2, N1, N1> plant,
|
||||
@@ -51,16 +53,18 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
|
||||
double armLengthMeters,
|
||||
double minAngleRads,
|
||||
double maxAngleRads,
|
||||
boolean simulateGravity) {
|
||||
this(
|
||||
plant,
|
||||
gearbox,
|
||||
gearing,
|
||||
armLengthMeters,
|
||||
minAngleRads,
|
||||
maxAngleRads,
|
||||
simulateGravity,
|
||||
null);
|
||||
boolean simulateGravity,
|
||||
double startingAngleRads,
|
||||
Matrix<N1, N1> measurementStdDevs) {
|
||||
super(plant, measurementStdDevs);
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
m_armLenMeters = armLengthMeters;
|
||||
m_minAngle = minAngleRads;
|
||||
m_maxAngle = maxAngleRads;
|
||||
m_simulateGravity = simulateGravity;
|
||||
|
||||
setState(VecBuilder.fill(startingAngleRads, 0));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -73,7 +77,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
|
||||
* @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 measurementStdDevs The standard deviations of the measurements.
|
||||
* @param startingAngleRads The initial position of the Arm simulation in radians.
|
||||
*/
|
||||
public SingleJointedArmSim(
|
||||
LinearSystem<N2, N1, N1> plant,
|
||||
@@ -83,14 +87,17 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
|
||||
double minAngleRads,
|
||||
double maxAngleRads,
|
||||
boolean simulateGravity,
|
||||
Matrix<N1, N1> measurementStdDevs) {
|
||||
super(plant, measurementStdDevs);
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
m_armLenMeters = armLengthMeters;
|
||||
m_minAngle = minAngleRads;
|
||||
m_maxAngle = maxAngleRads;
|
||||
m_simulateGravity = simulateGravity;
|
||||
double startingAngleRads) {
|
||||
this(
|
||||
plant,
|
||||
gearbox,
|
||||
gearing,
|
||||
armLengthMeters,
|
||||
minAngleRads,
|
||||
maxAngleRads,
|
||||
simulateGravity,
|
||||
startingAngleRads,
|
||||
null);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -103,6 +110,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
|
||||
* @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,
|
||||
@@ -111,7 +119,8 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
|
||||
double armLengthMeters,
|
||||
double minAngleRads,
|
||||
double maxAngleRads,
|
||||
boolean simulateGravity) {
|
||||
boolean simulateGravity,
|
||||
double startingAngleRads) {
|
||||
this(
|
||||
gearbox,
|
||||
gearing,
|
||||
@@ -120,6 +129,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
|
||||
minAngleRads,
|
||||
maxAngleRads,
|
||||
simulateGravity,
|
||||
startingAngleRads,
|
||||
null);
|
||||
}
|
||||
|
||||
@@ -133,6 +143,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
|
||||
* @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.
|
||||
* @param measurementStdDevs The standard deviations of the measurements.
|
||||
*/
|
||||
public SingleJointedArmSim(
|
||||
@@ -143,16 +154,18 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
|
||||
double minAngleRads,
|
||||
double maxAngleRads,
|
||||
boolean simulateGravity,
|
||||
double startingAngleRads,
|
||||
Matrix<N1, N1> measurementStdDevs) {
|
||||
super(
|
||||
this(
|
||||
LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
|
||||
gearbox,
|
||||
gearing,
|
||||
armLengthMeters,
|
||||
minAngleRads,
|
||||
maxAngleRads,
|
||||
simulateGravity,
|
||||
startingAngleRads,
|
||||
measurementStdDevs);
|
||||
m_gearbox = gearbox;
|
||||
m_gearing = gearing;
|
||||
m_armLenMeters = armLengthMeters;
|
||||
m_minAngle = minAngleRads;
|
||||
m_maxAngle = maxAngleRads;
|
||||
m_simulateGravity = simulateGravity;
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -33,6 +33,7 @@ class ElevatorSimTest {
|
||||
0.0,
|
||||
3.0,
|
||||
true,
|
||||
0.0,
|
||||
VecBuilder.fill(0.01));
|
||||
|
||||
try (var motor = new PWMVictorSPX(0);
|
||||
@@ -71,6 +72,7 @@ class ElevatorSimTest {
|
||||
0.0,
|
||||
1.0,
|
||||
true,
|
||||
0.0,
|
||||
VecBuilder.fill(0.01));
|
||||
|
||||
for (int i = 0; i < 100; i++) {
|
||||
@@ -91,7 +93,8 @@ class ElevatorSimTest {
|
||||
@Test
|
||||
void testStability() {
|
||||
var sim =
|
||||
new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false);
|
||||
new ElevatorSim(
|
||||
DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false, 0.0);
|
||||
|
||||
sim.setState(VecBuilder.fill(0, 0));
|
||||
sim.setInput(12);
|
||||
|
||||
@@ -14,10 +14,18 @@ import org.junit.jupiter.api.Test;
|
||||
class SingleJointedArmSimTest {
|
||||
SingleJointedArmSim m_sim =
|
||||
new SingleJointedArmSim(
|
||||
DCMotor.getVex775Pro(2), 300, 3.0, Units.inchesToMeters(30.0), -Math.PI, 0.0, true);
|
||||
DCMotor.getVex775Pro(2),
|
||||
300,
|
||||
3.0,
|
||||
Units.inchesToMeters(30.0),
|
||||
-Math.PI,
|
||||
0.0,
|
||||
true,
|
||||
Math.PI / 2.0);
|
||||
|
||||
@Test
|
||||
void testArmDisabled() {
|
||||
// Reset Arm angle to 0
|
||||
m_sim.setState(VecBuilder.fill(0.0, 0.0));
|
||||
|
||||
for (int i = 0; i < 12 / 0.02; i++) {
|
||||
|
||||
@@ -50,6 +50,7 @@ public class Arm implements AutoCloseable {
|
||||
Constants.kMinAngleRads,
|
||||
Constants.kMaxAngleRads,
|
||||
true,
|
||||
0,
|
||||
VecBuilder.fill(Constants.kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick
|
||||
);
|
||||
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
|
||||
|
||||
@@ -54,6 +54,7 @@ public class Elevator implements AutoCloseable {
|
||||
Constants.kMinElevatorHeightMeters,
|
||||
Constants.kMaxElevatorHeightMeters,
|
||||
true,
|
||||
0,
|
||||
VecBuilder.fill(0.01));
|
||||
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
|
||||
private final PWMSim m_motorSim = new PWMSim(m_motor);
|
||||
|
||||
@@ -55,6 +55,7 @@ class PotentiometerPIDTest {
|
||||
0.0,
|
||||
Robot.kFullHeightMeters,
|
||||
true,
|
||||
0,
|
||||
null);
|
||||
m_analogSim = new AnalogInputSim(Robot.kPotChannel);
|
||||
m_motorSim = new PWMSim(Robot.kMotorChannel);
|
||||
|
||||
Reference in New Issue
Block a user