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