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] 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: