implemented gain scheduling and some other tweaks for better steady s…

…tate performance

pi/ball_controller.py

@@ -1,41 +1,57 @@
 from point import Point
 import numpy as np
+import math
 
 
 class BallController:
-    def __init__(self, kp_x, ki_x, kd_x, kp_y, ki_y, kd_y, dt, saturation_angle):
-        # X-axis PID parameters
-        self.kp_x = kp_x
-        self.ki_x = ki_x
-        self.kd_x = kd_x
+    def __init__(self, kp_x, ki_x, kd_x, kp_y, ki_y, kd_y, dt, saturation_angle, max_euclidean_error, integral_windup_clear_threshold):
+        # X-axis PID parameter range
+        self.kp_range_x = kp_x
+        self.ki_range_x = ki_x
+        self.kd_range_x = kd_x
 
-        # Y-axis PID parameters
-        self.kp_y = kp_y
-        self.ki_y = ki_y
-        self.kd_y = kd_y
+        # Y-axis PID parameter range
+        self.kp_range_y = kp_y
+        self.ki_range_y = ki_y
+        self.kd_range_y = kd_y
 
         # Time step
         self.dt = dt
 
         # Saturation angle
         self.saturation_angle = saturation_angle
 
+        # Max Euclidean error for gain scheduling
+        self.max_euclidean_error = max_euclidean_error
+
+        # Max integral error before clearing, to prevent against windup
+        self.integral_windup_clear_threshold = integral_windup_clear_threshold
+
         # Initialize errors for x and y
         self.integral_error_x = 0
         self.integral_error_y = 0
 
         self.previous_error_x = 0
         self.previous_error_y = 0
 
-    def set_gains(self, kp_x, ki_x, kd_x, kp_y, ki_y, kd_y):
-        self.kp_x = kp_x
-        self.ki_x = ki_x
-        self.kd_x = kd_x
+        self.current_kp_x = 0
+        self.current_ki_x = 0
+        self.current_kd_x = 0
+
+        self.current_kp_y = 0
+        self.current_ki_y = 0
+        self.current_kd_y = 0
 
-        self.kp_y = kp_y
-        self.ki_y = ki_y
-        self.kd_y = kd_y
+    # def set_gains(self, kp_x, ki_x, kd_x, kp_y, ki_y, kd_y):
+    #     self.kp_x = kp_x
+    #     self.ki_x = ki_x
+    #     self.kd_x = kd_x
 
+    #     self.kp_y = kp_y
+    #     self.ki_y = ki_y
+    #     self.kd_y = kd_y
+
+    # Use gains as determined by euclidean error magnitude
     def update(self, error, integral_error, previous_error, kp, ki, kd):
         # Calculate integral and derivative errors
         integral_error += error * self.dt
@@ -55,27 +71,50 @@ def run_control_loop(self, desired_position: Point, current_position: Point):
         # error for y is inverted
         error_y = -error_y
 
+        error_euclidean = math.sqrt(error_x ** 2 + error_y ** 2)
+
+        # Interpolate between inner and outer gains to get current gains
+        self.update_instantaneous_gains(error_euclidean)
+
+
         # Update X control
         output_x, self.integral_error_x, self.previous_error_x = self.update(
             error_x,
             self.integral_error_x,
             self.previous_error_x,
-            self.kp_x,
-            self.ki_x,
-            self.kd_x,
+            self.current_kp_x,
+            self.current_ki_x,
+            self.current_kd_x,
         )
 
         # Update Y control
         output_y, self.integral_error_y, self.previous_error_y = self.update(
             error_y,
             self.integral_error_y,
             self.previous_error_y,
-            self.kp_y,
-            self.ki_y,
-            self.kd_y,
+            self.current_kp_y,
+            self.current_ki_y,
+            self.current_kd_y,
         )
 
+        if (self.integral_error_x > self.integral_windup_clear_threshold):
+            self.integral_error_x = 0
+
+        if (self.integral_error_y > self.integral_windup_clear_threshold):
+            self.integral_error_y = 0
+
         output_x = np.clip(output_x, -self.saturation_angle, self.saturation_angle)
         output_y = np.clip(output_y, -self.saturation_angle, self.saturation_angle)
 
         return output_x, output_y
+
+    def update_instantaneous_gains(self, euclidean_error):
+        self.current_kp_x = np.interp(euclidean_error, [0, self.max_euclidean_error], self.kp_range_x)
+        self.current_ki_x = np.interp(euclidean_error, [0, self.max_euclidean_error], self.ki_range_x)
+        self.current_kd_x = np.interp(euclidean_error, [0, self.max_euclidean_error], self.kd_range_x)
+
+        self.current_kp_y = np.interp(euclidean_error, [0, self.max_euclidean_error], self.kp_range_y)
+        self.current_ki_y = np.interp(euclidean_error, [0, self.max_euclidean_error], self.ki_range_y)
+        self.current_kd_y = np.interp(euclidean_error, [0, self.max_euclidean_error], self.kd_range_y)
+
+

pi/main.py

@@ -40,13 +40,13 @@ def __init__(
         cam_calibration_images: bool = False,
     ):
         self.dt = 0.02
-        self.saturate_angle = 6
+        self.saturate_angle = 12
         self.params = {
             "lh": 67.5 / 1000,
             "la": 122.2 / 1000,
             "platform_attachment_radius": 70 / 1000,
             "base_attachment_radius": 100 / 1000,
-            "resting_height": 0.17,
+            "resting_height": 0.15,
         }
 
         self.servo_offset_radians = [
@@ -84,18 +84,20 @@ def __init__(
             - self.params["platform_attachment_radius"],
         )
 
-        kp = 1.0
-        ki = 0.0
-        kd = 0.7
+        kp_range = [1.0, 2.0]
+        ki_range = [2.5, 0.0]
+        kd_range = [0.65, 0.65]
         self.ball_controller = BallController(
-            kp,
-            ki,
-            kd,
-            kp,
-            ki,
-            kd,
+            kp_range,
+            ki_range,
+            kd_range,
+            kp_range,
+            ki_range,
+            kd_range,
             self.dt,
             np.deg2rad(self.saturate_angle),
+            max_euclidean_error=0.15,
+            integral_windup_clear_threshold=10,
         )
 
         self.run_visualizer = run_visualizer
@@ -219,7 +221,7 @@ def run(self):
                     ki_x = self.slider_ki.val
                     kd_x = self.slider_kd.val
 
-                    self.ball_controller.set_gains(kp_x, ki_x, kd_x, kp_x, ki_x, kd_x)
+                    # self.ball_controller.set_gains(kp_x, ki_x, kd_x, kp_x, ki_x, kd_x)
 
                 pitch_rad = output_angles[0] + self.platform_offset_radians[0]
                 roll_rad = output_angles[1] + self.platform_offset_radians[1]