Add test and fix reset

wpilibsuite · Jan 6, 2023 · 1f94532 · 1f94532
1 parent 57fa451
commit 1f94532
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 1 deletion.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
@@ -231,7 +231,8 @@ public double getSetpoint() {
    * @return Whether the error is within the acceptable bounds.
    */
   public boolean atSetpoint() {
-    return m_haveMeasurement && m_haveSetpoint 
+    return m_haveMeasurement
+        && m_haveSetpoint
         && Math.abs(m_positionError) < m_positionTolerance
         && Math.abs(m_velocityError) < m_velocityTolerance;
   }
@@ -366,6 +367,7 @@ public void reset() {
     m_prevError = 0;
     m_totalError = 0;
     m_velocityError = 0;
+    m_haveMeasurement = false;
   }
 
   @Override

diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java
@@ -5,6 +5,8 @@
 package edu.wpi.first.math.controller;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
@@ -55,4 +57,16 @@ void derivativeGainOutputTest() {
 
     assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
   }
+
+  @Test
+  void atSetpointTest() {
+    m_controller.reset();
+    assertFalse(m_controller.atSetpoint());
+    m_controller.setTolerance(2);
+    assertFalse(m_controller.atSetpoint());
+    m_controller.setSetpoint(0);
+    assertFalse(m_controller.atSetpoint());
+    m_controller.calculate(0);
+    assertTrue(m_controller.atSetpoint());
+  }
 }