From 9856962552eb17cba93c90469136d85135118d01 Mon Sep 17 00:00:00 2001
From: Chris Gerth <chrisgerth010592@gmail.com>
Date: Fri, 27 Sep 2024 21:44:41 -0500
Subject: [PATCH] Transforming goals per alliance added

---
 drivetrain/controlStrategies/autoDrive.py | 5 +++--
 simgui.json                               | 1 +
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py
index 8cf5683..86be4e9 100644
--- a/drivetrain/controlStrategies/autoDrive.py
+++ b/drivetrain/controlStrategies/autoDrive.py
@@ -6,6 +6,7 @@
 from utils.singleton import Singleton
 from navigation.repulsorFieldPlanner import RepulsorFieldPlanner
 from drivetrain.drivetrainPhysical import MAX_DT_LINEAR_SPEED
+from utils.allianceTransformUtils import transform
 
 
 GOAL_PICKUP = Pose2d.fromFeet(40,5,Rotation2d.fromDegrees(0.0))
@@ -46,9 +47,9 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand
 
         # Handle command changes
         if(self._toPickup):
-            self._rfp.setGoal(GOAL_PICKUP)
+            self._rfp.setGoal(transform(GOAL_PICKUP))
         elif(self._toSpeaker):
-            self._rfp.setGoal(GOAL_SPEAKER)
+            self._rfp.setGoal(transform(GOAL_SPEAKER))
         elif(not self._toSpeaker and not self._toPickup):
             self._rfp.setGoal(None)
 
diff --git a/simgui.json b/simgui.json
index 42f09f4..a2e4f48 100644
--- a/simgui.json
+++ b/simgui.json
@@ -103,6 +103,7 @@
         "left": 150,
         "right": 2961,
         "top": 79,
+        "units": "meters",
         "width": 16.541748046875,
         "window": {
           "visible": true