From 4093fe6661538699ede7a610a9d3ff7cc371994a Mon Sep 17 00:00:00 2001 From: Nikolay Khabarov <2xl@mail.ru> Date: Sat, 10 Jun 2017 23:27:45 +0300 Subject: [PATCH] circular interpolation --- README.md | 6 +- cnc/gmachine.py | 38 +++-- cnc/hal.py | 26 +-- cnc/hal_raspberry/hal.py | 32 +--- cnc/hal_virtual.py | 68 ++++---- cnc/pulses.py | 323 ++++++++++++++++++++++++++++++++++++-- tests/test_coordinates.py | 2 +- tests/test_gmachine.py | 24 ++- tests/test_pulses.py | 149 ++++++++++++++++-- 9 files changed, 544 insertions(+), 124 deletions(-) diff --git a/README.md b/README.md index e86c8a8..f9befe3 100644 --- a/README.md +++ b/README.md @@ -31,8 +31,10 @@ perfect choice for easy development of this project. Video demo - [YouTube video](https://youtu.be/vcedo59raS4) # Current command support -G0, G1, G4, G20, G21, G28, G53, G90, G91, G92, M2, M3, M5, M30 -Commands can be easily added, see [gmachine.py](./cnc/gmachine.py) file. +G0, G1, G2, G3, G4, G17, G18, G19, G20, G21, G28, G53, G90, G91, G92, M2, M3, +M5, M30 +Commands can be easily added, see [gmachine.py](./cnc/gmachine.py) file. +Four axis are supported - X, Y, Z, E # Config All configs are stored in [config.py](./cnc/config.py) and contain hardware diff --git a/cnc/gmachine.py b/cnc/gmachine.py index 3909061..cfdd651 100644 --- a/cnc/gmachine.py +++ b/cnc/gmachine.py @@ -7,6 +7,7 @@ from cnc import hal from cnc.coordinates import Coordinates from cnc.enums import * from cnc.config import * +from cnc.pulses import * class GMachineException(Exception): @@ -31,7 +32,6 @@ class GMachine(object): self._convertCoordinates = 0 self._absoluteCoordinates = 0 self._plane = None - self._radius = None self.reset() hal.init() @@ -52,7 +52,6 @@ class GMachine(object): self._convertCoordinates = 1.0 self._absoluteCoordinates = True self._plane = PLANE_XY - self._radius = Coordinates(0.0, 0.0, 0.0, 0.0) def _spindle(self, spindle_speed): hal.join() @@ -72,7 +71,10 @@ class GMachine(object): if delta.is_zero(): return self.__check_delta(delta) - hal.move_linear(delta, velocity) + + logging.info("Moving linearly {}".format(delta)) + gen = PulseGeneratorLinear(delta, velocity) + hal.move(gen) # save position self._position = self._position + delta @@ -90,16 +92,18 @@ class GMachine(object): r = math.sqrt(ra * ra + rb * rb) if r == 0: raise GMachineException("circle radius is zero") - l = math.sqrt(da * da + db * db) sq = self.__quarter(-ra, -rb) - if l == 0: # full circle + if da == 0 and db == 0: # full circle ea = da eb = db eq = 5 # mark as non-existing to check all else: - ea = da / l * r + ra - eb = db / l * r + rb - eq = self.__quarter(ea - ra, eb - rb) + b = (db - rb) / (da - ra) + ea = math.copysign(math.sqrt(r * r / (1.0 + abs(b))), da - ra) + eb = math.copysign(math.sqrt(r * r - ea * ea), db - rb) + eq = self.__quarter(ea, eb) + ea += ra + eb += rb # iterate coordinates quarters and check if we fit table q = sq pq = q @@ -133,6 +137,10 @@ class GMachine(object): 1.0 / STEPPER_PULSES_PER_MM_Y, 1.0 / STEPPER_PULSES_PER_MM_Z, 1.0 / STEPPER_PULSES_PER_MM_E) + radius = radius.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) @@ -159,13 +167,19 @@ class GMachine(object): 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) + logging.info("Moving circularly {} {} {} with radius {}" + " and velocity {}". + format(self._plane, circle_end, direction, radius, velocity)) + gen = PulseGeneratorCircular(circle_end, radius, self._plane, direction, + velocity) + hal.move(gen) # 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) + gen = PulseGeneratorLinear(linear_delta, velocity) + hal.move(gen) # save position self._position = self._position + circle_end + linear_delta @@ -217,7 +231,8 @@ class GMachine(object): 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) + radius = gcode.radius(Coordinates(0.0, 0.0, 0.0, 0.0), + self._convertCoordinates) # check parameters if velocity <= 0 or velocity > STEPPER_MAX_VELOCITY_MM_PER_MIN: raise GMachineException("bad feed speed") @@ -275,5 +290,4 @@ class GMachine(object): self._velocity = velocity self._spindle_rpm = spindle_rpm self._pause = pause - self._radius = radius logging.debug("position {}".format(self._position)) diff --git a/cnc/hal.py b/cnc/hal.py index 390edd4..6463bc3 100644 --- a/cnc/hal.py +++ b/cnc/hal.py @@ -17,22 +17,9 @@ # do_something() # # -# def move_linear(delta, velocity): -# """ Move head to specified distance with specified speed. -# :param delta: Coordinated object, delta position in mm -# :param velocity: velocity in mm per min -# """ -# do_something() -# -# -# def move_circular(delta, radius, plane, velocity, direction): -# """ Move with circular interpolation. -# :param delta: finish position delta from the beginning, must be on -# circle on specified plane. Zero means full circle. -# :param radius: vector to center of circle. -# :param plane: plane to interpolate. -# :param velocity: velocity in mm per min. -# :param direction: clockwise or counterclockwise. +# def move(generator): +# """ Move head to according pulses in PulseGenerator. +# :param generator: PulseGenerator object # """ # do_something() # @@ -62,12 +49,9 @@ if 'init' not in locals(): raise NotImplementedError("hal.init() not implemented") if 'spindle_control' not in locals(): raise NotImplementedError("hal.spindle_control() not implemented") -if 'move_linear' not in locals(): - raise NotImplementedError("hal.move_linear() not implemented") -if 'move_circular' not in locals(): - raise NotImplementedError("hal.move_circular() not implemented") +if 'move' not in locals(): + raise NotImplementedError("hal.move() not implemented") if 'join' not in locals(): raise NotImplementedError("hal.join() not implemented") if 'deinit' not in locals(): raise NotImplementedError("hal.deinit() not implemented") - diff --git a/cnc/hal_raspberry/hal.py b/cnc/hal_raspberry/hal.py index 8bdc40f..de71c7f 100644 --- a/cnc/hal_raspberry/hal.py +++ b/cnc/hal_raspberry/hal.py @@ -108,14 +108,10 @@ def spindle_control(percent): pwm.remove_pin(SPINDLE_PWM_PIN) -def move_linear(delta, velocity): +def move(generator): """ Move head to specified position - :param delta: coordinated object, delta position in mm - :param velocity: velocity in mm per min + :param generator: PulseGenerator object. """ - logging.info("move {} with velocity {}".format(delta, velocity)) - # initialize generator - generator = PulseGeneratorLinear(delta, velocity) # wait if previous command still works while dma.is_active(): time.sleep(0.001) @@ -124,6 +120,7 @@ def move_linear(delta, velocity): dma.clear() prev = 0 is_ran = False + instant = INSTANT_RUN st = time.time() for dir, tx, ty, tz, te in generator: if dir: # set up directions @@ -166,14 +163,15 @@ def move_linear(delta, velocity): # matter for pulses with 1-2us length. prev = k + STEPPER_PULSE_LINGTH_US # instant run handling - if not is_ran and INSTANT_RUN: + if not is_ran and instant: if k > 500000: # wait at least 500 ms is uploaded if time.time() - st > 0.5: - # may be instant run should be canceled here? logging.warn("Buffer preparing for instant run took more " "time then buffer time") - dma.run_stream() - is_ran = True + instant = False + else: + dma.run_stream() + is_ran = True pt = time.time() if not is_ran: dma.run(False) @@ -184,20 +182,6 @@ def move_linear(delta, velocity): + str(round(generator.total_time_s(), 2)) + "s") -def move_circular(delta, radius, plane, velocity, direction): - """ Move with circular interpolation. - :param delta: finish position delta from the beginning, must be on - circle on specified plane. Zero means full circle. - :param radius: vector to center of circle. - :param plane: plane to interpolate. - :param velocity: velocity in mm per min. - :param direction: clockwise or counterclockwise. - """ - logging.info("TODO move_circular {} {} {} with radius {} and velocity {}". - format(plane, delta, direction, radius, velocity)) - # TODO - - def join(): """ Wait till motors work. """ diff --git a/cnc/hal_virtual.py b/cnc/hal_virtual.py index d98a668..47d6781 100644 --- a/cnc/hal_virtual.py +++ b/cnc/hal_virtual.py @@ -2,7 +2,7 @@ from __future__ import division import logging import time -from cnc.pulses import PulseGeneratorLinear +from cnc.pulses import PulseGeneratorLinear, PulseGeneratorCircular from cnc.config import * from cnc.coordinates import Coordinates @@ -25,36 +25,39 @@ def spindle_control(percent): logging.info("spindle control: {}%".format(percent)) -def move_linear(delta, velocity): - """ Move head to specified position - :param delta: Coordinated object, delta position in mm - :param velocity: velocity in mm per min +def move(generator): + """ Move head to specified position. + :param generator: PulseGenerator object. """ - logging.info("move {} with velocity {}".format(delta, velocity)) + delta = generator.delta() ix = iy = iz = ie = 0 - generator = PulseGeneratorLinear(delta, velocity) lx, ly, lz, le = None, None, None, None dx, dy, dz, de = 0, 0, 0, 0 mx, my, mz, me = 0, 0, 0, 0 + cx, cy, cz, ce = 0, 0, 0, 0 + dirx, diry, dirz, dire = 1, 1, 1, 1 st = time.time() direction_found = False for dir, tx, ty, tz, te in generator: if dir: direction_found = True - assert (tx < 0 and delta.x < 0) or (tx > 0 and delta.x > 0) \ - or delta.x == 0 - assert (ty < 0 and delta.y < 0) or (ty > 0 and delta.y > 0) \ - or delta.y == 0 - assert (tz < 0 and delta.z < 0) or (tz > 0 and delta.z > 0) \ - or delta.z == 0 - assert (te < 0 and delta.e < 0) or (te > 0 and delta.e > 0) \ - or delta.e == 0 + dirx, diry, dirz, dire = tx, ty, tz, te + if isinstance(generator, PulseGeneratorLinear): + assert (tx < 0 and delta.x < 0) or (tx > 0 and delta.x > 0) \ + or delta.x == 0 + assert (ty < 0 and delta.y < 0) or (ty > 0 and delta.y > 0) \ + or delta.y == 0 + assert (tz < 0 and delta.z < 0) or (tz > 0 and delta.z > 0) \ + or delta.z == 0 + assert (te < 0 and delta.e < 0) or (te > 0 and delta.e > 0) \ + or delta.e == 0 continue if tx is not None: if tx > mx: mx = tx tx = int(round(tx * 1000000)) - ix += 1 + ix += dirx + cx += 1 if lx is not None: dx = tx - lx assert dx > 0, "negative or zero time delta detected for x" @@ -65,7 +68,8 @@ def move_linear(delta, velocity): if ty > my: my = ty ty = int(round(ty * 1000000)) - iy += 1 + iy += diry + cy += 1 if ly is not None: dy = ty - ly assert dy > 0, "negative or zero time delta detected for y" @@ -76,7 +80,8 @@ def move_linear(delta, velocity): if tz > mz: mz = tz tz = int(round(tz * 1000000)) - iz += 1 + iz += dirz + cz += 1 if lz is not None: dz = tz - lz assert dz > 0, "negative or zero time delta detected for z" @@ -87,7 +92,8 @@ def move_linear(delta, velocity): if te > me: me = te te = int(round(te * 1000000)) - ie += 1 + ie += dire + ce += 1 if le is not None: de = te - le assert de > 0, "negative or zero time delta detected for e" @@ -100,30 +106,16 @@ def move_linear(delta, velocity): assert f.count(f[0]) == len(f), "fast forwarded pulse detected" pt = time.time() assert direction_found, "direction not found" - assert ix / STEPPER_PULSES_PER_MM_X == abs(delta.x), "x wrong number of pulses" - assert iy / STEPPER_PULSES_PER_MM_Y == abs(delta.y), "y wrong number of pulses" - assert iz / STEPPER_PULSES_PER_MM_Z == abs(delta.z), "z wrong number of pulses" - assert ie / STEPPER_PULSES_PER_MM_E == abs(delta.e), "e wrong number of pulses" + assert ix / STEPPER_PULSES_PER_MM_X == delta.x, "x wrong number of pulses" + assert iy / STEPPER_PULSES_PER_MM_Y == delta.y, "y wrong number of pulses" + assert iz / STEPPER_PULSES_PER_MM_Z == delta.z, "z wrong number of pulses" + assert ie / STEPPER_PULSES_PER_MM_E == delta.e, "e wrong number of pulses" assert max(mx, my, mz, me) <= generator.total_time_s(), "interpolation time or pulses wrong" - logging.debug("Did {}, {}, {}, {} iterations".format(ix, iy, iz, ie)) + logging.debug("Moved {}, {}, {}, {} iterations".format(ix, iy, iz, ie)) logging.info("prepared in " + str(round(pt - st, 2)) \ + "s, estimated " + str(round(generator.total_time_s(), 2)) + "s") -def move_circular(delta, radius, plane, velocity, direction): - """ Move with circular interpolation. - :param delta: finish position delta from the beginning, must be on - circle on specified plane. Zero means full circle. - :param radius: vector to center of circle. - :param plane: plane to interpolate. - :param velocity: velocity in mm per min. - :param direction: clockwise or counterclockwise. - """ - logging.info("TODO move_circular {} {} {} with radius {} and velocity {}". - format(plane, delta, direction, radius, velocity)) - # TODO - - def join(): """ Wait till motors work. """ diff --git a/cnc/pulses.py b/cnc/pulses.py index eff28fb..f6d1d8a 100644 --- a/cnc/pulses.py +++ b/cnc/pulses.py @@ -3,9 +3,10 @@ import math import logging from cnc.config import * +from cnc.enums import * from cnc.coordinates import Coordinates -SECONDS_IN_MINUTE = 60 +SECONDS_IN_MINUTE = 60.0 class PulseGenerator(object): @@ -33,10 +34,11 @@ class PulseGenerator(object): It's not implemented yet. """ - def __init__(self): + def __init__(self, delta): """ Create object. Do not create directly this object, inherit this class and implement interpolation function and related methods. All child have to call this method ( super().__init__() ). + :param delta: overall movement delta in mm, uses for debug purpose. """ self._iteration_x = 0 self._iteration_y = 0 @@ -46,20 +48,21 @@ class PulseGenerator(object): self._acceleration_time_s = 0.0 self._linear_time_s = 0.0 self._2Vmax_per_a = 0.0 + self._delta = delta def _get_movement_parameters(self): - """ Get for interpolation. This method have to be reimplemented - in parent classes and should calculate 3 parameters. + """ Get parameters for interpolation. This method have to be + reimplemented in parent classes and should calculate 3 parameters. :return: Tuple of three values: acceleration_time_s: time for accelerating and breaking motors during movement linear_time_s: time for uniform movement, it is total movement time minus acceleration and braking time - max_axis_velocity_mm_per_sec: maximum velocity of any of axis - during movement. Even if whole - movement is accelerated, this - value should be calculated as top - velocity. + max_axis_velocity_mm_per_sec: maximum axis velocity of all + axises during movement. Even if + whole movement is accelerated, + this value should be calculated + as top velocity. """ raise NotImplemented @@ -121,9 +124,12 @@ class PulseGenerator(object): # Vmax * Tpseudo = Vlinear * t - a * t^2 / 2 # V on start braking is Vlinear = Taccel * a = Tbreaking * a # Vmax * Tpseudo = Tbreaking * a * t - a * t^2 / 2 - return 2.0 * self._acceleration_time_s + self._linear_time_s \ - - math.sqrt(self._acceleration_time_s ** 2 - - self._2Vmax_per_a * bt) + d = self._acceleration_time_s ** 2 - self._2Vmax_per_a * bt + if d > 0: + d = math.sqrt(d) + else: + d = 0 + return 2.0 * self._acceleration_time_s + self._linear_time_s - d def __next__(self): # for python3 @@ -193,6 +199,12 @@ class PulseGenerator(object): acceleration_time_s, linear_time_s, _ = self._get_movement_parameters() return acceleration_time_s * 2.0 + linear_time_s + def delta(self): + """ Get overall movement distance. + :return: Movement distance for each axis in millimeters. + """ + return self._delta + class PulseGeneratorLinear(PulseGenerator): def __init__(self, delta_mm, velocity_mm_per_min): @@ -200,7 +212,7 @@ class PulseGeneratorLinear(PulseGenerator): :param delta_mm: movement distance of each axis. :param velocity_mm_per_min: desired velocity. """ - super(PulseGeneratorLinear, self).__init__() + super(PulseGeneratorLinear, self).__init__(delta_mm) # this class doesn't care about direction self._distance_mm = abs(delta_mm) # velocity of each axis @@ -231,7 +243,7 @@ class PulseGeneratorLinear(PulseGenerator): self._direction = math.copysign(1, delta_mm.x), \ math.copysign(1, delta_mm.y), \ math.copysign(1, delta_mm.z), \ - math.copysign(1, delta_mm.e) \ + math.copysign(1, delta_mm.e) def _get_movement_parameters(self): """ Return movement parameters, see super class for details. @@ -262,3 +274,286 @@ class PulseGeneratorLinear(PulseGenerator): t_e = self.__linear(ie / STEPPER_PULSES_PER_MM_E, self._distance_mm.e, self.max_velocity_mm_per_sec.e) return self._direction, (t_x, t_y, t_z, t_e) + + +class PulseGeneratorCircular(PulseGenerator): + def __init__(self, delta, radius, plane, direction, velocity): + """ Create pulse generator for circular interpolation. + Position calculates based on formulas: + R^2 = x^2 + y^2 + x = R * sin(phi) + y = R * cos(phi) + phi = omega * t, 2 * pi / omega = 2 * pi * R / V + phi = V * t / R + omega is angular_velocity. + so t = V / R * phi + phi can be calculated based on steps position. + Each axis can calculate circle phi base on iteration number, the + only one difference, that there is four quarters of circle and + signs for movement and solving expressions are different. So + we use additional variables to control it. + :param delta: finish position delta from the beginning, must be on + circle on specified plane. Zero means full circle. + :param radius: vector to center of circle. + :param plane: plane to interpolate. + :param direction: clockwise or counterclockwise. + :param velocity: velocity in mm per min. + """ + super(PulseGeneratorCircular, self).__init__(delta) + self._plane = plane + self._direction = direction + velocity = velocity / SECONDS_IN_MINUTE + # Get circle start point and end point. + if self._plane == PLANE_XY: + sa = -radius.x + sb = -radius.y + ea = sa + delta.x + eb = sb + delta.y + apm = STEPPER_PULSES_PER_MM_X + bpm = STEPPER_PULSES_PER_MM_Y + elif self._plane == PLANE_YZ: + sa = -radius.y + sb = -radius.z + ea = sa + delta.y + eb = sb + delta.z + apm = STEPPER_PULSES_PER_MM_Y + bpm = STEPPER_PULSES_PER_MM_Z + elif self._plane == PLANE_ZX: + sa = -radius.z + sb = -radius.x + ea = sa + delta.z + eb = sb + delta.x + apm = STEPPER_PULSES_PER_MM_Z + bpm = STEPPER_PULSES_PER_MM_X + # adjust radius to fit into axises step. + self._radius = round(math.sqrt(sa * sa + sb * sb) * min(apm, bpm)) \ + / min(apm, bpm) + self._radius2 = self._radius * self._radius + self._start_a = sa + self._start_b = sb + assert round(math.sqrt(ea * ea + eb * eb) * min(apm, bpm)) \ + / min(apm, bpm) == self._radius, "Wrong end point" + + # Calculate angles and directions. + start_angle = self.__angle(sa, sb) + end_angle = self.__angle(ea, eb) + delta_angle = end_angle - start_angle + if delta_angle < 0 or (delta_angle == 0 and direction == CW): + delta_angle += 2 * math.pi + if direction == CCW: + delta_angle -= 2 * math.pi + if direction == CW: + if start_angle >= math.pi: + self._dir_b = 1 + else: + self._dir_b = -1 + if math.pi / 2 <= start_angle < 3 * math.pi / 2: + self._dir_a = -1 + else: + self._dir_a = 1 + elif direction == CCW: + if 0 < start_angle <= math.pi: + self._dir_b = 1 + else: + self._dir_b = -1 + if start_angle <= math.pi / 2 or start_angle > 3 * math.pi / 2: + self._dir_a = -1 + else: + self._dir_a = 1 + self._side_a = self._start_b < 0 or (self._start_b == 0 and self._dir_b < 0) + self._side_b = self._start_a < 0 or (self._start_a == 0 and self._dir_a < 0) + self._start_angle = start_angle + logging.debug("start angle {}, end angle {}, delta {}".format( + start_angle * 180.0 / math.pi, + end_angle * 180.0 / math.pi, + delta_angle * 180.0 / math.pi)) + delta_angle = abs(delta_angle) + self._delta_angle = delta_angle + + # calculate values for interpolation. + + # calculate travel distance for axis in circular move. + self._iterations_a = 0 + self._iterations_b = 0 + end_angle_m = end_angle + if start_angle >= end_angle: + end_angle_m += 2 * math.pi + rstart = int(start_angle / (math.pi / 2.0)) + rend = int(end_angle_m / (math.pi / 2.0)) + if rend - rstart >= 4: + self._iterations_a = 4 * int(self._radius * apm) + self._iterations_b = 4 * int(self._radius * apm) + else: + if rstart == rend: + self._iterations_a = int(abs(sa - ea) * apm) + self._iterations_b = int(abs(sb - eb) * bpm) + else: + for r in range(rstart, rend + 1): + i = r + if i >= 4: + i -= 4 + if r == rstart: + if i == 0 or i == 2: + self._iterations_a += int(self._radius * apm) - int(abs(sa) * apm) + else: + self._iterations_a += int(abs(sa) * apm) + if i == 1 or i == 3: + self._iterations_b += int(self._radius * bpm) - int(abs(sb) * bpm) + else: + self._iterations_b += int(abs(sb) * bpm) + elif r == rend: + if i == 0 or i == 2: + self._iterations_a += int(abs(ea) * apm) + else: + self._iterations_a += int(self._radius * apm) - int(abs(ea) * apm) + if i == 1 or i == 3: + self._iterations_b += int(abs(eb) * bpm) + else: + self._iterations_b += int(self._radius * bpm) - int(abs(eb) * bpm) + else: + self._iterations_a += int(self._radius * apm) + self._iterations_b += int(self._radius * bpm) + if direction == CCW: + self._iterations_a = 4 * int(self._radius * apm) - self._iterations_a + self._iterations_b = 4 * int(self._radius * bpm) - self._iterations_b + + arc = delta_angle * self._radius + e2 = delta.e * delta.e + if self._plane == PLANE_XY: + self._iterations_3rd = abs(delta.z) * STEPPER_PULSES_PER_MM_Z + l = math.sqrt(arc * arc + delta.z * delta.z + e2) + self._velocity_3rd = abs(delta.z) / l * velocity + self._third_dir = math.copysign(1, delta.z) + elif self._plane == PLANE_YZ: + self._iterations_3rd = abs(delta.x) * STEPPER_PULSES_PER_MM_X + l = math.sqrt(arc * arc + delta.x * delta.x + e2) + self._velocity_3rd = abs(delta.x) / l * velocity + self._third_dir = math.copysign(1, delta.x) + elif self._plane == PLANE_ZX: + self._iterations_3rd = abs(delta.y) * STEPPER_PULSES_PER_MM_Y + l = math.sqrt(arc * arc + delta.y * delta.y + e2) + self._velocity_3rd = abs(delta.y) / l * velocity + self._third_dir = math.copysign(1, delta.y) + self._iterations_e = abs(delta.e) * STEPPER_PULSES_PER_MM_E + # Velocity splits with corresponding distance. + cV = arc / l * velocity + self._RdivV = self._radius / cV + self._e_velocity = abs(delta.e) / l * velocity + self._e_dir = math.copysign(1, delta.e) + self.max_velocity_mm_per_sec = max(cV, self._velocity_3rd, + self._e_velocity) + self.acceleration_time_s = self.max_velocity_mm_per_sec \ + / STEPPER_MAX_ACCELERATION_MM_PER_S2 + if STEPPER_MAX_ACCELERATION_MM_PER_S2 * self.acceleration_time_s ** 2 \ + > l: + self.acceleration_time_s = math.sqrt(l / + STEPPER_MAX_ACCELERATION_MM_PER_S2) + self.linear_time_s = 0.0 + self.max_velocity_mm_per_sec = l / self.acceleration_time_s + else: + linear_distance_mm = l - self.acceleration_time_s ** 2 \ + * STEPPER_MAX_ACCELERATION_MM_PER_S2 + self.linear_time_s = linear_distance_mm / velocity + + def __angle(self, a, b): + # Calculate angle of entry point (a, b) of circle with center in (0,0) + angle = math.acos(b / math.sqrt(a * a + b * b)) + if a < 0: + return 2 * math.pi - angle + return angle + + def _get_movement_parameters(self): + """ Return movement parameters, see super class for details. + """ + return self.acceleration_time_s, \ + self.linear_time_s, \ + self.max_velocity_mm_per_sec + + def __circularHelper(self, start, i, pulses_per_mm, side, dir): + np = start + dir * i / pulses_per_mm + np = round(np, 10) + if np > self._radius: + np -= 2 * (np - self._radius) + np = round(np, 10) + dir = -dir + side = not side + if np < -self._radius: + np -= 2 * (np + self._radius) + np = round(np, 10) + dir = -dir + side = not side + if np > self._radius: + np -= 2 * (np - self._radius) + np = round(np, 10) + dir = -dir + side = not side + return np, dir, side + + def __circularFindTime(self, a, b): + angle = self.__angle(a, b) + if self._direction == CW: + delta_angle = angle - self._start_angle + else: + delta_angle = self._start_angle - angle + if delta_angle <= 0: + delta_angle += 2 * math.pi + return self._RdivV * delta_angle + + def __circularA(self, i, pulses_per_mm): + if i >= self._iterations_a: + return self._dir_a, None + a, dir, side = self.__circularHelper(self._start_a, i + 1, + pulses_per_mm, self._side_a, + self._dir_a) + # last item can be slightly more then end angle due to float precision + if i + 1 == self._iterations_a: + return dir, self._RdivV * self._delta_angle + b = math.sqrt(self._radius2 - a * a) + if side: + b = -b + return dir, self.__circularFindTime(a, b) + + def __circularB(self, i, pulses_per_mm): + if i >= self._iterations_b: + return self._dir_b, None + b, dir, side = self.__circularHelper(self._start_b, i + 1, + pulses_per_mm, self._side_b, + self._dir_b) + # last item can be slightly more then end angle due to float precision + if i + 1 == self._iterations_b: + return dir, self._RdivV * self._delta_angle + a = math.sqrt(self._radius2 - b * b) + if side: + a = -a + return dir, self.__circularFindTime(a, b) + + def __linear(self, i, total_i, pulses_per_mm, velocity): + if i >= total_i: + return None + return i / pulses_per_mm / velocity + + def _interpolation_function(self, ix, iy, iz, ie): + """ Calculate interpolation values for linear movement, see super class + for details. + """ + if self._plane == PLANE_XY: + dx, tx = self.__circularA(ix, STEPPER_PULSES_PER_MM_X) + dy, ty = self.__circularB(iy, STEPPER_PULSES_PER_MM_Y) + tz = self.__linear(iz, self._iterations_3rd, STEPPER_PULSES_PER_MM_Z, + self._velocity_3rd) + dz = self._third_dir + elif self._plane == PLANE_YZ: + dy, ty = self.__circularA(iy, STEPPER_PULSES_PER_MM_Y) + dz, tz = self.__circularB(iz, STEPPER_PULSES_PER_MM_Z) + tx = self.__linear(ix, self._iterations_3rd, STEPPER_PULSES_PER_MM_X, + self._velocity_3rd) + dx = self._third_dir + elif self._plane == PLANE_ZX: + dz, tz = self.__circularA(iz, STEPPER_PULSES_PER_MM_Z) + dx, tx = self.__circularB(ix, STEPPER_PULSES_PER_MM_X) + ty = self.__linear(iy, self._iterations_3rd, STEPPER_PULSES_PER_MM_Y, + self._velocity_3rd) + dy = self._third_dir + te = self.__linear(ie, self._iterations_e, STEPPER_PULSES_PER_MM_E, + self._e_velocity) + return (dx, dy, dz, self._e_dir), (tx, ty, tz, te) diff --git a/tests/test_coordinates.py b/tests/test_coordinates.py index 78537c6..940a779 100644 --- a/tests/test_coordinates.py +++ b/tests/test_coordinates.py @@ -139,4 +139,4 @@ class TestCoordinates(unittest.TestCase): if __name__ == '__main__': - unittest.main() \ No newline at end of file + unittest.main() diff --git a/tests/test_gmachine.py b/tests/test_gmachine.py index 80732bd..b31d33c 100644 --- a/tests/test_gmachine.py +++ b/tests/test_gmachine.py @@ -91,14 +91,34 @@ class TestGMachine(unittest.TestCase): m.do_command, GCode.parse_line("G2X4Y4I2J2")) self.assertRaises(GMachineException, m.do_command, GCode.parse_line("G3X4Y4I2J2")) + m.do_command(GCode.parse_line("G17")) m.do_command(GCode.parse_line("G1X1")) m.do_command(GCode.parse_line("G2J1")) m.do_command(GCode.parse_line("G3J1")) self.assertEqual(m.position(), Coordinates(1, 0, 0, 0)) - m.do_command(GCode.parse_line("G1X5Y5")) + m.do_command(GCode.parse_line("G1X10Y10")) + m.do_command(GCode.parse_line("G2X9I1")) + self.assertEqual(m.position(), Coordinates(9, 10, 0, 0)) + m.do_command(GCode.parse_line("G19")) + m.do_command(GCode.parse_line("G1X10Y10Z10")) + m.do_command(GCode.parse_line("G3Y8K1")) + self.assertEqual(m.position(), Coordinates(10, 8, 10, 0)) + m.do_command(GCode.parse_line("G17")) + m.do_command(GCode.parse_line("G1X5Y5Z0")) m.do_command(GCode.parse_line("G2X0Y0Z5I-2J-2")) self.assertEqual(m.position(), Coordinates(0, 0, 5, 0)) - + m.do_command(GCode.parse_line("G17")) + m.do_command(GCode.parse_line("G1X90Y90")) + m.do_command(GCode.parse_line("G2X90Y70I-5J-5")) + self.assertEqual(m.position(), Coordinates(90, 70, 5, 0)) + m.do_command(GCode.parse_line("G18")) + m.do_command(GCode.parse_line("G1X90Y90Z20E0")) + m.do_command(GCode.parse_line("G2Z20X70I-5K-5E22")) + self.assertEqual(m.position(), Coordinates(70, 90, 20, 22)) + m.do_command(GCode.parse_line("G19")) + m.do_command(GCode.parse_line("G1X90Y90Z20")) + m.do_command(GCode.parse_line("G2Y90Z0J-5K-5E27")) + self.assertEqual(m.position(), Coordinates(90, 90, 0, 27)) def test_g4(self): m = GMachine() diff --git a/tests/test_pulses.py b/tests/test_pulses.py index 1a81b54..99b2653 100644 --- a/tests/test_pulses.py +++ b/tests/test_pulses.py @@ -19,8 +19,17 @@ class TestPulses(unittest.TestCase): self.assertRaises(ZeroDivisionError, PulseGeneratorLinear, Coordinates(0, 0, 0, 0), self.v) + self.assertRaises(ZeroDivisionError, PulseGeneratorCircular, + Coordinates(0, 0, 0, 0), Coordinates(0, 0, 9, 9), + PLANE_XY, CW, self.v) + self.assertRaises(ZeroDivisionError, PulseGeneratorCircular, + Coordinates(0, 0, 0, 0), Coordinates(9, 0, 0, 9), + PLANE_YZ, CW, self.v) + self.assertRaises(ZeroDivisionError, PulseGeneratorCircular, + Coordinates(0, 0, 0, 0), Coordinates(0, 9, 0, 9), + PLANE_ZX, CW, self.v) - def test_step(self): + def test_step_linear(self): # Check if PulseGenerator returns correctly single step movement. g = PulseGeneratorLinear(Coordinates(1.0 / STEPPER_PULSES_PER_MM_X, 0, 0, 0), @@ -52,19 +61,121 @@ class TestPulses(unittest.TestCase): self.assertEqual(pe, 0) self.assertEqual(i, 1) - def test_linear_with_hal_virtual(self): + + def __check_circular(self, delta, radius, plane, direction = CW): + g = PulseGeneratorCircular(delta, radius, plane, direction, self.v) + x, y, z, e = 0, 0, 0, 0 + dx, dy, dz, de = None, None, None, None + dir_changed = 0 + dir_requested = False + t = -1 + for dir, px, py, pz, pe in g: + if dir: + dx, dy, dz, de = px, py, pz, pe + dir_requested = True + continue + if dir_requested: # ignore last change + dir_requested = False + dir_changed += 1 + if px is not None: + x += dx + if py is not None: + y += dy + if pz is not None: + z += dz + if pe is not None: + e += de + v = list(i for i in (px, py, pz, pe) if i is not None) + self.assertEqual(min(v), max(v)) + self.assertLessEqual(t, min(v)) + t = max(v) + return dir_changed, Coordinates(x / STEPPER_PULSES_PER_MM_X, + y / STEPPER_PULSES_PER_MM_Y, + z / STEPPER_PULSES_PER_MM_Z, + e / STEPPER_PULSES_PER_MM_E) + + def test_single_radius_circles(self): + # Check if PulseGenerator returns correctly single radius movement in + # both direction. + _, pos = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(1.0 / STEPPER_PULSES_PER_MM_X, 0, 0, 0), + PLANE_XY, CW) + self.assertEqual(pos, Coordinates(0, 0, 0, 0)) + _, pos = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(-1.0 / STEPPER_PULSES_PER_MM_X, 0, 0, 0), + PLANE_XY, CW) + self.assertEqual(pos, Coordinates(0, 0, 0, 0)) + _, pos = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(0, 1.0 / STEPPER_PULSES_PER_MM_Y, 0, 0), + PLANE_YZ, CW) + self.assertEqual(pos, Coordinates(0, 0, 0, 0)) + _, pos = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(0, -1.0 / STEPPER_PULSES_PER_MM_Y, 0, 0), + PLANE_YZ, CW) + self.assertEqual(pos, Coordinates(0, 0, 0, 0)) + _, pos = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(0, 0, 1.0 / STEPPER_PULSES_PER_MM_Z, 0), + PLANE_ZX, CW) + self.assertEqual(pos, Coordinates(0, 0, 0, 0)) + _, pos = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(0, 0, -1.0 / STEPPER_PULSES_PER_MM_Z, 0), + PLANE_ZX, CW) + self.assertEqual(pos, Coordinates(0, 0, 0, 0)) + _, pos = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(1.0 / STEPPER_PULSES_PER_MM_X, 0, 0, 0), + PLANE_XY, CCW) + self.assertEqual(pos, Coordinates(0, 0, 0, 0)) + _, pos = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(-1.0 / STEPPER_PULSES_PER_MM_X, 0, 0, 0), + PLANE_XY, CCW) + self.assertEqual(pos, Coordinates(0, 0, 0, 0)) + _, pos = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(0, 1.0 / STEPPER_PULSES_PER_MM_Y, 0, 0), + PLANE_YZ, CCW) + self.assertEqual(pos, Coordinates(0, 0, 0, 0)) + _, pos = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(0, -1.0 / STEPPER_PULSES_PER_MM_Y, 0, 0), + PLANE_YZ, CCW) + self.assertEqual(pos, Coordinates(0, 0, 0, 0)) + _, pos = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(0, 0, 1.0 / STEPPER_PULSES_PER_MM_Z, 0), + PLANE_ZX, CCW) + self.assertEqual(pos, Coordinates(0, 0, 0, 0)) + _, pos = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(0, 0, -1.0 / STEPPER_PULSES_PER_MM_Z, 0), + PLANE_ZX, CCW) + self.assertEqual(pos, Coordinates(0, 0, 0, 0)) + + def test_with_hal_virtual(self): # Using hal_virtual module for this test, it already contains plenty # of asserts for wrong number of pulses, pulse timing issues etc - hal_virtual.move_linear(Coordinates(1, 0, 0, 0), self.v) - hal_virtual.move_linear(Coordinates(25.4, 0, 0, 0), self.v) - hal_virtual.move_linear(Coordinates(25.4, 0, 0, 0), self.v) - hal_virtual.move_linear(Coordinates(25.4, 0, 0, 0), self.v) - hal_virtual.move_linear(Coordinates(TABLE_SIZE_X_MM, + hal_virtual.move(PulseGeneratorLinear(Coordinates(1, 0, 0, 0), + self.v)) + hal_virtual.move(PulseGeneratorLinear(Coordinates(25.4, 0, 0, 0), + self.v)) + hal_virtual.move(PulseGeneratorLinear(Coordinates(25.4, 0, 0, 0), + self.v)) + hal_virtual.move(PulseGeneratorLinear(Coordinates(25.4, 0, 0, 0), + self.v)) + hal_virtual.move(PulseGeneratorLinear(Coordinates(TABLE_SIZE_X_MM, TABLE_SIZE_Y_MM, TABLE_SIZE_Z_MM, - 100.0), self.v) + 100.0), self.v)) + hal_virtual.move(PulseGeneratorCircular(Coordinates(0, 20, 0, 0), + Coordinates(-10, 10, 0, 0), + PLANE_XY, CW, self.v)) + hal_virtual.move(PulseGeneratorCircular(Coordinates(-4, -4, 0, 0), + Coordinates(-2, -2, 0, 0), + PLANE_XY, CW, self.v)) + hal_virtual.move(PulseGeneratorCircular(Coordinates(- 2.0 / STEPPER_PULSES_PER_MM_X, + - 2.0 / STEPPER_PULSES_PER_MM_Y, + 0, 0), + Coordinates(- 1.0 / STEPPER_PULSES_PER_MM_X, + - 1.0 / STEPPER_PULSES_PER_MM_Y, + 0, 0), + PLANE_XY, CW, self.v)) - def test_twice_faster(self): + def test_twice_faster_linear(self): # Checks if one axis moves exactly twice faster, pulses are correct. m = Coordinates(2, 4, 0, 0) g = PulseGeneratorLinear(m, self.v) @@ -112,10 +223,23 @@ class TestPulses(unittest.TestCase): self.assertEqual(m.z * STEPPER_PULSES_PER_MM_Z, iz) self.assertEqual(m.e * STEPPER_PULSES_PER_MM_E, ie) self.assertLessEqual(t, g.total_time_s()) + _, pos = self.__check_circular(Coordinates(0, 8, 0, 7), + Coordinates(1, 0, 1, 0), + PLANE_ZX, CCW) + self.assertEqual(pos, Coordinates(0, 8, 0, 7)) + _, pos = self.__check_circular(Coordinates(5, 0, 0, 6), + Coordinates(0, 1, -1, 0), + PLANE_YZ, CW) + self.assertEqual(pos, Coordinates(5, 0, 0, 6)) + _, pos = self.__check_circular(Coordinates(-2, -2, 3, 2), + Coordinates(-1, -1, 0, 0), + PLANE_XY, CCW) + self.assertEqual(pos, Coordinates(-2, -2, 3, 2)) def test_acceleration_velocity(self): # Check if acceleration present in pulses sequence and if velocity - # is correct + # is correct, since PulseGenerator is responsible for this, check only + # one child class. m = Coordinates(TABLE_SIZE_X_MM, 0, 0, 0) g = PulseGeneratorLinear(m, self.v) i = 0 @@ -156,6 +280,11 @@ class TestPulses(unittest.TestCase): dir_found = True # check dirs self.assertTrue(px < 0 and py > 0 and pz < 0 and pe > 0) + # check for circle, full circle + dir_changed, _ = self.__check_circular(Coordinates(0, 0, 0, 0), + Coordinates(1.0, 1.0, 0, 0), + PLANE_ZX, CCW) + self.assertEqual(dir_changed, 4) if __name__ == '__main__':