vincent/PyCNC
1
0
Fork 0
mirror of https://github.com/sinseman44/PyCNC.git synced 2026-05-05 21:18:11 +00:00
Code Issues Projects Releases Wiki Activity

improve circular interpolation for arbitary pulses per mm in config

Browse Source
This commit is contained in:
Nikolay Khabarov
2017-06-26 00:12:33 +03:00
parent ca174344a6
commit 6a510ffc1c
2 changed files with 62 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
cnc/pulses.py
View File

@@ -334,7 +334,10 @@ class PulseGeneratorCircular(PulseGenerator):
# adjust radius to fit into axises step. # adjust radius to fit into axises step.
radius = (round(math.sqrt(sa * sa + sb * sb) * min(apm, bpm)) radius = (round(math.sqrt(sa * sa + sb * sb) * min(apm, bpm))
/ min(apm, bpm)) / min(apm, bpm))
self._radius2 = radius * radius radius_a = (round(math.sqrt(sa * sa + sb * sb) * apm) / apm)
radius_b = (round(math.sqrt(sa * sa + sb * sb) * bpm) / bpm)
self._radius_a2 = radius_a * radius_a
self._radius_b2 = radius_b * radius_b
self._radius_a_pulses = int(radius * apm) self._radius_a_pulses = int(radius * apm)
self._radius_b_pulses = int(radius * bpm) self._radius_b_pulses = int(radius * bpm)
self._start_a_pulses = int(sa * apm) self._start_a_pulses = int(sa * apm)
@@ -391,12 +394,12 @@ class PulseGeneratorCircular(PulseGenerator):
quarter_start = int(start_angle / (math.pi / 2.0)) quarter_start = int(start_angle / (math.pi / 2.0))
quarter_end = int(end_angle_m / (math.pi / 2.0)) quarter_end = int(end_angle_m / (math.pi / 2.0))
if quarter_end - quarter_start >= 4: if quarter_end - quarter_start >= 4:
self._iterations_a = 4 * int(radius * apm) self._iterations_a = 4 * round(radius * apm)
self._iterations_b = 4 * int(radius * apm) self._iterations_b = 4 * round(radius * bpm)
else: else:
if quarter_start == quarter_end: if quarter_start == quarter_end:
self._iterations_a = int(abs(sa - ea) * apm) self._iterations_a = round(abs(sa - ea) * apm)
self._iterations_b = int(abs(sb - eb) * bpm) self._iterations_b = round(abs(sb - eb) * bpm)
else: else:
for r in range(quarter_start, quarter_end + 1): for r in range(quarter_start, quarter_end + 1):
i = r i = r
@@ -404,64 +407,80 @@ class PulseGeneratorCircular(PulseGenerator):
i -= 4 i -= 4
if r == quarter_start: if r == quarter_start:
if i == 0 or i == 2: if i == 0 or i == 2:
self._iterations_a += int(radius * apm) \ self._iterations_a += round(radius * apm) \
- int(abs(sa) * apm) - round(abs(sa) * apm)
else: else:
self._iterations_a += int(abs(sa) * apm) self._iterations_a += round(abs(sa) * apm)
if i == 1 or i == 3: if i == 1 or i == 3:
self._iterations_b += int(radius * bpm) \ self._iterations_b += round(radius * bpm) \
- int(abs(sb) * bpm) - round(abs(sb) * bpm)
else: else:
self._iterations_b += int(abs(sb) * bpm) self._iterations_b += round(abs(sb) * bpm)
elif r == quarter_end: elif r == quarter_end:
if i == 0 or i == 2: if i == 0 or i == 2:
self._iterations_a += int(abs(ea) * apm) self._iterations_a += round(abs(ea) * apm)
else: else:
self._iterations_a += int(radius * apm) \ self._iterations_a += round(radius * apm) \
- int(abs(ea) * apm) - round(abs(ea) * apm)
if i == 1 or i == 3: if i == 1 or i == 3:
self._iterations_b += int(abs(eb) * bpm) self._iterations_b += round(abs(eb) * bpm)
else: else:
self._iterations_b += int(radius * bpm) \ self._iterations_b += round(radius * bpm) \
- int(abs(eb) * bpm) - round(abs(eb) * bpm)
else: else:
self._iterations_a += int(radius * apm) self._iterations_a += round(radius * apm)
self._iterations_b += int(radius * bpm) self._iterations_b += round(radius * bpm)
if direction == CCW: if direction == CCW:
self._iterations_a = 4 * int(radius * apm) - self._iterations_a self._iterations_a = 4 * round(radius * apm) - self._iterations_a
self._iterations_b = 4 * int(radius * bpm) - self._iterations_b self._iterations_b = 4 * round(radius * bpm) - self._iterations_b
arc = delta_angle * radius arc = delta_angle * radius
e2 = delta.e * delta.e e2 = delta.e * delta.e
if self._plane == PLANE_XY: if self._plane == PLANE_XY:
self._iterations_3rd = abs(delta.z) * STEPPER_PULSES_PER_MM_Z self._iterations_3rd = abs(delta.z) * STEPPER_PULSES_PER_MM_Z
l = math.sqrt(arc * arc + delta.z * delta.z + e2) l = math.sqrt(arc * arc + delta.z * delta.z + e2)
self._velocity_3rd = abs(delta.z) / l * velocity if l == 0:
self._velocity_3rd = velocity
else:
self._velocity_3rd = abs(delta.z) / l * velocity
self._third_dir = math.copysign(1, delta.z) self._third_dir = math.copysign(1, delta.z)
elif self._plane == PLANE_YZ: elif self._plane == PLANE_YZ:
self._iterations_3rd = abs(delta.x) * STEPPER_PULSES_PER_MM_X self._iterations_3rd = abs(delta.x) * STEPPER_PULSES_PER_MM_X
l = math.sqrt(arc * arc + delta.x * delta.x + e2) l = math.sqrt(arc * arc + delta.x * delta.x + e2)
self._velocity_3rd = abs(delta.x) / l * velocity if l == 0:
self._velocity_3rd = velocity
else:
self._velocity_3rd = abs(delta.x) / l * velocity
self._third_dir = math.copysign(1, delta.x) self._third_dir = math.copysign(1, delta.x)
elif self._plane == PLANE_ZX: elif self._plane == PLANE_ZX:
self._iterations_3rd = abs(delta.y) * STEPPER_PULSES_PER_MM_Y self._iterations_3rd = abs(delta.y) * STEPPER_PULSES_PER_MM_Y
l = math.sqrt(arc * arc + delta.y * delta.y + e2) l = math.sqrt(arc * arc + delta.y * delta.y + e2)
self._velocity_3rd = abs(delta.y) / l * velocity if l == 0:
self._velocity_3rd = velocity
else:
self._velocity_3rd = abs(delta.y) / l * velocity
self._third_dir = math.copysign(1, delta.y) self._third_dir = math.copysign(1, delta.y)
else: else:
raise ValueError("Unknown plane") raise ValueError("Unknown plane")
self._iterations_e = abs(delta.e) * STEPPER_PULSES_PER_MM_E self._iterations_e = abs(delta.e) * STEPPER_PULSES_PER_MM_E
# Velocity splits with corresponding distance. # Velocity splits with corresponding distance.
circular_velocity = arc / l * velocity if l == 0:
circular_velocity = velocity
self._e_velocity = velocity
else:
circular_velocity = arc / l * velocity
self._e_velocity = abs(delta.e) / l * velocity
self._r_div_v = radius / circular_velocity self._r_div_v = radius / circular_velocity
self._e_velocity = abs(delta.e) / l * velocity
self._e_dir = math.copysign(1, delta.e) self._e_dir = math.copysign(1, delta.e)
self.max_velocity_mm_per_sec = max(circular_velocity, self.max_velocity_mm_per_sec = max(circular_velocity,
self._velocity_3rd, self._e_velocity) self._velocity_3rd, self._e_velocity)
self.acceleration_time_s = (self.max_velocity_mm_per_sec self.acceleration_time_s = (self.max_velocity_mm_per_sec
/ STEPPER_MAX_ACCELERATION_MM_PER_S2) / STEPPER_MAX_ACCELERATION_MM_PER_S2)
if STEPPER_MAX_ACCELERATION_MM_PER_S2 * self.acceleration_time_s ** 2 \ if l == 0:
> l: self.linear_time_s = 0.0
self.max_velocity_mm_per_sec = 0
elif STEPPER_MAX_ACCELERATION_MM_PER_S2 * self.acceleration_time_s \
** 2 > l:
self.acceleration_time_s = \ self.acceleration_time_s = \
math.sqrt(l / STEPPER_MAX_ACCELERATION_MM_PER_S2) math.sqrt(l / STEPPER_MAX_ACCELERATION_MM_PER_S2)
self.linear_time_s = 0.0 self.linear_time_s = 0.0
@@ -520,10 +539,10 @@ class PulseGeneratorCircular(PulseGenerator):
self._radius_a_pulses, self._radius_a_pulses,
self._side_a, self._dir_a) self._side_a, self._dir_a)
a /= pulses_per_mm a /= pulses_per_mm
# last item can be slightly more then end angle due to float precision # first and last item can be slightly out of bound due float precision
if i + 1 == self._iterations_a: if i + 1 == self._iterations_a:
return direction, self._r_div_v * self._delta_angle return direction, self._r_div_v * self._delta_angle
b = math.sqrt(self._radius2 - a * a) b = math.sqrt(self._radius_a2 - a * a)
if side: if side:
b = -b b = -b
return direction, self.__circular_find_time(a, b) return direction, self.__circular_find_time(a, b)
@@ -535,10 +554,10 @@ class PulseGeneratorCircular(PulseGenerator):
self._radius_b_pulses, self._radius_b_pulses,
self._side_b, self._dir_b) self._side_b, self._dir_b)
b /= pulses_per_mm b /= pulses_per_mm
# last item can be slightly more then end angle due to float precision # first and last item can be slightly out of bound due float precision
if i + 1 == self._iterations_b: if i + 1 == self._iterations_b:
return direction, self._r_div_v * self._delta_angle return direction, self._r_div_v * self._delta_angle
a = math.sqrt(self._radius2 - b * b) a = math.sqrt(self._radius_b2 - b * b)
if side: if side:
a = -a a = -a
return direction, self.__circular_find_time(a, b) return direction, self.__circular_find_time(a, b)

16
tests/test_pulses.py
View File

@@ -167,18 +167,24 @@ class TestPulses(unittest.TestCase):
m = Coordinates(2, 4, 0, 0) m = Coordinates(2, 4, 0, 0)
g = PulseGeneratorLinear(m, self.v) g = PulseGeneratorLinear(m, self.v)
i = 0 i = 0
j = 0
k = 0
for direction, px, py, pz, pe in g: for direction, px, py, pz, pe in g:
if direction: if direction:
continue continue
if i % 2 == 0: if py is not None:
self.assertNotEqual(px, None) k += 1
j += 1
else: else:
self.assertEqual(px, None) self.assertNotEqual(px, None)
self.assertNotEqual(py, None) if px is not None:
if i != 0:
self.assertEqual(j, 2)
j = 0
self.assertEqual(pz, None) self.assertEqual(pz, None)
self.assertEqual(pe, None) self.assertEqual(pe, None)
i += 1 i += 1
self.assertEqual(m.find_max() * STEPPER_PULSES_PER_MM_Y, i) self.assertEqual(k / STEPPER_PULSES_PER_MM_Y, m.y)
def test_pulses_count_and_timings(self): def test_pulses_count_and_timings(self):
# Check if number of pulses is equal to specified distance. # Check if number of pulses is equal to specified distance.