From c02b8b0a3c47ec5a4096003bc309061193dfdc17 Mon Sep 17 00:00:00 2001
From: Bradley Bickford <bradleybickford@gmail.com>
Date: Thu, 7 Mar 2024 20:36:35 -0500
Subject: [PATCH] Final changes that make field oriented work post auto

---
 .../paths/From Subwoofer Far Edge to Amp.path   |  8 ++++----
 src/main/java/frc/robot/RobotContainer.java     | 17 ++++++++++++++++-
 2 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/src/main/deploy/pathplanner/paths/From Subwoofer Far Edge to Amp.path b/src/main/deploy/pathplanner/paths/From Subwoofer Far Edge to Amp.path
index 3f728df..810cee1 100644
--- a/src/main/deploy/pathplanner/paths/From Subwoofer Far Edge to Amp.path	
+++ b/src/main/deploy/pathplanner/paths/From Subwoofer Far Edge to Amp.path	
@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.4304615881959033,
-        "y": 3.1531161244140673
+        "x": 2.4368920788482864,
+        "y": 3.1622483921028106
       },
       "isLocked": false,
       "linkedName": null
@@ -20,8 +20,8 @@
         "y": 7.565612949407717
       },
       "prevControl": {
-        "x": 1.7961068649410026,
-        "y": 6.511464659292955
+        "x": 1.8176568333482528,
+        "y": 6.336387277909188
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 97bbe47..c7fff27 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -5,8 +5,13 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPlannerTrajectory;
+import com.pathplanner.lib.util.PathPlannerLogging;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -78,9 +83,19 @@ public class RobotContainer {
         tab.add("Drivetrain Pose", poseSendable)
             .withPosition(0, 0)
             .withSize(2, 3);
+
+        tab.add("Auto Selector", autoChooser)
+            .withPosition(2, 0)
+            .withSize(2, 1);
+
+        tab.addDouble("Current Pose Rotation", () -> { return drivetrain.getPose().getRotation().getDegrees(); })
+            .withPosition(2, 1)
+            .withSize(2, 1)
+            .withWidget(BuiltInWidgets.kTextView);
     }
 
     public Command getAutonomousCommand() {
-        return autoChooser.getSelected();
+        return drivetrain.zeroHeadingCommand().andThen(autoChooser.getSelected());
+        
     }
 }