105 lines
4 KiB
Python
105 lines
4 KiB
Python
import numpy as np
|
|
class Controller:
|
|
def __init__(self):
|
|
pass
|
|
|
|
def __call__(self, gtm, *args, **kwargs):
|
|
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):
|
|
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.
|
|
|
|
def __init__(self, p, i, d):
|
|
super().__init__()
|
|
self.p, self.i, self.d = p, i, d
|
|
|
|
def __call__(self, gtm, *args, **kwargs):
|
|
err_x_new, err_y_new, err_z_new = gtm.delta_steps
|
|
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
|
|
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
|
|
|
|
self.err_x, self.err_y, self.err_z = err_x_new, err_y_new, err_z_new
|
|
self.derr_x, self.derr_y, self.derr_z = derr_x_new, derr_y_new, derr_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 * self.derr_x + self.i * self.err_x + self.d * self.d2err_x
|
|
speed_cmd_y = self.p * self.derr_y + self.i * self.err_y + self.d * self.d2err_y
|
|
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
|
|
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
|
|
|
|
|
|
class DummyController(Controller):
|
|
last_delta = np.nan
|
|
|
|
def __init__(self):
|
|
super().__init__()
|
|
|
|
def __call__(self, gtm, *args, **kwargs):
|
|
|
|
feed = gtm.feedrate
|
|
dx, dy, dz = gtm.delta_units
|
|
feed_x = feed * dx/(dx**2 + dy**2 + dz**2)**0.5
|
|
feed_y = feed * dy/(dx**2 + dy**2 + dz**2)**0.5
|
|
feed_z = feed * dz/(dz**2 + dy**2 + dz**2)**0.5
|
|
|
|
print("feed (m/min) : ", feed)
|
|
print("current : ", gtm.current_units)
|
|
print("target : ", gtm.target_units)
|
|
print("deltas: ", dx, dy, dz)
|
|
|
|
gtm.x_direction = 1 if gtm.delta_steps[0] > 0 else 0
|
|
gtm.y_direction = 1 if gtm.delta_steps[1] > 0 else 0
|
|
gtm.z_direction = 1 if gtm.delta_steps[2] > 0 else 0
|
|
|
|
feed_x,feed_y,feed_z = abs(feed_x), abs(feed_y), abs(feed_z)
|
|
print("Feeds (m/min) : ", feed_x, feed_y, feed_z)
|
|
print("dirs : ", gtm.x_direction, gtm.y_direction, gtm.z_direction)
|
|
|
|
gtm.x_throttle = 0 if feed_x == 0. else np.floor(gtm.FAST_XY_FEEDRATE/feed_x)
|
|
gtm.y_throttle = 0 if feed_y == 0. else np.floor(gtm.FAST_XY_FEEDRATE/feed_y)
|
|
gtm.z_throttle = 0 if feed_z == 0. else np.floor(gtm.FAST_Z_FEEDRATE/feed_z)
|
|
|
|
gtm.x_throttle = 0 if gtm.x_throttle >= 65536 else gtm.x_throttle
|
|
gtm.y_throttle = 0 if gtm.y_throttle >= 65536 else gtm.y_throttle
|
|
gtm.z_throttle = 0 if gtm.z_throttle >= 65536 else gtm.z_throttle
|
|
|
|
print("Throttles : ", gtm.x_throttle, gtm.y_throttle, gtm.z_throttle)
|
|
|
|
# If we are close enough, we're good
|
|
if(gtm.x_throttle == 0 and gtm.y_throttle == 0 and gtm.z_throttle == 0):
|
|
last_delta = np.nan
|
|
return True
|
|
|
|
delta = (dx**2 + dy**2 + dz**2)**.5
|
|
if(delta > self.last_delta and self.last_delta < 2.0):
|
|
self.last_delta = np.nan
|
|
return True
|
|
|
|
self.last_delta = delta
|
|
|
|
print("Expected move (mm/call)", gtm.ctrl_step*feed*1000./60)
|
|
print("Delta (mm) ", (dx**2 + dy**2 + dz**2)**.5)
|
|
return False
|