vincent/PyCNC
1
0
Fork 0
mirror of https://github.com/sinseman44/PyCNC.git synced 2026-07-16 08:37:09 +00:00
Code Issues Projects Releases Wiki Activity

circular interpolation

Browse Source
This commit is contained in:
Nikolay Khabarov
2017-06-10 23:27:45 +03:00
parent dfb710f4a5
commit 4093fe6661
9 changed files with 544 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cnc/gmachine.py
+26 -12
View File
@@ -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))
cnc/hal.py
+5 -21
View File
@@ -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")
cnc/hal_raspberry/hal.py
+8 -24
View File
@@ -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.
"""
cnc/hal_virtual.py
+30 -38
View File
@@ -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.
"""
cnc/pulses.py
+309 -14
View File
@@ -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)