diff --git a/pi/ball_controller.py b/pi/ball_controller.py index 3b89419..ecb0a60 100644 --- a/pi/ball_controller.py +++ b/pi/ball_controller.py @@ -1,18 +1,19 @@ 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 @@ -20,6 +21,12 @@ def __init__(self, kp_x, ki_x, kd_x, kp_y, ki_y, kd_y, dt, saturation_angle): # 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 @@ -27,15 +34,24 @@ def __init__(self, kp_x, ki_x, kd_x, kp_y, ki_y, kd_y, dt, saturation_angle): 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,14 +71,20 @@ 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 @@ -70,12 +92,29 @@ def run_control_loop(self, desired_position: Point, current_position: Point): 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) + + diff --git a/pi/main.py b/pi/main.py index 683abd0..d4af9d4 100644 --- a/pi/main.py +++ b/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]