Merge pull request #49 from Open-STEM/SpeedControlBugFix

Speed control bug fix, Straight and Turn retuned

Author: AnselChang

.gitignore

@@ -1,5 +1,6 @@
 .vscode/
 .git/
 docs/_build/
-
+.micropico
+.picowgo
 secrets.json

.picowgo

@@ -38,7 +38,7 @@ def circle():
 # Follow the perimeter of a square with variable sidelength
 def square(sidelength):
     for sides in range(4):
-        drivetrain.straight(sidelength, 80)
+        drivetrain.straight(sidelength, 0.8)
         drivetrain.turn(90)
     # Alternatively:
     # polygon(sidelength, 4)

Examples/drive_examples.py

@@ -145,24 +145,26 @@ 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.075,
-                kd = 0.005,
-                minOutput = 0.25,
-                maxOutput = max_effort,
-                tolerance = 0.1,
-                toleranceCount = 3,
+                kp = 0.1,
+                ki = 0.04,
+                kd = 0.06,
+                min_output = 0.25,
+                max_output = 0.5,
+                max_integral = 10,
+                tolerance = 0.25,
+                tolerance_count = 3,
             )
 
         # Secondary controller to keep encoder values in sync
         if secondary_controller is None:
             secondary_controller = PID(
-                kp = 0.025, kd=0.0025,
+                kp = 0.075, kd=0.005,
             )
 
         if self.imu is not None:
@@ -174,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
@@ -190,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)
 
@@ -206,12 +208,12 @@ def straight(self, distance: float, max_effort: float = 0.5, timeout: float = No
     def turn(self, turn_degrees: float, max_effort: float = 0.5, timeout: float = None, main_controller: Controller = None, secondary_controller: Controller = None, use_imu:bool = True) -> bool:
         """
         Turn the robot some relative heading given in turnDegrees, and exit function when the robot has reached that heading.
-        Speed is bounded from -1 (turn counterclockwise the relative heading at full speed) to 1 (turn clockwise the relative heading at full speed)
+        effort is bounded from -1 (turn counterclockwise the relative heading at full speed) to 1 (turn clockwise the relative heading at full speed)
         Uses the IMU to determine the heading of the robot and P control for the motor controller.
 
         :param turnDegrees: The number of angle for the robot to turn (In Degrees)
         :type turnDegrees: float
-        :param max_effort: The max effort for which the robot to travel (Bounded from -1 to 1). Default is half effort forward.
+        :param max_effort: The max speed for which the robot to travel (Bounded from -1 to 1)
         :type max_effort: float
         :param timeout: The amount of time before the robot stops trying to turn and continues to the next step (In Seconds)
         :type timeout: float
@@ -226,27 +228,29 @@ def turn(self, turn_degrees: float, max_effort: float = 0.5, timeout: float = No
         """
 
         if max_effort < 0:
-            max_effort *= -1
-            turn_degrees *= -1
+            max_effort = -max_effort
+            turn_degrees = -turn_degrees
 
         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 = .015,
-                kd = 0.0012,
-                minOutput = 0.25,
-                maxOutput = max_effort,
-                tolerance = 0.5,
-                toleranceCount = 3
+                kp = 0.02,
+                ki = 0.001,
+                kd = 0.00165,
+                min_output = 0.35,
+                max_output = 0.5,
+                max_integral = 75,
+                tolerance = 1,
+                tolerance_count = 3
             )
 
         # Secondary controller to keep encoder values in sync
         if secondary_controller is None:
             secondary_controller = PID(
-                kp = 0.002,
+                kp = 1.0,
             )
 
         if use_imu and (self.imu is not None):
@@ -255,25 +259,25 @@ def turn(self, turn_degrees: float, max_effort: float = 0.5, timeout: float = No
         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_effort(-turnSpeed - encoderCorrection, turnSpeed - encoderCorrection)
+            self.set_effort(-turn_speed - encoder_correction, turn_speed - encoder_correction)
 
             time.sleep(0.01)
 

XRPLib/differential_drive.py

@@ -115,21 +115,23 @@ def get_speed(self) -> float:
     def set_speed(self, speed_rpm: float = None):
         """
         Sets target speed (in rpm) to be maintained passively
-        Call with no parameters to turn off speed control
+        Call with no parameters or 0 to turn off speed control
 
         :param target_speed_rpm: The target speed for the motor in rpm, or None
         :type target_speed_rpm: float, or None
         """
-        if speed_rpm is None:
+        if speed_rpm is None or speed_rpm == 0:
             self.target_speed = None
             self.set_effort(0)
             self.speed = 0
+            self.updateTimer.deinit()
             return
         # If the update timer is not running, start it at 50 Hz (20ms updates)
         self.updateTimer.init(period=20, callback=lambda t:self._update())
         # Convert from rev per min to counts per 20ms (60 sec/min, 50 Hz)
         self.target_speed = speed_rpm*self._encoder.resolution/(60*50)
         self.speedController.clear_history()
+        self.prev_position = self.get_position_counts()
 
     def set_speed_controller(self, new_controller: Controller):
         """

XRPLib/encoded_motor.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
+        output = self.kp * error + self.ki * integral - self.kd * derivative
+        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,11 +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.prev_error = 0
+        self.prev_integral = 0
+        self.prev_output = 0
+        self.prev_time = None
         self.times = 0

XRPLib/pid.py

@@ -11,7 +11,7 @@
 # using the EncodedMotor since the default drivetrain uses the IMU and takes 3 seconds to init
 for i in range(4):
     motor = EncodedMotor.get_default_encoded_motor(i+1)
-    motor.set_effort(0)
+    motor.set_speed(0)
     motor.reset_encoder_position()
 
 # Turn off the on-board LED