Actually applying PID changes. Renamed symbols

Author: KalticCodes

XRPLib/differential_drive.py

@@ -145,20 +145,20 @@ def straight(self, distance: float, max_effort: float = 0.5, timeout: float = No
             distance *= -1
 
         time_out = Timeout(timeout)
-        startingLeft = self.get_left_encoder_position()
-        startingRight = self.get_right_encoder_position()
+        starting_left = self.get_left_encoder_position()
+        starting_right = self.get_right_encoder_position()
 
 
         if main_controller is None:
             main_controller = PID(
                 kp = 0.04,
                 ki = 0.0125,
                 kd = 0.025,
-                minOutput = 0.225,
-                maxOutput = 0.5,
-                maxIntegral = 35,
+                min_output = 0.225,
+                max_output = 0.5,
+                max_integral = 35,
                 tolerance = 0.1,
-                toleranceCount = 3,
+                tolerance_count = 3,
             )
 
         # Secondary controller to keep encoder values in sync
@@ -176,13 +176,13 @@ def straight(self, distance: float, max_effort: float = 0.5, timeout: float = No
         while True:
 
             # calculate the distance traveled
-            leftDelta = self.get_left_encoder_position() - startingLeft
-            rightDelta = self.get_right_encoder_position() - startingRight
-            distTraveled = (leftDelta + rightDelta) / 2
+            left_delta = self.get_left_encoder_position() - starting_left
+            right_delta = self.get_right_encoder_position() - starting_right
+            dist_traveled = (left_delta + right_delta) / 2
 
             # PID for distance
-            distanceError = distance - distTraveled
-            effort = main_controller.update(distanceError)
+            distance_error = distance - dist_traveled
+            effort = main_controller.update(distance_error)
 
             if main_controller.is_done() or time_out.is_done():
                 break
@@ -192,7 +192,7 @@ def straight(self, distance: float, max_effort: float = 0.5, timeout: float = No
                 # record current heading to maintain it
                 current_heading = self.imu.get_yaw()
             else:
-                current_heading = ((rightDelta-leftDelta)/2)*360/(self.track_width*math.pi)
+                current_heading = ((right_delta-left_delta)/2)*360/(self.track_width*math.pi)
 
             headingCorrection = secondary_controller.update(initial_heading - current_heading)
 
@@ -232,20 +232,20 @@ def turn(self, turn_degrees: float, max_effort: float = 40, timeout: float = Non
             turn_degrees *= -1
 
         time_out = Timeout(timeout)
-        startingLeft = self.get_left_encoder_position()
-        startingRight = self.get_right_encoder_position()
+        starting_left = self.get_left_encoder_position()
+        starting_right = self.get_right_encoder_position()
 
         if main_controller is None:
             main_controller = PID(
                 kp = 0.02,
                 ki = 0.005,
                 kd = 0.012,
-                minOutput = 0.35,
-                maxOutput = max_effort,
-                maxDerivative = 0.5,
-                maxIntegral = 50,
+                min_output = 0.35,
+                max_output = max_effort,
+                max_derivative = 0.5,
+                max_integral = 50,
                 tolerance = 0.5,
-                toleranceCount = 2
+                tolerance_count = 2
             )
 
         # Secondary controller to keep encoder values in sync
@@ -260,25 +260,25 @@ def turn(self, turn_degrees: float, max_effort: float = 40, timeout: float = Non
         while True:
 
             # calculate encoder correction to minimize drift
-            leftDelta = self.get_left_encoder_position() - startingLeft
-            rightDelta = self.get_right_encoder_position() - startingRight
-            encoderCorrection = secondary_controller.update(leftDelta + rightDelta)
+            left_delta = self.get_left_encoder_position() - starting_left
+            right_delta = self.get_right_encoder_position() - starting_right
+            encoder_correction = secondary_controller.update(left_delta + right_delta)
 
             if use_imu and (self.imu is not None):
                 # calculate turn error (in degrees) from the imu
-                turnError = turn_degrees - self.imu.get_yaw()
+                turn_error = turn_degrees - self.imu.get_yaw()
             else:
                 # calculate turn error (in degrees) from the encoder counts
-                turnError = turn_degrees - ((rightDelta-leftDelta)/2)*360/(self.track_width*math.pi)
+                turn_error = turn_degrees - ((right_delta-left_delta)/2)*360/(self.track_width*math.pi)
 
             # Pass the turn error to the main controller to get a turn speed
-            turnSpeed = main_controller.update(turnError)
+            turn_speed = main_controller.update(turn_error)
 
             # exit if timeout or tolerance reached
             if main_controller.is_done() or time_out.is_done():
                 break
 
-            self.set_speed(-turnSpeed - encoderCorrection, turnSpeed - encoderCorrection)
+            self.set_speed(-turn_speed - encoder_correction, turn_speed - encoder_correction)
 
             time.sleep(0.01)
 

XRPLib/pid.py

@@ -11,43 +11,45 @@ def __init__(self,
                  kp = 1.0,
                  ki = 0.0,
                  kd = 0.0,
-                 minOutput = 0.0,
-                 maxOutput = 1.0,
-                 maxDerivative = None,
+                 min_output = 0.0,
+                 max_output = 1.0,
+                 max_derivative = None,
+                 max_integral = None,
                  tolerance = 0.1,
-                 toleranceCount = 1
+                 tolerance_count = 1
                  ):
         """
         :param kp: proportional gain
         :param ki: integral gain
         :param kd: derivative gain
-        :param minOutput: minimum output
-        :param maxOutput: maximum output
-        :param maxDerivative: maximum derivative (change per second)
+        :param min_output: minimum output
+        :param max_output: maximum output
+        :param max_derivative: maximum derivative (change per second)
+        :param max_integral: maximum integral windup allowed (will cap integral at this value)
         :param tolerance: tolerance for exit condition
-        :param numTimesInTolerance: number of times in tolerance for exit condition
+        :param tolerance_count: number of times the error needs to be within tolerance for is_done to return True
         """
         self.kp = kp
         self.ki = ki
         self.kd = kd
-        self.minOutput = minOutput
-        self.maxOutput = maxOutput
-        self.maxDerivative = maxDerivative
+        self.min_output = min_output
+        self.max_output = max_output
+        self.max_derivative = max_derivative
+        self.max_integral = max_integral
         self.tolerance = tolerance
-        self.toleranceCount = toleranceCount
+        self.tolerance_count = tolerance_count
 
-        self.prevError = 0
-        self.prevIntegral = 0
-        self.prevOutput = 0
+        self.prev_error = 0
+        self.prev_integral = 0
+        self.prev_output = 0
 
-        self.startTime = None
-        self.prevTime = None
+        self.start_time = None
+        self.prev_time = None
 
         # number of actual times in tolerance
         self.times = 0
 
     def _handle_exit_condition(self, error: float):
-
         if abs(error) < self.tolerance:
             # if error is within tolerance, increment times in tolerance
             self.times += 1
@@ -65,43 +67,47 @@ def update(self, error: float) -> float:
         :return: The system output from the controller, to be used as an effort value or for any other purpose
         :rtype: float
         """
-        currentTime = time.ticks_ms()
-        if self.prevTime is None:
+        current_time = time.ticks_ms()
+        if self.prev_time is None:
             # First update after instantiation
-            self.startTime = currentTime
+            self.start_time = current_time
             timestep = 0.01
         else:
             # get time delta in seconds
-            timestep = time.ticks_diff(currentTime, self.prevTime) / 1000
-        self.prevTime = currentTime # cache time for next update
+            timestep = time.ticks_diff(current_time, self.prev_time) / 1000
+        self.prev_time = current_time # cache time for next update
 
         self._handle_exit_condition(error)
 
-        integral = self.prevIntegral + error * timestep
-        derivative = (error - self.prevError) / timestep
+        integral = self.prev_integral + error * timestep
+        
+        if self.max_integral is not None:
+            integral = max(-self.max_integral, min(self.max_integral, integral))
+
+        derivative = (error - self.prev_error) / timestep
 
         # derive output
         output = self.kp * error + self.ki * integral + self.kd * derivative
-        self.prevError = error
-        self.prevIntegral = integral
+        self.prev_error = error
+        self.prev_integral = integral
 
         # Bound output by minimum
         if output > 0:
-            output = max(self.minOutput, output)
+            output = max(self.min_output, output)
         else:
-            output = min(-self.minOutput, output)
+            output = min(-self.min_output, output)
 
         # Bound output by maximum
-        output = max(-self.maxOutput, min(self.maxOutput, output))
+        output = max(-self.max_output, min(self.max_output, output))
 
         # Bound output by maximum acceleration
-        if self.maxDerivative is not None:
-            lowerBound = self.prevOutput - self.maxDerivative * timestep
-            upperBound = self.prevOutput + self.maxDerivative * timestep
-            output = max(lowerBound, min(upperBound, output))
+        if self.max_derivative is not None:
+            lower_bound = self.prev_output - self.max_derivative * timestep
+            upper_bound = self.prev_output + self.max_derivative * timestep
+            output = max(lower_bound, min(upper_bound, output))
 
         # cache output for next update
-        self.prevOutput = output
+        self.prev_output = output
 
         return output
 
@@ -110,12 +116,11 @@ def is_done(self) -> bool:
         :return: if error is within tolerance for numTimesInTolerance consecutive times, or timed out
         :rtype: bool
         """
-
-        return self.times >= self.toleranceCount
+        return self.times >= self.tolerance_count
 
     def clear_history(self):
-        self.prevError = 0
-        self.prevIntegral = 0
-        self.prevOutput = 0
-        self.times = 0
-        self.prevTime = None
+        self.prev_error = 0
+        self.prev_integral = 0
+        self.prev_output = 0
+        self.prev_time = None
+        self.times = 0