mboyer02/GCode-Generator
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

fixed controller class

Browse source
This commit is contained in:
mpboyer 2025-01-14 17:10:10 +01:00
parent 19f1ca3cc3
commit 7616d4f93e
2 changed files with 22 additions and 19 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
.gitignore vendored
View file

@ -1 +1,3 @@
.idea/ .idea/
__pycache__
out

37
controller.py
View file

@ -5,33 +5,31 @@ class Controller:
pass pass
def __call__(self, gtm, *args, **kwargs): def __call__(self, gtm, *args, **kwargs):
assert isinstance(gtm)
return None return None
# We control position, but the output of the controller is a set speed. # We control position, but the output of the controller is a set speed.
# Therefore, we have proportional, differential and second differential terms. # Therefore, we have proportional, differential and second differential terms.
class PIDController(Controller): class PIDController(Controller):
def __init__(self, p, i, d): def __init__(self, p, i, d):
err_x, err_y, err_z = 0., 0., 0. super().__init__()
derr_x, derr_y, derr_z = 0., 0., 0. self.err_x, self.err_y, self.err_z = 0., 0., 0.
d2err_x, d2err_y, d2err_z = 0., 0., 0. self.derr_x, self.derr_y, self.derr_z = 0., 0., 0.
self.d2err_x, self.d2err_y, self.d2err_z = 0., 0., 0.
self.p, self.i, self.d = p,i,d self.p, self.i, self.d = p,i,d
def __call__(self, gtm, *args, **kwargs): def __call__(self, gtm, *args, **kwargs):
err_x_new, err_y_new, err_z_new = gtm.delta_steps err_x_new, err_y_new, err_z_new = gtm.delta_steps
derr_x_new, derr_y_new, derr_z_new = derr_x_new, derr_y_new, derr_z_new = err_x_new - self.err_x, err_y_new - self.err_y, err_z_new - self.err_z
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 - self.derr_x, derr_y_new - self.derr_y, derr_z_new - self.derr_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 self.err_x, self.err_y, self.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 self.derr_x, self.derr_y, self.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 self.d2err_x, self.d2err_y, self.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_x = self.p * self.derr_x + self.i * self.err_x + self.d * self.d2err_x
speed_cmd_y = self.p * derr_y + self.i * err_y + self.d * d2err_y speed_cmd_y = self.p * self.derr_y + self.i * self.err_y + self.d * self.d2err_y
speed_cmd_z = self.p * derr_z + self.i * err_z + self.d * d2err_z speed_cmd_z = self.p * self.derr_z + self.i * self.err_z + self.d * self.d2err_z
# It is the responsability of the coefficients to make sure this results in the desired feedrate # 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_throttle = gtm.target_feedrate/abs(speed_cmd_x)
@ -43,9 +41,12 @@ class PIDController(Controller):
gtm.z_throttle = gtm.target_feedrate/abs(speed_cmd_z) gtm.z_throttle = gtm.target_feedrate/abs(speed_cmd_z)
gtm.z_direction = 1 if (speed_cmd_z > 0) else 0 gtm.z_direction = 1 if (speed_cmd_z > 0) else 0
if(gtm.x_throttle > 65536): if gtm.x_throttle > 65536:
gtm.x_throttle = 0 gtm.x_throttle = 0
if(gtm.y_throttle > 65536):
if gtm.y_throttle > 65536:
gtm.y_throttle = 0 gtm.y_throttle = 0
if(gtm.z_throttle > 65536):
if gtm.z_throttle > 65536:
gtm.z_throttle = 0 gtm.z_throttle = 0