From 5e1500600797ee64a7b006905ef0d3a609993d7a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9l=C3=A8ne=20Corbineau?= Date: Mon, 13 Jan 2025 17:52:51 +0100 Subject: [PATCH] PID Controller basics --- controller.py | 39 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 37 insertions(+), 2 deletions(-) diff --git a/controller.py b/controller.py index 4d3f1d8..e219f8c 100644 --- a/controller.py +++ b/controller.py @@ -8,9 +8,44 @@ class Controller: assert isinstance(gtm, GCodeToMotors) return None - +# We control position, but the output of the controller is a set speed. +# Therefore, we have proportional, differential and second differential terms. class PIDController(Controller): + def __init__(self, p, i, d): + err_x, err_y, err_z = 0., 0., 0. + derr_x, derr_y, derr_z = 0., 0., 0. + d2err_x, d2err_y, d2err_z = 0., 0., 0. + + self.p, self.i, self.d = p,i,d + def __call__(self, gtm, *args, **kwargs): - self. + err_x_new, err_y_new, err_z_new = gtm.delta_steps + derr_x_new, derr_y_new, derr_z_new = + err_x_new - err_x, err_y_new - err_y, err_z_new - err_z + d2err_x_new, d2err_y_new, d2err_z_new = + derr_x_new - derr_x, derr_y_new - derr_y, derr_z_new - derr_z + err_x,err_y,err_z = err_x_new, err_y_new, err_z_new + derr_x,derr_y,derr_z = derr_x_new, derr_y_new, derr_z_new + d2err_x,d2err_y,d2err_z = d2err_x_new,d2err_y_new,d2err_z_new + + speed_cmd_x = self.p * derr_x + self.i * err_x + self.d * d2err_x + speed_cmd_y = self.p * derr_y + self.i * err_y + self.d * d2err_y + speed_cmd_z = self.p * derr_z + self.i * err_z + self.d * d2err_z + # It is the responsability of the coefficients to make sure this results in the desired feedrate + gtm.x_throttle = gtm.target_feedrate/abs(speed_cmd_x) + gtm.x_direction = 1 if (speed_cmd_x > 0) else 0 + + gtm.y_throttle = gtm.target_feedrate/abs(speed_cmd_y) + gtm.y_direction = 1 if (speed_cmd_y > 0) else 0 + + gtm.z_throttle = gtm.target_feedrate/abs(speed_cmd_z) + gtm.z_direction = 1 if (speed_cmd_z > 0) else 0 + + if(gtm.x_throttle > 65536): + gtm.x_throttle = 0 + if(gtm.y_throttle > 65536): + gtm.y_throttle = 0 + if(gtm.z_throttle > 65536): + gtm.z_throttle = 0