def linear_move_poll(self, target_pos: Coordinate, speed: float = None, track_speed=False, current_pos=None): """ Moves the robot linearly to a coordinate. :param target_pos: Coordinate for the target position. :param speed: Movement speed for tool. :param track_speed: :param current_pos: :return: """ # Set speed if speed is not None: self.set_speed(speed, 'linear') # Only send command if any coordinates are passed, otherwise just set the speed if len(target_pos.values) > 0 and any(a is not None for a in target_pos.values): # Fill None values with current position to predict correct response if current_pos is None: # No need to do this twice current_pos = self.protocol.get_current_xyzabc() target_pos.add_axis(current_pos) target_pos.update_empty(current_pos) # Send move command self.protocol.linear_move(target_pos) # Wait until position is reached t, v = cmp_response(R3Protocol_Cmd.CURRENT_XYZABC, target_pos.to_melfa_response(), self.protocol.reader, track_speed=track_speed) return t, v return None, None
def test_update_empty_exception(self): """ Test exception raising for not-present axis. :return: """ a = Coordinate((0, 0, None), "XYZ") b = Coordinate((1, 2, 3), "XYA") with pytest.raises(TypeError) as excinfo: a.update_empty(b) assert str(excinfo.value.args[0]) == "Incompatible axis."
def test_update_empty_successful(self): """ Test successful update. :return: """ axes = "XYZ" a = Coordinate((0, 0, None), axes) b = Coordinate((1, 2, 3), axes) a.update_empty(b) assert list(a.values) == [0, 0, 3] and list(a.axes) == ["X", "Y", "Z"]
def go_home(self, option="") -> None: """ Moves the robot to its current home point (current work coordinate origin or global safe position respectively) :return: """ if self.work_coordinate_active: # Acquire new zero coordinate zero = Coordinate(self.zero.values, self.zero.axes) if option != "": zero = zero.reduce_to_axes(option, make_none=True) # Acquire current position to determine robot orientation current_position = self.protocol.reader.get_current_xyzabc() zero.add_axis(current_position) zero.update_empty(current_position) # Move to zero self.linear_move_poll(zero) else: self.go_safe_pos()
def circular_move_poll(self, target_pos: Coordinate, center_pos: Coordinate, is_clockwise: bool, speed: Optional[float] = None, start_pos: Optional[Coordinate] = None) -> None: """ Moves the robot on a (counter-)clockwise arc around a center position to a target position. :param start_pos: Coordinate for the start position, defaults to current position if None. The robot first performs a linear movement to the start position if it is not equal to the current position. :param target_pos: Coordinate for the target position. :param center_pos: Coordinate for the center of the arc. :param is_clockwise: Flag to indicate clockwise|counter-clockwise direction. :param speed: Movement speed for tool. """ # Determine start position if start_pos is None: start_pos = self.protocol.get_current_xyzabc() start_pos = start_pos.reduce_to_axes('XYZ') # Set speed if speed is not None: self.set_speed(speed, "linear") # Only send command if any coordinates are passed, otherwise just set the speed if len(target_pos.values) > 0 and any(a is not None for a in target_pos.values): # Update positions to be complete target_pos.update_empty(start_pos) center_pos.update_empty(start_pos) start_pos_np = np.array(start_pos.values) target_pos_np = np.array(target_pos.values) center_pos_np = np.array(center_pos.values) angle = self.get_directed_angle(start_pos_np, target_pos_np, center_pos_np, is_clockwise) if abs(angle) >= pi: # Intermediate points for angles >= 180° im_pos_np = get_intermediate_point(angle, start_pos_np, target_pos_np, center_pos_np, self.active_plane) im_pos = Coordinate(list(im_pos_np), 'XYZ') im_pos.update_empty(start_pos) # Position assignments if abs(angle) == 2 * pi: # Calculate additional intermediate point angle = self.get_directed_angle(start_pos_np, im_pos_np, center_pos_np, is_clockwise) im_pos2_np = get_intermediate_point( angle, start_pos_np, im_pos_np, center_pos_np, self.active_plane) im_pos2 = Coordinate(list(im_pos2_np), 'XYZ') im_pos2.update_empty(start_pos) # Global variables self.set_global_positions(["P1", "P2", "P3"], [start_pos, im_pos2, im_pos]) # Send move command self.protocol.circular_move_full("P1", "P2", "P3") else: # Global variables self.set_global_positions(["P1", "P2", "P3"], [start_pos, im_pos, target_pos]) # Send move command self.protocol.circular_move_intermediate("P1", "P2", "P3") else: # Global variables self.set_global_positions(["P1", "P2", "P3"], [start_pos, target_pos, center_pos]) # Send move command self.protocol.circular_move_centre("P1", "P2", "P3") # Wait until position is reached cmp_response(R3Protocol_Cmd.CURRENT_XYZABC, target_pos.to_melfa_response(), self.protocol.reader)