def __check_delta(self, delta):
pos = self._position + delta
if not pos.is_in_aabb(
Coordinates(0.0, 0.0, 0.0, 0.0),
Coordinates(TABLE_SIZE_X_MM, TABLE_SIZE_Y_MM, TABLE_SIZE_Z_MM,
0)):
raise GMachineException("out of effective area")
def home(self):
""" Move head to park position
"""
d = Coordinates(0, 0, -self._position.z, 0)
self._move_linear(d, STEPPER_MAX_VELOCITY_MM_PER_MIN)
d = Coordinates(-self._position.x, -self._position.y, 0, 0)
self._move_linear(d, STEPPER_MAX_VELOCITY_MM_PER_MIN)
def _pos_check(self, target_pos, delta_mm, velocity_mm_per_min):
tolerance_digit = 1
dut = PathGenerator(delta_mm, target_pos, velocity_mm_per_min)
last_pos = Coordinates(0, 0, 0, 0)
for px, py, pz, pe in dut:
last_pos = Coordinates(px, py, pz, 0)
distance_error = last_pos - target_pos
self.assertAlmostEqual(distance_error.length(), 0.0, tolerance_digit)
def reset(self):
""" Reinitialize all program configurable thing.
"""
self._velocity = 1000
self._spindle_rpm = 1000
self._pause = 0
self._local = Coordinates(0.0, 0.0, 0.0, 0.0)
self._convertCoordinates = 1.0
self._absoluteCoordinates = True
self._plane = PLANE_XY
self._radius = Coordinates(0.0, 0.0, 0.0, 0.0)
def test_position_overshoot_x(self):
targets = [Coordinates(x, 0, 0, 0) for x in [60]]
too_fast_speed = MAX_VELOCITY_MM_PER_MIN_X * 1.1
velocities = [too_fast_speed]
for target_pos in targets:
delta_mm = target_pos
for velocity_mm_per_min in velocities:
self._overshoot_check(target_pos, delta_mm,
velocity_mm_per_min)
def radius(self, default, multiply):
""" Get radius for circular interpolation(I, J, K or R).
:param default: Default values, if any of coords is not specified.
:param multiply: If value exist, multiply it by this value.
:return: Coord object.
"""
i = self.get('I', default.x, multiply)
j = self.get('J', default.y, multiply)
k = self.get('K', default.z, multiply)
return Coordinates(i, j, k, 0)
def coordinates(self, default, multiply):
""" Get X, Y and Z values as Coord object.
:param default: Default values, if any of coordinates is not specified.
:param multiply: If value exist, multiply it by this value.
:return: Coord object.
"""
x = self.get('X', default.x, multiply)
y = self.get('Y', default.y, multiply)
z = self.get('Z', default.z, multiply)
e = self.get('E', default.e, multiply)
return Coordinates(x, y, z, e)
def test_end_position(self):
targets = [Coordinates(x, x, x, 0) for x in range(1, 101, 20)]
too_fast_speed = math.ceil(
max(MAX_VELOCITY_MM_PER_MIN_X, MAX_VELOCITY_MM_PER_MIN_Y,
MAX_VELOCITY_MM_PER_MIN_Z) * 1.1)
velocities = [1, too_fast_speed]
for target_pos in targets:
delta_mm = target_pos
for velocity_mm_per_min in velocities:
self._pos_check(target_pos, delta_mm, velocity_mm_per_min)
def _circular(self, delta, radius, velocity, direction):
delta = delta.round(1.0 / STEPPER_PULSES_PER_MM_X,
1.0 / STEPPER_PULSES_PER_MM_Y,
1.0 / STEPPER_PULSES_PER_MM_Z,
1.0 / STEPPER_PULSES_PER_MM_E)
self.__check_delta(delta)
# get delta vector and put it on circle
circle_end = Coordinates(0, 0, 0, 0)
if self._plane == PLANE_XY:
circle_end.x, circle_end.y = self.__adjust_circle(
delta.x, delta.y, radius.x, radius.y, direction,
self._position.x, self._position.y, TABLE_SIZE_X_MM,
TABLE_SIZE_Y_MM)
circle_end.z = delta.z
elif self._plane == PLANE_YZ:
circle_end.y, circle_end.z = self.__adjust_circle(
delta.y, delta.z, radius.y, radius.z, direction,
self._position.y, self._position.z, TABLE_SIZE_Y_MM,
TABLE_SIZE_Z_MM)
circle_end.x = delta.x
elif self._plane == PLANE_ZX:
circle_end.z, circle_end.x = self.__adjust_circle(
delta.z, delta.x, radius.z, radius.x, direction,
self._position.z, self._position.x, TABLE_SIZE_Z_MM,
TABLE_SIZE_X_MM)
circle_end.y = delta.y
circle_end.e = delta.e
circle_end = circle_end.round(1.0 / STEPPER_PULSES_PER_MM_X,
1.0 / STEPPER_PULSES_PER_MM_Y,
1.0 / STEPPER_PULSES_PER_MM_Z,
1.0 / STEPPER_PULSES_PER_MM_E)
hal.move_circular(circle_end, radius, self._plane, velocity, direction)
# if finish coords is not on circle, move some distance linearly
linear_delta = delta - circle_end
if not linear_delta.is_zero():
logging.info(
"Moving additionally {} to finish circle command".format(
linear_delta))
hal.move_linear(linear_delta, velocity)
# save position
self._position = self._position + circle_end + linear_delta
def _speed_check(self, velocity_mm_per_min, delta_mm):
#print("testing {:.2f} mm/min".format(velocity_mm_per_min))
new_pos = delta_mm
dut = PathGenerator(delta_mm, new_pos, velocity_mm_per_min)
old_pos = Coordinates(0, 0, 0, 0)
max_speed = 0
for px, py, pz, pe in dut:
p = Coordinates(px, py, pz, 0)
delta = p - old_pos
old_pos = p
v = delta.length() / REAL_TIME_DT
max_speed = max(max_speed, v)
self.assertNotEqual(
dut.linear_time_s, 0.0,
"Not enough space to reach the max speed ({:.2f}mm/min) given that acceleration"
.format(velocity_mm_per_min))
self.assertAlmostEqual(
max_speed, velocity_mm_per_min / SECONDS_IN_MINUTE, 1,
"Effective max speed not correct when trying {:f} mm/min".format(
velocity_mm_per_min))
def __init__(self):
""" Initialization.
"""
self._position = Coordinates(0.0, 0.0, 0.0, 0.0)
# init variables
self._velocity = 0
self._spindle_rpm = 0
self._pause = 0
self._local = None
self._convertCoordinates = 0
self._absoluteCoordinates = 0
self._plane = None
self._radius = None
self.reset()
hal.init()
def _overshoot_check(self, target_pos, delta_mm, velocity_mm_per_min):
tolerance_digit = 1
dut = PathGenerator(delta_mm, target_pos, velocity_mm_per_min)
max_pos = Coordinates(0, 0, 0, 0)
for px, py, pz, pe in dut:
p = Coordinates(px, py, pz, 0)
max_pos.x = max(max_pos.x, p.x)
max_pos.y = max(max_pos.y, p.y)
max_pos.z = max(max_pos.z, p.z)
distance_error = max_pos - target_pos
self.assertAlmostEqual(distance_error.length(), 0.0, tolerance_digit)
def do_command(self, gcode):
""" Perform action.
:param gcode: GCode object which represent one gcode line
"""
if gcode is None:
return
logging.debug("got command " + str(gcode.params))
# read command
c = gcode.command()
if c is None and gcode.has_coordinates():
c = 'G1'
# read parameters
if self._absoluteCoordinates:
coord = gcode.coordinates(self._position, self._convertCoordinates)
coord = coord + self._local
delta = coord - self._position
else:
delta = gcode.coordinates(Coordinates(0.0, 0.0, 0.0, 0.0),
self._convertCoordinates)
coord = self._position + delta
velocity = gcode.get('F', self._velocity)
spindle_rpm = gcode.get('S', self._spindle_rpm)
pause = gcode.get('P', self._pause)
radius = gcode.radius(self._radius, self._convertCoordinates)
# check parameters
if velocity <= 0 or velocity > STEPPER_MAX_VELOCITY_MM_PER_MIN:
raise GMachineException("bad feed speed")
if spindle_rpm < 0 or spindle_rpm > SPINDLE_MAX_RPM:
raise GMachineException("bad spindle speed")
if pause < 0:
raise GMachineException("bad delay")
# select command and run it
if c == 'G0': # rapid move
self._move_linear(delta, STEPPER_MAX_VELOCITY_MM_PER_MIN)
elif c == 'G1': # linear interpolation
self._move_linear(delta, velocity)
elif c == 'G2': # circular interpolation, clockwise
self._circular(delta, radius, velocity, CW)
elif c == 'G3': # circular interpolation, counterclockwise
self._circular(delta, radius, velocity, CCW)
elif c == 'G4': # delay in s
hal.join()
time.sleep(pause)
elif c == 'G17': # XY plane select
self._plane = PLANE_XY
elif c == 'G18': # ZX plane select
self._plane = PLANE_ZX
elif c == 'G19': # YZ plane select
self._plane = PLANE_YZ
elif c == 'G20': # switch to inches
self._convertCoordinates = 25.4
elif c == 'G21': # switch to mm
self._convertCoordinates = 1.0
elif c == 'G28': # home
self.home()
elif c == 'G53': # switch to machine coords
self._local = Coordinates(0.0, 0.0, 0.0, 0.0)
elif c == 'G90': # switch to absolute coords
self._absoluteCoordinates = True
elif c == 'G91': # switch to relative coords
self._absoluteCoordinates = False
elif c == 'G92': # switch to local coords
self._local = self._position - \
gcode.coordinates(Coordinates(0.0, 0.0, 0.0, 0.0),
self._convertCoordinates)
elif c == 'M3': # spinle on
self._spindle(spindle_rpm)
elif c == 'M5': # spindle off
self._spindle(0)
elif c == 'M2' or c == 'M30': # program finish, reset everything.
self.reset()
elif c == 'M111': # enable debug
logging.getLogger().setLevel(logging.DEBUG)
elif c is None: # command not specified(for example, just F was passed)
pass
else:
raise GMachineException("unknown command")
# save parameters on success
self._velocity = velocity
self._spindle_rpm = spindle_rpm
self._pause = pause
self._radius = radius
logging.debug("position {}".format(self._position))
def test_max_speed_x(self):
max_possible_speed = MAX_VELOCITY_MM_PER_MIN_X
delta_mm = Coordinates(60, 0, 0, 0)
for speed in (1, int(math.floor(max_possible_speed))):
self._speed_check(speed, delta_mm)