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