Add set tolerance method to rotation PID controller

lasarobotics · Sep 18, 2024 · 66d8012 · 66d8012
1 parent 629f3c9
commit 66d8012
Showing 1 changed file with 9 additions and 0 deletions.
diff --git a/src/main/java/org/lasarobotics/drive/RotatePIDController.java b/src/main/java/org/lasarobotics/drive/RotatePIDController.java
@@ -89,6 +89,15 @@ public Measure<Velocity<Angle>> calculate(Measure<Angle> currentAngle, Measure<V
     return Units.DegreesPerSecond.of(m_pidController.calculate(currentAngle.in(Units.Degrees)));
   }
 
+  /**
+   * Set the tolerable error
+   * @param positionTolerance Position error that is tolerable
+   * @param velocityTolerance Velocity error that is tolerable
+   */
+  public void setTolerance(Measure<Angle> positionTolerance, Measure<Velocity<Angle>> velocityTolerance) {
+    m_pidController.setTolerance(positionTolerance.in(Units.Degrees), velocityTolerance.in(Units.DegreesPerSecond));
+  }
+
   /**
    * Set setpoint of rotation PID controller
    * @param angle