Skip to content

Commit

Permalink
finalize elevatorSim
Browse files Browse the repository at this point in the history
  • Loading branch information
@narmstro2020
narmstro2020 committed Apr 1, 2024
1 parent 2e84d74 commit 052f285
Show file tree
Hide file tree
Showing 2 changed files with 110 additions and 122 deletions.
189 changes: 79 additions & 110 deletions wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,49 +12,54 @@
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;

/** Represents a simulated elevator mechanism. */
public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
// Gearbox for the elevator.
private final DCMotor m_gearbox;

// The gearing from the elevator motors to the output.
private final double m_gearing;

// The min allowable height for the elevator.
private final double m_minHeight;

// The max allowable height for the elevator.
private final double m_maxHeight;

// Whether the simulator should simulate gravity.
private final boolean m_simulateGravity;
//The effective gravity of the system factoring in friction
private final double m_gMetersPerSecondSq;

/**
* 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 kG The gravity gain of the elevator.
* @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 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.
*/
@SuppressWarnings("this-escape")
public ElevatorSim(
LinearSystem<N2, N1, N1> plant,
double kG,
DCMotor gearbox,
double gearing,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
m_minHeight = minHeightMeters;
m_maxHeight = maxHeightMeters;
m_simulateGravity = simulateGravity;
m_gMetersPerSecondSq = kG * plant.getB(0, 0);

setState(startingHeightMeters, 0);
}
Expand All @@ -65,155 +70,94 @@ public ElevatorSim(
* @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 kG The gravity gain of the elevator.
* @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 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, N1> plant,
double kG,
DCMotor gearbox,
double gearing,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters) {
this(
plant,
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.
*/
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);
}
super(plant);
m_gearbox = gearbox;
m_gearing = gearing;
m_minHeight = minHeightMeters;
m_maxHeight = maxHeightMeters;
m_gMetersPerSecondSq = kG * plant.getB(0, 0);

/**
* 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.
*/
public ElevatorSim(
double kV,
double kA,
DCMotor gearbox,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
this(
LinearSystemId.identifyPositionSystem(kV, kA),
gearbox,
minHeightMeters,
maxHeightMeters,
simulateGravity,
startingHeightMeters,
measurementStdDevs);
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 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 gMetersPerSecond The effective gravity of the elevator.
* @param startingHeightMeters The starting height of the elevator.
* @param measurementStdDevs The standard deviations of the measurements.
*/
public ElevatorSim(
LinearSystem<N2, N1, N1> plant,
DCMotor gearbox,
double gearing,
double carriageMassKg,
double drumRadiusMeters,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double gMetersPerSecond,
double startingHeightMeters,
Matrix<N1, N1> measurementStdDevs) {
this(
LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
gearbox,
minHeightMeters,
maxHeightMeters,
simulateGravity,
startingHeightMeters,
measurementStdDevs);
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
m_minHeight = minHeightMeters;
m_maxHeight = maxHeightMeters;
m_gMetersPerSecondSq = gMetersPerSecond;

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 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 gMetersPerSecond The effective gravity of the elevator.
* @param startingHeightMeters The starting height of the elevator.
*/
public ElevatorSim(
LinearSystem<N2, N1, N1> plant,
DCMotor gearbox,
double gearing,
double carriageMassKg,
double drumRadiusMeters,
double minHeightMeters,
double maxHeightMeters,
boolean simulateGravity,
double gMetersPerSecond,
double startingHeightMeters) {
this(
gearbox,
gearing,
carriageMassKg,
drumRadiusMeters,
minHeightMeters,
maxHeightMeters,
simulateGravity,
startingHeightMeters,
null);
}
super(plant);
m_gearbox = gearbox;
m_gearing = gearing;
m_minHeight = minHeightMeters;
m_maxHeight = maxHeightMeters;
m_gMetersPerSecondSq = gMetersPerSecond;

setState(startingHeightMeters, 0);
}

/**
* Sets the elevator's state. The new position will be limited between the minimum and maximum
Expand Down Expand Up @@ -304,6 +248,24 @@ public double getCurrentDrawAmps() {
* Math.signum(appliedVoltage);
}

/**
* Returns the elevator's drum radius.
*
* @return The elevator's drum radius.
*/
public double getDrumRadiusMeters() {
return -m_gearing * m_plant.getB(0, 0) / (m_gearbox.KvRadPerSecPerVolt * m_plant.getA(1, 1));
}

/**
* Returns the carriage mass of the elvator.
*
* @return The elevator's carriage mass.
*/
public double getCarriageMassKg() {
return -m_plant.getA(1, 1) * m_gearbox.KvRadPerSecPerVolt * m_gearbox.KtNMPerAmp / (m_gearbox.rOhms * Math.pow(m_plant.getB(0, 0), 2));
}

/**
* Sets the input voltage for the elevator.
*
Expand All @@ -313,6 +275,15 @@ public void setInputVoltage(double volts) {
setInput(volts);
}

/**
* Gets the input voltage for the DC motor.
*
* @return The DC motor's input voltage.
*/
public double getInputVoltage() {
return getInput(0);
}

/**
* Updates the state of the elevator.
*
Expand All @@ -327,9 +298,7 @@ protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, d
NumericalIntegration.rkdp(
(x, _u) -> {
Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
if (m_simulateGravity) {
xdot = xdot.plus(VecBuilder.fill(0, -9.8));
}
xdot = xdot.plus(VecBuilder.fill(0, -m_gMetersPerSecondSq));
return xdot;
},
currentXhat,
Expand Down
43 changes: 31 additions & 12 deletions wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,16 +24,19 @@ void testStateSpaceSimWithElevator() {

@SuppressWarnings("resource")
var controller = new PIDController(10, 0, 0);

var plant = LinearSystemId.createElevatorSystem(
DCMotor.getVex775Pro(4),
8,
0.75 * 25.4 / 1000.0,
14.67);
var sim =
new ElevatorSim(
plant,
DCMotor.getVex775Pro(4),
14.67,
8,
0.75 * 25.4 / 1000.0,
0.0,
3.0,
true,
0,
3,
9.8,
0.0,
VecBuilder.fill(0.01));

Expand Down Expand Up @@ -64,15 +67,19 @@ void testStateSpaceSimWithElevator() {

@Test
void testMinMax() {
var plant = LinearSystemId.createElevatorSystem(
DCMotor.getVex775Pro(4),
8,
0.75 * 25.4 / 1000.0,
14.67);
var sim =
new ElevatorSim(
plant,
DCMotor.getVex775Pro(4),
14.67,
8.0,
0.75 * 25.4 / 1000.0,
0.0,
1.0,
true,
0,
3,
9.8,
0.0,
VecBuilder.fill(0.01));

Expand All @@ -93,9 +100,21 @@ void testMinMax() {

@Test
void testStability() {
var plant = LinearSystemId.createElevatorSystem(
DCMotor.getVex775Pro(4),
4,
Units.inchesToMeters(0.5),
100);
var sim =
new ElevatorSim(
DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false, 0.0);
plant,
DCMotor.getVex775Pro(4),
100,
0,
10,
0.0,
0.0,
VecBuilder.fill(0.01));

sim.setState(VecBuilder.fill(0, 0));
sim.setInput(12);
Expand Down

0 comments on commit 052f285

Please sign in to comment.