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

[wpilib] LinearSystemSim.setState: calculate new output state (#7799)

Browse Source 
Establishes the invariant that the state and measurement always match up, even immediately after construction.
This commit is contained in:
HarryXChen
2025-02-19 01:49:28 -05:00
committed by GitHub
parent bd2211119f
commit c898853b4d
6 changed files with 80 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
View File

@@ -63,6 +63,17 @@ class ElevatorSimTest {
}
}
@Test
void testInitialState() {
double startingHeightMeters = 0.5;
var sim =
new ElevatorSim(
DCMotor.getKrakenX60(2), 20, 8.0, 0.1, 0.0, 1.0, true, startingHeightMeters, 0.01, 0.0);
assertEquals(startingHeightMeters, sim.getPositionMeters());
assertEquals(0, sim.getVelocityMetersPerSecond());
}
@Test
void testMinMax() {
var sim =

48
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java
View File

@@ -12,28 +12,46 @@ import edu.wpi.first.math.util.Units;
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,
Math.PI / 2.0);
@Test
void testArmDisabled() {
SingleJointedArmSim sim =
new SingleJointedArmSim(
DCMotor.getVex775Pro(2),
300,
3.0,
Units.inchesToMeters(30.0),
-Math.PI,
0.0,
true,
Math.PI / 2.0);
// Reset Arm angle to 0
m_sim.setState(VecBuilder.fill(0.0, 0.0));
sim.setState(VecBuilder.fill(0.0, 0.0));
for (int i = 0; i < 12 / 0.02; i++) {
m_sim.setInput(0.0);
m_sim.update(0.020);
sim.setInput(0.0);
sim.update(0.020);
}
// the arm should swing down
assertEquals(-Math.PI / 2.0, m_sim.getAngleRads(), 0.1);
assertEquals(-Math.PI / 2.0, sim.getAngleRads(), 0.1);
}
@Test
void testInitialState() {
double startingAngleRads = Math.PI / 4.0;
SingleJointedArmSim sim =
new SingleJointedArmSim(
DCMotor.getKrakenX60(2),
125,
3.0,
Units.inchesToMeters(30.0),
0,
Math.PI / 2.0,
true,
startingAngleRads);
assertEquals(startingAngleRads, sim.getAngleRads());
assertEquals(0, sim.getVelocityRadPerSec());
}
}