Reset drive pose at start

Mechanical-Advantage · Dec 8, 2023 · 242ef4f · 242ef4f
1 parent 41219df
commit 242ef4f
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 2 deletions.
diff --git 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
@@ -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();