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 _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)