def execute(self, data):
self.logger.log("Acquire Information From Antenna: Execution.")
antenna_data = None
antenna_data = self.hardware.antenna.get_information_from_signal()
if antenna_data is None:
self.logger.log(
"Acquire Information From Antenna: Not acquired yet.")
return (self.current_step, None)
elif antenna_data is not None and antenna_data.painting_number is None and antenna_data.zoom is None and antenna_data.orientation is None:
self.logger.log(
"Acquire Information From Antenna: Acquisition has failed. Going back to starting search point and retrying the entire sequence."
)
self.hardware.antenna.reinitialize()
self.antenna_information.strength_curve = defaultdict(lambda: 0)
return (
Step.PREPARE_TRAVEL_TO_ANTENNA_ZONE,
Packet(
PacketType.NOTIFICATION,
"Acquisition has failed. Going back to starting search point."
))
else:
self.logger.log(
"Acquire Information From Antenna: Acquired! PNB = {0} - ZOOM = {1} - ORI = {2}"
.format(int(antenna_data.painting_number),
int(antenna_data.zoom), int(antenna_data.orientation)))
self.antenna_information.painting_number = int(
antenna_data.painting_number)
self.antenna_information.zoom = int(antenna_data.zoom)
self.antenna_information.orientation = float(
antenna_data.orientation)
return (next_step(self.current_step), None)
def execute(self, data):
if not self.is_positional_telemetry_recieved(data):
return (self.current_step, None)
self.logger.log("Prepare Travel to Antenna Area: Execution.")
self.pathfinder.generate_path_to_checkpoint_a_to_b(
self.pathfinder.get_point_of_interest(
PointOfInterest.ANTENNA_START_SEARCH_POINT))
self.pathfinder.robot_status.set_position(data[0])
new_vector = (self.pathfinder.robot_status.
generate_new_translation_vector_towards_current_target(
self.pathfinder.robot_status.get_position()))
self.logger.log(
"Prepare Travel to Antenna Area: Sending vector {0} to robot at position {1} to reach"
"the antenna's starting point.".format(
new_vector, self.pathfinder.robot_status.get_position()))
self.hardware.wheels.move(new_vector)
return (next_step(self.current_step),
Packet(PacketType.PATH, self.pathfinder.get_current_path()))
def execute(self, data):
self.logger.log("Prepare Exit of Drawing Area: Execution.")
self.hardware.pen.raise_pen()
possible_exit_locations = self.pathfinder.get_point_of_interest(
PointOfInterest.EXIT_DRAWING_ZONE_AFTER_CYCLE)
for exit_location in possible_exit_locations:
if self.pathfinder.is_checkpoint_accessible(exit_location):
self.pathfinder.generate_path_to_checkpoint(exit_location)
break
self.logger.log(
"Prepare Exit of Drawing Area: Exit location at position = {0}".
format(self.pathfinder.robot_status.target_position))
self.hardware.wheels.move(
self.pathfinder.robot_status.
generate_new_translation_vector_towards_current_target(
self.pathfinder.robot_status.get_position()))
return (next_step(self.current_step),
Packet(PacketType.PATH, self.pathfinder.get_current_path()))
def execute(self, telemetry_data):
if not self.is_positional_telemetry_recieved(telemetry_data):
return (self.current_step, None)
self.logger.log("Prepare Realign With First Vertex Drawn: Execution.")
drawing_zone_origin_x, drawing_zone_origin_y = self.pathfinder.get_point_of_interest(
PointOfInterest.DRAWING_ZONE)
first_vertex_x, first_vertex_y = self.vision.get_captured_vertices(
self.antenna_information.zoom,
self.antenna_information.orientation)[0]
position_x = drawing_zone_origin_x + first_vertex_x
position_y = drawing_zone_origin_y + first_vertex_y
self.logger.log(
"Prepare Realign With First Vertex Drawn: Moving towards first vertex at position = {0}"
.format((position_x, position_y)))
self.pathfinder.generate_path_to_checkpoint_a_to_b(
(position_x, position_y))
self.hardware.wheels.move(
self.pathfinder.robot_status.
generate_new_translation_vector_towards_current_target(
telemetry_data[0]))
return (next_step(self.current_step),
self.pathfinder.get_current_path())
def execute(self, data):
self.logger.log("Prepare Travel to Drawing Area: Execution.")
drawing_zone_origin_x, drawing_zone_origin_y = self.pathfinder.get_point_of_interest(
PointOfInterest.DRAWING_ZONE)
first_vertex_x, first_vertex_y = self.vision_data.get_captured_vertices(
self.antenna_information.zoom,
self.antenna_information.orientation)[0]
position_x = drawing_zone_origin_x + first_vertex_x
position_y = drawing_zone_origin_y + first_vertex_y
self.logger.log(
"Prepare Travel to Drawing Area: Moving towards first vertex at position = {0}"
.format((position_x, position_y)))
self.pathfinder.generate_path_to_checkpoint((position_x, position_y))
self.hardware.wheels.move(
self.pathfinder.robot_status.
generate_new_translation_vector_towards_current_target(
self.pathfinder.robot_status.get_position()))
self.logger.log(
"Prepare Travel to Drawing Area: Sending vector = {0} to robot at position = {1} to target = {2}"
.format(self.hardware.wheels.last_vector_given,
self.pathfinder.robot_status.get_position(),
self.pathfinder.robot_status.target_position))
return (next_step(self.current_step),
Packet(PacketType.PATH, self.pathfinder.get_current_path()))
def execute(self, telemetry_data):
if not self.is_positional_telemetry_recieved(telemetry_data):
return (self.current_step, None)
self.logger.log("Prepare Travel to Painting: Execution.")
self.hardware.pen.raise_pen()
self.logger.log("Prepare Travel to Painting: painting_nb = {0}".format(
self.antenna_information.painting_number))
figure_position = self.pathfinder.figures.get_position_to_take_figure_from(
self.antenna_information.painting_number)
self.pathfinder.generate_path_to_checkpoint(figure_position)
self.hardware.wheels.move(
self.pathfinder.robot_status.
generate_new_translation_vector_towards_current_target(
self.pathfinder.robot_status.get_position()))
self.logger.log(
"Prepare Travel to Painting: Sending {0} vector to robot at position {1} towards target {2}"
.format(self.hardware.wheels.last_vector_given,
self.pathfinder.robot_status.get_position(),
self.pathfinder.robot_status.target_position))
return (next_step(self.current_step),
Packet(PacketType.PATH, self.pathfinder.get_current_path()))
def execute(self, telemetry_data):
self.logger.log("Translation Check Without Telemetry: Step = {0}".format(self.current_step))
if self.servo_wheels_manager.translation_status == TranslationStatus.MOVING:
if not self.servo_wheels_manager.is_current_translation_movement_done(self.hardware.wheels):
return (self.current_step, None)
else:
self.pathfinder.robot_status.set_position(self.pathfinder.robot_status.target_position)
path_status, new_vector = self.pathfinder.get_vector_to_next_node(
self.pathfinder.robot_status.get_position())
if path_status == PathStatus.CHECKPOINT_REACHED:
return (next_step(self.current_step), None)
elif path_status == PathStatus.INTERMEDIATE_NODE_REACHED:
self.hardware.wheels.move(new_vector)
return (self.current_step, None)
def execute(self, data):
self.logger.log("Prepare Moving of Antenna Offset: Execution.")
position_x, position_y = self.pathfinder.robot_status.get_position()
position_y = position_y + PEN_TO_ANTENNA_OFFSET
self.pathfinder.generate_path_to_checkpoint_a_to_b(
(position_x, position_y))
self.hardware.wheels.move(
self.pathfinder.robot_status.
generate_new_translation_vector_towards_current_target(
self.pathfinder.robot_status.get_position()))
return (next_step(self.current_step),
Packet(PacketType.PATH, self.pathfinder.get_current_path()))
def execute(self, data):
print("prepare search for antenna position command")
self.hardware.antenna.start_sampling()
self.pathfinder.generate_path_to_checkpoint_a_to_b(
self.pathfinder.get_point_of_interest(
PointOfInterest.ANTENNA_STOP_SEARCH_POINT))
self.hardware.wheels.move(
self.pathfinder.robot_status.
generate_new_translation_vector_towards_current_target(
self.pathfinder.robot_status.get_position()))
return (next_step(self.current_step),
Packet(PacketType.PATH, self.pathfinder.get_current_path()))
def execute(self, game_map_data):
self.logger.log("Build Game Map: Execution.")
self.hardware.lights.turn_off_red_led()
self.pathfinder.set_game_map(game_map_data)
rotation_angle = (
self.pathfinder.robot_status.
set_target_heading_and_get_angular_difference(STANDARD_HEADING))
self.logger.log(
"Build Game Map: Aligning westwards with rotation angle = {0}".
format(rotation_angle))
self.hardware.wheels.rotate(rotation_angle)
return (next_step(self.current_step), None)
def execute(self, telemetry_data):
if not self.is_positional_telemetry_recieved(telemetry_data):
return (self.current_step, None)
self.logger.log("Prepare Align With Capture Command: Execution")
figure_position = self.pathfinder.figures.get_position_to_take_figure_from(
self.antenna_information.painting_number)
self.pathfinder.generate_path_to_checkpoint_a_to_b(figure_position)
self.hardware.wheels.move(
self.pathfinder.robot_status.
generate_new_translation_vector_towards_current_target(
telemetry_data[0]))
return (next_step(self.current_step),
self.pathfinder.get_current_path())
def execute(self, data):
self.logger.log("Face Relevant Figure for Capture: Execution.")
orientation = self.pathfinder.figures.get_orientation_to_take_figure_from(
self.antenna_information.painting_number)
rotation_angle = (
self.pathfinder.robot_status.
set_target_heading_and_get_angular_difference(orientation))
self.logger.log(
"Face Relevant Figure for Capture: Requested heading = {0} deg - Rotation angle = {0} deg"
.format(self.pathfinder.robot_status.target_position,
rotation_angle))
self.hardware.wheels.rotate(rotation_angle)
return (next_step(self.current_step), None)
def execute(self, data):
self.logger.log("Prepare Marking Antenna: Execution.")
self.hardware.pen.lower_pen()
position_x, position_y = self.pathfinder.robot_status.get_position()
position_x = position_x - 0.8
self.pathfinder.generate_path_to_checkpoint_a_to_b(
(position_x, position_y))
self.hardware.wheels.move(
self.pathfinder.robot_status.
generate_new_translation_vector_towards_current_target(
self.pathfinder.robot_status.get_position()))
return (next_step(self.current_step),
Packet(PacketType.PATH, self.pathfinder.get_current_path()))
def execute(self, telemetry_data):
if not self.is_positional_telemetry_recieved(telemetry_data):
return (self.current_step, None)
position = telemetry_data[0]
orientation = telemetry_data[1]
self.logger.log("Translation Check: Step = {0} - Telemetry position = {1} - Telemetry heading = {2}".format(
self.current_step, position, orientation))
if self.servo_wheels_manager.translation_status == TranslationStatus.MOVING:
if not self.servo_wheels_manager.is_current_translation_movement_done(self.hardware.wheels):
return (self.current_step, None)
else:
self.servo_wheels_manager.translating_start_heading_correction(orientation,
self.pathfinder.robot_status,
self.hardware.wheels)
return (self.current_step, None)
elif self.servo_wheels_manager.translation_status == TranslationStatus.CORRECTING_HEADING:
if not self.servo_wheels_manager.is_current_rotation_movement_done(self.hardware.wheels):
return (self.current_step, None)
else:
self.servo_wheels_manager.translating_start_position_correction(position, self.pathfinder.robot_status,
self.hardware.wheels)
return (self.current_step, None)
elif self.servo_wheels_manager.translation_status == TranslationStatus.CORRECTING_POSITION:
if not self.servo_wheels_manager.is_current_translation_movement_done(self.hardware.wheels):
return (self.current_step, None)
else:
path_status, new_vector = self.servo_wheels_manager.finish_translation_and_get_new_path_status_and_vector(
position, self.pathfinder)
if path_status == PathStatus.CHECKPOINT_REACHED:
return (next_step(self.current_step), None)
elif path_status == PathStatus.INTERMEDIATE_NODE_REACHED:
self.hardware.wheels.move(new_vector)
return (self.current_step, None)
else:
self.servo_wheels_manager.translating_start_position_correction(position,
self.pathfinder.robot_status,
self.hardware.wheels)
return (self.current_step, None)
def execute(self, data):
self.logger.log(
"Capture Figure: Execution with zoom = {0} and orientation = {1}".
format(self.antenna_information.zoom,
self.antenna_information.orientation))
try:
self.vision.capture()
self.vision.get_captured_vertices(
self.antenna_information.zoom,
self.antenna_information.orientation)
self.logger.log("Capture Figure: Success.")
self.hardware.lights.light_green_led(1000)
rotation_angle = (self.pathfinder.robot_status.
set_target_heading_and_get_angular_difference(
STANDARD_HEADING))
self.hardware.wheels.rotate(rotation_angle)
self.logger.log(
"Capture Figure: Rotating to standard heading from heading = {0}"
.format(self.hardware.wheels.last_degrees_of_rotation_given))
self.vision.pixel_coordinates.append(
self.vision.pixel_coordinates[0])
return (next_step(self.current_step), [
Packet(PacketType.FIGURE_VERTICES,
self.vision.pixel_coordinates),
Packet(PacketType.FIGURE_IMAGE, self.vision.last_capture)
])
except PaintingFrameNotFound:
self.logger.log(
"Capture Figure: Failure - Painting Frame Not Found")
return (Step.REPOSITION_FOR_CAPTURE_RETRY, None)
except VerticesNotFound:
self.logger.log("Capture Figure: Failure - Vertices Not Found")
return (Step.REPOSITION_FOR_CAPTURE_RETRY, None)
def execute(self, data):
self.logger.log("Prepare to Draw: Execution.")
drawing_reference_origin = self.pathfinder.get_point_of_interest(
PointOfInterest.DRAWING_ZONE)
self.pathfinder.nodes_queue_to_checkpoint.clear()
for vertex in self.vision.get_captured_vertices(
self.antenna_information.zoom,
self.antenna_information.orientation):
point_to_cross_x = drawing_reference_origin[0] + vertex[0]
point_to_cross_y = drawing_reference_origin[1] + vertex[1]
self.pathfinder.nodes_queue_to_checkpoint.append(
(point_to_cross_x, point_to_cross_y))
first_vertex_x, first_vertex_y = self.vision.get_captured_vertices(
self.antenna_information.zoom,
self.antenna_information.orientation)[0]
first_vertex_x += drawing_reference_origin[0]
first_vertex_y += drawing_reference_origin[1]
self.pathfinder.nodes_queue_to_checkpoint.append(
(first_vertex_x, first_vertex_y))
self.pathfinder.robot_status.set_position(
(first_vertex_x, first_vertex_y))
self.logger.log("Prepare to Draw: Vertices to draw = {0}".format(
self.pathfinder.nodes_queue_to_checkpoint))
self.hardware.wheels.move(
self.pathfinder.robot_status.
generate_new_translation_vector_towards_new_target(
self.pathfinder.nodes_queue_to_checkpoint.popleft()))
self.hardware.pen.lower_pen()
return (next_step(self.current_step),
Packet(PacketType.PATH, self.pathfinder.get_current_path()))
def execute(self, telemetry_data):
self.logger.log("Prepare Moving to Antenna Position: Execution.")
self.hardware.antenna.stop_sampling()
try:
position_x, position_y = max(
self.antenna_information.strength_curve,
key=self.antenna_information.strength_curve.get)
self.logger.log(
"Prepare Moving to Antenna Position: Calculated position of max signal amplitude = {0}"
.format((position_x, position_y)))
self.pathfinder.generate_path_to_checkpoint_a_to_b(
(position_x, position_y))
self.hardware.wheels.move(
self.pathfinder.robot_status.
generate_new_translation_vector_towards_current_target(
self.pathfinder.robot_status.get_position()))
self.logger.log(
"Prepare Moving to Antenna Position: Sending vector = {0} to robot at position = {1} to target = {2}"
.format(self.hardware.wheels.last_vector_given,
self.pathfinder.robot_status.get_position(),
self.pathfinder.robot_status.target_position))
return (next_step(self.current_step),
Packet(PacketType.PATH,
self.pathfinder.get_current_path()))
except ValueError:
return (
Step.COMPUTE_PAINTINGS_AREA,
Packet(
PacketType.NOTIFICATION,
"Antenna position has not been found! Aborting search."))