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)