From 242ef4ffce8ab156129d5d4231eeb98137c444d3 Mon Sep 17 00:00:00 2001
From: manthan-acharya <manthan.boston@gmail.com>
Date: Thu, 7 Dec 2023 19:55:53 -0500
Subject: [PATCH] Reset drive pose at start

---
 .../java/org/littletonrobotics/frc2023/RobotContainer.java    | 4 ++--
 .../frc2023/commands/DriveTrajectoryNew.java                  | 1 +
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/java/org/littletonrobotics/frc2023/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2023/RobotContainer.java
index 7d58e47..98f6c2b 100644
--- a/src/main/java/org/littletonrobotics/frc2023/RobotContainer.java
+++ b/src/main/java/org/littletonrobotics/frc2023/RobotContainer.java
@@ -153,7 +153,7 @@ public RobotContainer() {
             (Double voltage) -> drive.runCharacterizationVolts(voltage),
             drive::getCharacterizationVelocity));
     // only add to auto map if there
-    Optional<File> trajectoryFile = ChoreoTrajectoryFactory.getTrajectoryFile("OnePieceBalance");
+    Optional<File> trajectoryFile = ChoreoTrajectoryFactory.getTrajectoryFile("NewPath");
     trajectoryFile.ifPresent(
         file ->
             autoChooser.addOption(
@@ -280,6 +280,6 @@ public void bindControls() {
   }
 
   public Command getAutonomousCommand() {
-    return Commands.none();
+    return autoChooser.get();
   }
 }
diff --git a/src/main/java/org/littletonrobotics/frc2023/commands/DriveTrajectoryNew.java b/src/main/java/org/littletonrobotics/frc2023/commands/DriveTrajectoryNew.java
index aec4b1c..0aeee95 100644
--- a/src/main/java/org/littletonrobotics/frc2023/commands/DriveTrajectoryNew.java
+++ b/src/main/java/org/littletonrobotics/frc2023/commands/DriveTrajectoryNew.java
@@ -94,6 +94,7 @@ public void initialize() {
     Logger.getInstance().recordOutput("Odometry/Trajectory", trajectory.getTrajectoryPoses());
 
     // Reset all controllers
+    drive.setPose(trajectory.getTrajectoryPoses()[0]);
     timer.reset();
     timer.start();
     xController.reset();