From 614a8f451704aea22b548b17c10ea79ab7ec9ee0 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Thu, 14 Mar 2024 13:41:40 +1100 Subject: [PATCH 1/4] Remove redundant "internal" from naming --- autonomous/autonomous.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index 396784a4..e9c39f18 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -17,7 +17,7 @@ def rotation_to_red_speaker(position: Translation2d) -> Rotation2d: class PodiumSpeakerAmpTopcentre(AutoBase): - MODE_NAME = "5 notes: internal, podium, speaker, amp, top centre" + MODE_NAME = "5 notes: podium, speaker, amp, centre 1" def __init__(self) -> None: note_paths = [ @@ -36,7 +36,7 @@ def __init__(self) -> None: class PodiumSpeakerAmp(AutoBase): - MODE_NAME = "4 notes: internal, podium, speaker, amp" + MODE_NAME = "4 notes: podium, speaker, amp" def __init__(self) -> None: note_paths = [ @@ -59,7 +59,7 @@ def __init__(self) -> None: class AmpCentre1(AutoBase): # The "top" or north notes of the field - MODE_NAME = "3 notes: internal, amp, centre 1" + MODE_NAME = "3 notes: amp, centre 1" def __init__(self) -> None: note_paths = [ @@ -80,7 +80,7 @@ def __init__(self) -> None: class SpeakerCentre3(AutoBase): # Moving through the stage - MODE_NAME = "3 notes: internal, speaker, centre 3" + MODE_NAME = "3 notes: speaker, centre 3" def __init__(self) -> None: note_paths = [ @@ -107,7 +107,7 @@ def __init__(self) -> None: class Centre3Centre5(AutoBase): # Stay in the south of the field to avoid interfering with allies using the close notes - MODE_NAME = "3 notes: internal, center 3, center 5" + MODE_NAME = "3 notes: center 3, center 5" # Not yet tested DISABLED = True From 10018a1b6e25a3c90fba102bceb071871a3ad916 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Thu, 14 Mar 2024 13:48:08 +1100 Subject: [PATCH 2/4] Update sim start positions --- autonomous/autonomous.py | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index e9c39f18..d6d92d1a 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -5,6 +5,7 @@ Path, ShootingPositions, PathPositions, + TeamPoses, ) from utilities import game from autonomous.base import AutoBase @@ -16,7 +17,7 @@ def rotation_to_red_speaker(position: Translation2d) -> Rotation2d: return t.angle() + Rotation2d(math.pi) -class PodiumSpeakerAmpTopcentre(AutoBase): +class PodiumSpeakerAmpCentre1(AutoBase): MODE_NAME = "5 notes: podium, speaker, amp, centre 1" def __init__(self) -> None: @@ -50,9 +51,10 @@ def __init__(self) -> None: Path([NotePositions.amp], face_target=True), ] # Start pose only needs to be on the correct half of the field, - # so choose the podium as a reference point + # so choose the subwoofer start_pose = Pose2d( - NotePositions.podium, rotation_to_red_speaker(NotePositions.podium) + TeamPoses.RED_TEST_POSE.translation(), + rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()), ) super().__init__(note_paths, shoot_paths, start_pose) @@ -71,9 +73,10 @@ def __init__(self) -> None: Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True), ] # Start pose only needs to be on the correct half of the field, - # so choose the amp as a reference point + # so choose the subwoofer start_pose = Pose2d( - NotePositions.amp, rotation_to_red_speaker(NotePositions.amp) + TeamPoses.RED_TEST_POSE.translation(), + rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()), ) super().__init__(note_paths, shoot_paths, start_pose) @@ -98,9 +101,10 @@ def __init__(self) -> None: ), ] # Start pose only needs to be on the correct half of the field, - # so choose the speaker as a reference point + # so choose the subwoofer start_pose = Pose2d( - NotePositions.speaker, rotation_to_red_speaker(NotePositions.speaker) + TeamPoses.RED_TEST_POSE.translation(), + rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()), ) super().__init__(note_paths, shoot_paths, start_pose) From d09dd78367a28d8194c2dd7fb01df6d9faf37c25 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Thu, 14 Mar 2024 13:57:57 +1100 Subject: [PATCH 3/4] Add extra note versions of our autonomous routines --- autonomous/autonomous.py | 112 +++++++++++++++++++++++++++++++++++++-- utilities/position.py | 1 + 2 files changed, 110 insertions(+), 3 deletions(-) diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index d6d92d1a..f24b7a03 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -81,6 +81,30 @@ def __init__(self) -> None: super().__init__(note_paths, shoot_paths, start_pose) +class AmpCentre1Centre2(AutoBase): + # The "top" or north notes of the field + MODE_NAME = "4 notes: amp, centre 1, centre 2" + + def __init__(self) -> None: + note_paths = [ + Path([NotePositions.amp], face_target=False), + Path([PathPositions.avoid_wall, NotePositions.Centre1], face_target=False), + Path([PathPositions.avoid_wall, NotePositions.Centre2], face_target=False), + ] + shoot_paths = [ + Path([NotePositions.amp], face_target=True), + Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True), + Path([PathPositions.avoid_wall, NotePositions.amp], face_target=True), + ] + # Start pose only needs to be on the correct half of the field, + # so choose the subwoofer + start_pose = Pose2d( + TeamPoses.RED_TEST_POSE.translation(), + rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()), + ) + super().__init__(note_paths, shoot_paths, start_pose) + + class SpeakerCentre3(AutoBase): # Moving through the stage MODE_NAME = "3 notes: speaker, centre 3" @@ -109,11 +133,79 @@ def __init__(self) -> None: super().__init__(note_paths, shoot_paths, start_pose) +class SpeakerCentre3Centre4(AutoBase): + # Moving through the stage + MODE_NAME = "4 notes: speaker, centre 3, centre 4" + + def __init__(self) -> None: + note_paths = [ + Path([NotePositions.speaker], face_target=False), + Path( + [PathPositions.stage_transition_N, NotePositions.Centre3], + face_target=False, + ), + Path( + [PathPositions.stage_transition_N, NotePositions.Centre4], + face_target=False, + ), + ] + shoot_paths = [ + Path([NotePositions.speaker], face_target=True), + Path( + [PathPositions.stage_transition_N, NotePositions.speaker], + face_target=True, + ), + Path( + [PathPositions.stage_transition_N, NotePositions.speaker], + face_target=True, + ), + ] + # Start pose only needs to be on the correct half of the field, + # so choose the subwoofer + start_pose = Pose2d( + TeamPoses.RED_TEST_POSE.translation(), + rotation_to_red_speaker(TeamPoses.RED_TEST_POSE.translation()), + ) + super().__init__(note_paths, shoot_paths, start_pose) + + class Centre3Centre5(AutoBase): # Stay in the south of the field to avoid interfering with allies using the close notes - MODE_NAME = "3 notes: center 3, center 5" - # Not yet tested - DISABLED = True + MODE_NAME = "3 notes: centre 3, centre 5" + + def __init__(self) -> None: + note_paths = [ + Path( + [ + PathPositions.stage_transition_S_entry, + PathPositions.stage_transition_S, + NotePositions.Centre3, + ], + face_target=False, + ), + Path([NotePositions.Centre5], face_target=False), + ] + + shoot_paths = [ + Path( + [ + PathPositions.stage_transition_S, + PathPositions.stage_transition_S_entry, + ShootingPositions.source_side, + ], + face_target=True, + ), + Path([ShootingPositions.source_side], face_target=True), + ] + sim_start_pos = Translation2d(15.4, 2.94) + 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) + + +class Centre3Centre4Centre5(AutoBase): + # Stay in the south of the field to avoid interfering with allies using the close notes + MODE_NAME = "4 notes: centre 3, centre 4, centre 5" def __init__(self) -> None: note_paths = [ @@ -125,6 +217,13 @@ def __init__(self) -> None: ], face_target=False, ), + Path( + [ + PathPositions.avoid_stage_S, + NotePositions.Centre4, + ], + face_target=False, + ), Path([NotePositions.Centre5], face_target=False), ] @@ -137,6 +236,13 @@ def __init__(self) -> None: ], face_target=True, ), + Path( + [ + PathPositions.avoid_stage_S, + ShootingPositions.source_side, + ], + face_target=True, + ), Path([ShootingPositions.source_side], face_target=True), ] sim_start_pos = Translation2d(15.4, 2.94) diff --git a/utilities/position.py b/utilities/position.py index fa97cc9b..030fcfe1 100644 --- a/utilities/position.py +++ b/utilities/position.py @@ -92,3 +92,4 @@ class PathPositions: stage_transition_S = Translation2d(11.4, 3.74) stage_transition_S_entry = Translation2d(13.0, 2.5) avoid_wall = Translation2d(10.80, 6.55) + avoid_stage_S = Translation2d(10.66, 1.40) From 60f7f59a7038bd504dc53a119298ad6bc0f72940 Mon Sep 17 00:00:00 2001 From: Michael Kennedy <34204085+mlists@users.noreply.github.com> Date: Thu, 14 Mar 2024 14:02:50 +1100 Subject: [PATCH 4/4] Reorder and reroute our southern autonomous paths --- autonomous/autonomous.py | 40 +++++++++++++++++++--------------------- 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/autonomous/autonomous.py b/autonomous/autonomous.py index f24b7a03..746e6abf 100644 --- a/autonomous/autonomous.py +++ b/autonomous/autonomous.py @@ -169,33 +169,31 @@ def __init__(self) -> None: super().__init__(note_paths, shoot_paths, start_pose) -class Centre3Centre5(AutoBase): +class Centre5Centre4(AutoBase): # Stay in the south of the field to avoid interfering with allies using the close notes - MODE_NAME = "3 notes: centre 3, centre 5" + MODE_NAME = "3 notes: centre 5, centre 4" def __init__(self) -> None: note_paths = [ + Path([NotePositions.Centre5], face_target=False), Path( [ - PathPositions.stage_transition_S_entry, - PathPositions.stage_transition_S, - NotePositions.Centre3, + PathPositions.avoid_stage_S, + NotePositions.Centre4, ], face_target=False, ), - Path([NotePositions.Centre5], face_target=False), ] shoot_paths = [ + Path([ShootingPositions.source_side], face_target=True), Path( [ - PathPositions.stage_transition_S, - PathPositions.stage_transition_S_entry, + PathPositions.avoid_stage_S, ShootingPositions.source_side, ], face_target=True, ), - Path([ShootingPositions.source_side], face_target=True), ] sim_start_pos = Translation2d(15.4, 2.94) rotation = rotation_to_red_speaker(sim_start_pos) @@ -203,47 +201,47 @@ def __init__(self) -> None: super().__init__(note_paths, shoot_paths, sim_start_pose) -class Centre3Centre4Centre5(AutoBase): +class Centre5Centre4Centre3(AutoBase): # Stay in the south of the field to avoid interfering with allies using the close notes - MODE_NAME = "4 notes: centre 3, centre 4, centre 5" + MODE_NAME = "4 notes: centre 5, centre 4, centre 3" def __init__(self) -> None: note_paths = [ + Path([NotePositions.Centre5], face_target=False), Path( [ - PathPositions.stage_transition_S_entry, - PathPositions.stage_transition_S, - NotePositions.Centre3, + PathPositions.avoid_stage_S, + NotePositions.Centre4, ], face_target=False, ), Path( [ - PathPositions.avoid_stage_S, - NotePositions.Centre4, + PathPositions.stage_transition_S_entry, + PathPositions.stage_transition_S, + NotePositions.Centre3, ], face_target=False, ), - Path([NotePositions.Centre5], face_target=False), ] shoot_paths = [ + Path([ShootingPositions.source_side], face_target=True), Path( [ - PathPositions.stage_transition_S, - PathPositions.stage_transition_S_entry, + PathPositions.avoid_stage_S, ShootingPositions.source_side, ], face_target=True, ), Path( [ - PathPositions.avoid_stage_S, + PathPositions.stage_transition_S, + PathPositions.stage_transition_S_entry, ShootingPositions.source_side, ], face_target=True, ), - Path([ShootingPositions.source_side], face_target=True), ] sim_start_pos = Translation2d(15.4, 2.94) rotation = rotation_to_red_speaker(sim_start_pos)