diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index b8b2f211..75f5e2f9 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -36,7 +36,12 @@ def __init__(self) -> None: Path([ShootingPositions.amp_speaker_bounce]), Path([NotePositions.amp]), ] - super().__init__(note_paths, shoot_paths) + # Start pose only needs to be on the correct half of the field, + # so choose the podium as a reference point + start_pose = Pose2d( + NotePositions.podium, rotation_to_red_speaker(NotePositions.podium) + ) + super().__init__(note_paths, shoot_paths, start_pose) class AmpCentre1(AutoBase): @@ -52,7 +57,12 @@ def __init__(self) -> None: Path([NotePositions.amp]), Path([PathPositions.avoid_wall, NotePositions.amp]), ] - super().__init__(note_paths, shoot_paths) + # Start pose only needs to be on the correct half of the field, + # so choose the amp as a reference point + start_pose = Pose2d( + NotePositions.amp, rotation_to_red_speaker(NotePositions.amp) + ) + super().__init__(note_paths, shoot_paths, start_pose) class SpeakerCentre3(AutoBase): @@ -68,7 +78,12 @@ def __init__(self) -> None: Path([NotePositions.speaker]), Path([PathPositions.stage_transition_N, NotePositions.speaker]), ] - super().__init__(note_paths, shoot_paths) + # Start pose only needs to be on the correct half of the field, + # so choose the speaker as a reference point + start_pose = Pose2d( + NotePositions.speaker, rotation_to_red_speaker(NotePositions.speaker) + ) + super().__init__(note_paths, shoot_paths, start_pose) class Centre3Centre5(AutoBase): diff --git a/components/led.py b/components/led.py index a8b20462..d69188cc 100644 --- a/components/led.py +++ b/components/led.py @@ -92,6 +92,15 @@ def morse(self) -> None: def rainbow(self) -> None: self.pattern = Rainbow(HsvColour.RED) + def invalid_start(self) -> None: + self.pattern = Flash(HsvColour.RED) + + def missing_start_pose(self) -> None: + self.pattern = Flash(HsvColour.CYAN) + + def no_vision(self) -> None: + self.pattern = Flash(HsvColour.ORANGE) + def disabled(self) -> None: self.pattern = Solid(HsvColour.OFF) diff --git a/components/vision.py b/components/vision.py index 65c2c35e..5806169b 100644 --- a/components/vision.py +++ b/components/vision.py @@ -1,4 +1,5 @@ import math +import time from typing import Optional import wpilib @@ -21,6 +22,9 @@ class VisualLocalizer: # Give bias to the best pose by multiplying this const to the alt dist BEST_POSE_BIAS = 1.2 + # Time since the last target sighting we allow before informing drivers + TIMEOUT = 1.0 # s + add_to_estimator = tunable(True) should_log = tunable(False) @@ -41,6 +45,7 @@ def __init__( self.camera = PhotonCamera(name) self.camera_to_robot = Transform3d(pos, rot).inverse() self.last_timestamp = -1 + self.last_recieved_timestep = -1.0 self.single_best_log = field.getObject(name + "single_best_log") self.single_alt_log = field.getObject(name + "single_alt_log") @@ -68,6 +73,7 @@ def execute(self) -> None: if timestamp == self.last_timestamp: return + self.last_recieved_timestep = time.monotonic() self.last_timestamp = timestamp if results.multiTagResult.estimatedPose.isPresent: @@ -140,6 +146,9 @@ def execute(self) -> None: ) ) + def sees_target(self): + return time.monotonic() - self.last_recieved_timestep < self.TIMEOUT + def estimate_poses_from_apriltag( cam_to_robot: Transform3d, target: PhotonTrackedTarget diff --git a/robot.py b/robot.py index 43224b7b..8de90135 100644 --- a/robot.py +++ b/robot.py @@ -17,11 +17,12 @@ from controllers.intake import Intake from controllers.shooter import Shooter -from utilities.game import is_red - +from autonomous.base import AutoBase +from utilities.game import is_red from utilities.scalers import rescale_js from utilities.functions import clamp +from utilities.position import on_same_side_of_stage class MyRobot(magicbot.MagicRobot): @@ -192,6 +193,29 @@ def disabledPeriodic(self) -> None: self.vision_port.execute() self.vision_starboard.execute() + # check if we can see targets + if ( + not self.vision_port.sees_target() + and not self.vision_starboard.sees_target() + ): + self.status_lights.no_vision() + else: + # check we start on the correct side of the stage + selected_auto = self._automodes.chooser.getSelected() + if isinstance(selected_auto, AutoBase): + intended_start_pose = selected_auto.get_starting_pose() + if intended_start_pose is not None: + if on_same_side_of_stage( + intended_start_pose, self.chassis.get_pose() + ): + self.status_lights.rainbow() + else: + self.status_lights.invalid_start() + else: + self.status_lights.missing_start_pose() + else: + self.status_lights.missing_start_pose() + def autonomousInit(self) -> None: pass diff --git a/utilities/position.py b/utilities/position.py index 20258942..e6fe3b8a 100644 --- a/utilities/position.py +++ b/utilities/position.py @@ -56,6 +56,12 @@ class TeamPoses: RED_PODIUM = field_flip_pose2d(BLUE_PODIUM) +def on_same_side_of_stage(pose1: Pose2d, pose2: Pose2d) -> bool: + return not ( + (pose1.y > TeamPoses.BLUE_PODIUM.y) ^ (pose2.y > TeamPoses.BLUE_PODIUM.y) + ) + + class PathPositions: stage_transition_N = Translation2d(11.4, 4.5) stage_transition_S = Translation2d(11.4, 3.74)