From 075c91a0ced65ba3323976c7394895af1eccd851 Mon Sep 17 00:00:00 2001 From: Max Worrall <96847801+Tsunami014@users.noreply.github.com> Date: Fri, 15 Mar 2024 15:18:42 +1100 Subject: [PATCH 1/4] Updated south notes --- autonomous/autonomous.py | 11 ++++++++--- utilities/position.py | 2 +- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index 746e6abf..d4d92140 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -207,7 +207,9 @@ class Centre5Centre4Centre3(AutoBase): def __init__(self) -> None: note_paths = [ - Path([NotePositions.Centre5], face_target=False), + Path( + [PathPositions.avoid_stage_S, NotePositions.Centre5], face_target=False + ), Path( [ PathPositions.avoid_stage_S, @@ -226,7 +228,10 @@ def __init__(self) -> None: ] shoot_paths = [ - Path([ShootingPositions.source_side], face_target=True), + Path( + [PathPositions.avoid_stage_S, ShootingPositions.source_side], + face_target=True, + ), Path( [ PathPositions.avoid_stage_S, @@ -243,7 +248,7 @@ def __init__(self) -> None: face_target=True, ), ] - sim_start_pos = Translation2d(15.4, 2.94) + sim_start_pos = ShootingPositions.source_side rotation = rotation_to_red_speaker(sim_start_pos) sim_start_pose = Pose2d(sim_start_pos, rotation) super().__init__(note_paths, shoot_paths, sim_start_pose) diff --git a/utilities/position.py b/utilities/position.py index 030fcfe1..1501a1b8 100644 --- a/utilities/position.py +++ b/utilities/position.py @@ -66,7 +66,7 @@ class ShootingPositions: amp_speaker_bounce = Translation2d( 14.7, (NotePositions.amp.y + NotePositions.speaker.y) / 2 ) - source_side = Translation2d(14.7, 2.8) + source_side = Translation2d(15.556, 4.034) class TeamPoses: From 94a2f6ccc34e3e70569513c096dbe1eef1a10fbf Mon Sep 17 00:00:00 2001 From: Max Worrall <96847801+Tsunami014@users.noreply.github.com> Date: Fri, 15 Mar 2024 15:21:12 +1100 Subject: [PATCH 2/4] Fixed centre5&4 note auto --- autonomous/autonomous.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index d4d92140..807eac81 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -175,7 +175,9 @@ class Centre5Centre4(AutoBase): def __init__(self) -> None: note_paths = [ - Path([NotePositions.Centre5], face_target=False), + Path( + [PathPositions.avoid_stage_S, NotePositions.Centre5], face_target=False + ), Path( [ PathPositions.avoid_stage_S, @@ -186,7 +188,10 @@ def __init__(self) -> None: ] shoot_paths = [ - Path([ShootingPositions.source_side], face_target=True), + Path( + [PathPositions.avoid_stage_S, ShootingPositions.source_side], + face_target=True, + ), Path( [ PathPositions.avoid_stage_S, @@ -195,7 +200,7 @@ def __init__(self) -> None: face_target=True, ), ] - sim_start_pos = Translation2d(15.4, 2.94) + sim_start_pos = ShootingPositions.source_side rotation = rotation_to_red_speaker(sim_start_pos) sim_start_pose = Pose2d(sim_start_pos, rotation) super().__init__(note_paths, shoot_paths, sim_start_pose) From ec2665f92dc2badf07567b5bb90588639ea9288e Mon Sep 17 00:00:00 2001 From: Max Worrall <96847801+Tsunami014@users.noreply.github.com> Date: Fri, 15 Mar 2024 15:32:08 +1100 Subject: [PATCH 3/4] Added safety point to help avoid bad starting positions --- autonomous/autonomous.py | 14 ++++++++++++-- utilities/position.py | 1 + 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index 807eac81..75e705d5 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -176,7 +176,12 @@ class Centre5Centre4(AutoBase): def __init__(self) -> None: note_paths = [ Path( - [PathPositions.avoid_stage_S, NotePositions.Centre5], face_target=False + [ + PathPositions.avoid_starting_faults, + PathPositions.avoid_stage_S, + NotePositions.Centre5, + ], + face_target=False, ), Path( [ @@ -213,7 +218,12 @@ class Centre5Centre4Centre3(AutoBase): def __init__(self) -> None: note_paths = [ Path( - [PathPositions.avoid_stage_S, NotePositions.Centre5], face_target=False + [ + PathPositions.avoid_starting_faults, + PathPositions.avoid_stage_S, + NotePositions.Centre5, + ], + face_target=False, ), Path( [ diff --git a/utilities/position.py b/utilities/position.py index 1501a1b8..ec687d5e 100644 --- a/utilities/position.py +++ b/utilities/position.py @@ -93,3 +93,4 @@ class PathPositions: stage_transition_S_entry = Translation2d(13.0, 2.5) avoid_wall = Translation2d(10.80, 6.55) avoid_stage_S = Translation2d(10.66, 1.40) + avoid_starting_faults = Translation2d(14.429, 2.946) From 6af90d1bf56c1f8d804645c9aba2b07c6b3f63d9 Mon Sep 17 00:00:00 2001 From: Max Worrall <96847801+Tsunami014@users.noreply.github.com> Date: Fri, 15 Mar 2024 15:53:41 +1100 Subject: [PATCH 4/4] Redid the check for intended start pos! --- robot.py | 22 ++++++++++++++-------- utilities/position.py | 11 ++--------- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/robot.py b/robot.py index 8b6c3112..fab311ed 100644 --- a/robot.py +++ b/robot.py @@ -22,7 +22,7 @@ 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, y_close_to_stage +from utilities.position import distance_between class MyRobot(magicbot.MagicRobot): @@ -47,6 +47,8 @@ class MyRobot(magicbot.MagicRobot): vision_port: VisualLocalizer vision_starboard: VisualLocalizer + START_POS_TOLERANCE = 1 + def createObjects(self) -> None: self.data_log = wpilib.DataLogManager.getLog() @@ -71,7 +73,7 @@ def createObjects(self) -> None: ) def teleopInit(self) -> None: - pass + self.field.getObject("Intended start pos").setPoses([]) def teleopPeriodic(self) -> None: if self.climber.should_lock_mechanisms(): @@ -204,6 +206,7 @@ def disabledPeriodic(self) -> None: if ( not self.vision_port.sees_target() and not self.vision_starboard.sees_target() + and not self.isSimulation() ): self.status_lights.no_vision() else: @@ -213,11 +216,14 @@ def disabledPeriodic(self) -> None: intended_start_pose = selected_auto.get_starting_pose() current_pose = self.chassis.get_pose() if intended_start_pose is not None: - if on_same_side_of_stage(intended_start_pose, current_pose): - if y_close_to_stage(current_pose): - self.status_lights.too_close_to_stage() - else: - self.status_lights.rainbow() + self.field.getObject("Intended start pos").setPose( + intended_start_pose + ) + if ( + distance_between(intended_start_pose, current_pose) + < self.START_POS_TOLERANCE + ): + self.status_lights.rainbow() else: self.status_lights.invalid_start() else: @@ -226,7 +232,7 @@ def disabledPeriodic(self) -> None: self.status_lights.missing_start_pose() def autonomousInit(self) -> None: - pass + self.field.getObject("Intended start pos").setPoses([]) if __name__ == "__main__": diff --git a/utilities/position.py b/utilities/position.py index ec687d5e..a3d3c2a0 100644 --- a/utilities/position.py +++ b/utilities/position.py @@ -76,15 +76,8 @@ class TeamPoses: RED_PODIUM = field_flip_pose2d(BLUE_PODIUM) -def on_same_side_of_stage(intended_start_pose: Pose2d, current_pose: Pose2d) -> bool: - return not ( - (intended_start_pose.y > TeamPoses.BLUE_PODIUM.y) - ^ (current_pose.y > TeamPoses.BLUE_PODIUM.y) - ) - - -def y_close_to_stage(pose: Pose2d) -> bool: - return abs(pose.y - TeamPoses.BLUE_PODIUM.y) < 0.9 +def distance_between(intended_start_pose: Pose2d, current_pose: Pose2d) -> float: + return (intended_start_pose.translation() - current_pose.translation()).norm() class PathPositions: