[examples] Fix flaky ArmSimulationTest (#7170)

Co-authored-by: Starlight220 <[email protected]>

Author: narmstro2020

wpilibcExamples/src/test/cpp/examples/ArmSimulation/cpp/ArmSimulationTest.cpp

@@ -50,11 +50,11 @@ class ArmSimulationTest : public testing::TestWithParam<units::degree_t> {
 TEST_P(ArmSimulationTest, Teleop) {
   EXPECT_TRUE(frc::Preferences::ContainsKey(kArmPositionKey));
   EXPECT_TRUE(frc::Preferences::ContainsKey(kArmPKey));
-  EXPECT_DOUBLE_EQ(kDefaultArmSetpoint.value(),
-                   frc::Preferences::GetDouble(kArmPositionKey, NAN));
-
   frc::Preferences::SetDouble(kArmPositionKey, GetParam().value());
   units::degree_t setpoint = GetParam();
+  EXPECT_DOUBLE_EQ(setpoint.value(),
+                   frc::Preferences::GetDouble(kArmPositionKey, NAN));
+
   // teleop init
   {
     frc::sim::DriverStationSim::SetAutonomous(false);
@@ -68,7 +68,7 @@ TEST_P(ArmSimulationTest, Teleop) {
   {
     frc::sim::StepTiming(3_s);
 
-    // Ensure elevator is still at 0.
+    // Ensure arm is still at minimum angle.
     EXPECT_NEAR(kMinAngle.value(), m_encoderSim.GetDistance(), 2.0);
   }
 

wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/armsimulation/ArmSimulationTest.java

@@ -71,11 +71,8 @@ void stopThread() {
   void teleopTest(double setpoint) {
     assertTrue(Preferences.containsKey(Constants.kArmPositionKey));
     assertTrue(Preferences.containsKey(Constants.kArmPKey));
-    assertEquals(
-        Constants.kDefaultArmSetpointDegrees,
-        Preferences.getDouble(Constants.kArmPositionKey, Double.NaN));
-
     Preferences.setDouble(Constants.kArmPositionKey, setpoint);
+    assertEquals(setpoint, Preferences.getDouble(Constants.kArmPositionKey, Double.NaN));
     // teleop init
     {
       DriverStationSim.setAutonomous(false);
@@ -87,10 +84,10 @@ void teleopTest(double setpoint) {
     }
 
     {
-      // advance 50 timesteps
+      // advance 150 timesteps
       SimHooks.stepTiming(3);
 
-      // Ensure elevator is still at 0.
+      // Ensure arm is still at minimum angle.
       assertEquals(Constants.kMinAngleRads, m_encoderSim.getDistance(), 2.0);
     }
 
@@ -115,7 +112,7 @@ void teleopTest(double setpoint) {
       m_joystickSim.setTrigger(false);
       m_joystickSim.notifyNewData();
 
-      // advance 75 timesteps
+      // advance 150 timesteps
       SimHooks.stepTiming(3.0);
 
       assertEquals(Constants.kMinAngleRads, m_encoderSim.getDistance(), 2.0);