From 6c600c997fdd5e71ad530c57dd042b70a211ff65 Mon Sep 17 00:00:00 2001 From: David Vo Date: Sun, 10 Mar 2024 22:12:14 +1100 Subject: [PATCH] autonomous: Add get_starting_pose() method --- autonomous/base.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/autonomous/base.py b/autonomous/base.py index 07409947..6063fd1b 100644 --- a/autonomous/base.py +++ b/autonomous/base.py @@ -75,10 +75,8 @@ def setup(self) -> None: def on_enable(self): # Setup starting position in the simulator - if RobotBase.isSimulation() and self.starting_pose is not None: - starting_pose = self.starting_pose - if not game.is_red(): - starting_pose = game.field_flip_pose2d(self.starting_pose) + starting_pose = self.get_starting_pose() + if RobotBase.isSimulation() and starting_pose is not None: self.chassis.set_pose(starting_pose) super().on_enable() @@ -91,6 +89,14 @@ def is_close_enough_to_shoot(self) -> bool: ).norm() < self.SHOOTING_POSITION_TOLERANCE return False + def get_starting_pose(self) -> Pose2d | None: + starting_pose = self.starting_pose + if starting_pose is None: + return None + if not game.is_red(): + starting_pose = game.field_flip_pose2d(starting_pose) + return starting_pose + @state(first=True) def initialise(self) -> None: # Make a working copy of the NotePaths so that we can pop