Drive modes

src/main/deploy/choreo/blueMoveForward1m.traj

@@ -0,0 +1,108 @@
+{
+ "name":"blueMoveForward1m",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":1.0, "y":6.0, "heading":0.0, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":2.5, "y":6.0, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"1 m", "val":1.0}, "y":{"exp":"6 m", "val":6.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":50, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"2.5 m", "val":2.5}, "y":{"exp":"6 m", "val":6.0}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.3429,
+    "y":0.2667
+   },
+   "backLeft":{
+    "x":-0.3429,
+    "y":0.2667
+   },
+   "mass":68.0388555,
+   "inertia":6.0,
+   "gearing":6.75,
+   "radius":0.0508,
+   "vmax":136.1356816555577,
+   "tmax":0.13,
+   "cof":1.2,
+   "bumper":{
+    "front":0.4064,
+    "side":0.4064,
+    "back":0.4064
+   },
+   "differentialTrackWidth":0.5588
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,2.4755],
+  "samples":[
+    {"t":0.0, "x":1.0, "y":6.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":1.01468, "ay":0.0, "alpha":0.0, "fx":[17.25935,17.25935,17.25935,17.25935], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.04951, "x":1.00124, "y":6.0, "heading":0.0, "vx":0.05024, "vy":0.0, "omega":0.0, "ax":1.01463, "ay":0.0, "alpha":0.0, "fx":[17.2586,17.2586,17.2586,17.2586], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.09902, "x":1.00497, "y":6.0, "heading":0.0, "vx":0.10047, "vy":0.0, "omega":0.0, "ax":1.01458, "ay":0.0, "alpha":0.0, "fx":[17.25776,17.25776,17.25776,17.25776], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.14853, "x":1.01119, "y":6.0, "heading":0.0, "vx":0.1507, "vy":0.0, "omega":0.0, "ax":1.01453, "ay":0.0, "alpha":0.0, "fx":[17.25682,17.25682,17.25682,17.25682], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.19804, "x":1.0199, "y":6.0, "heading":0.0, "vx":0.20093, "vy":0.0, "omega":0.0, "ax":1.01447, "ay":0.0, "alpha":0.0, "fx":[17.25577,17.25577,17.25577,17.25577], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.24755, "x":1.03109, "y":6.0, "heading":0.0, "vx":0.25116, "vy":0.0, "omega":0.0, "ax":1.0144, "ay":0.0, "alpha":0.0, "fx":[17.25458,17.25458,17.25458,17.25458], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.29706, "x":1.04477, "y":6.0, "heading":0.0, "vx":0.30138, "vy":0.0, "omega":0.0, "ax":1.01432, "ay":0.0, "alpha":0.0, "fx":[17.25321,17.25321,17.25321,17.25321], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.34657, "x":1.06093, "y":6.0, "heading":0.0, "vx":0.3516, "vy":0.0, "omega":0.0, "ax":1.01422, "ay":0.0, "alpha":0.0, "fx":[17.25164,17.25164,17.25164,17.25164], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.39608, "x":1.07958, "y":6.0, "heading":0.0, "vx":0.40181, "vy":0.0, "omega":0.0, "ax":1.01411, "ay":0.0, "alpha":0.0, "fx":[17.2498,17.2498,17.2498,17.2498], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.44559, "x":1.10072, "y":6.0, "heading":0.0, "vx":0.45202, "vy":0.0, "omega":0.0, "ax":1.01399, "ay":0.0, "alpha":0.0, "fx":[17.24763,17.24763,17.24763,17.24763], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.4951, "x":1.12434, "y":6.0, "heading":0.0, "vx":0.50223, "vy":0.0, "omega":0.0, "ax":1.01383, "ay":0.0, "alpha":0.0, "fx":[17.24502,17.24502,17.24502,17.24502], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.54461, "x":1.15045, "y":6.0, "heading":0.0, "vx":0.55242, "vy":0.0, "omega":0.0, "ax":1.01365, "ay":0.0, "alpha":0.0, "fx":[17.24183,17.24183,17.24183,17.24183], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.59412, "x":1.17904, "y":6.0, "heading":0.0, "vx":0.60261, "vy":0.0, "omega":0.0, "ax":1.01341, "ay":0.0, "alpha":0.0, "fx":[17.23784,17.23784,17.23784,17.23784], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.64363, "x":1.21012, "y":6.0, "heading":0.0, "vx":0.65278, "vy":0.0, "omega":0.0, "ax":1.01311, "ay":0.0, "alpha":0.0, "fx":[17.2327,17.2327,17.2327,17.2327], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.69314, "x":1.24368, "y":6.0, "heading":0.0, "vx":0.70294, "vy":0.0, "omega":0.0, "ax":1.01271, "ay":0.0, "alpha":0.0, "fx":[17.22585,17.22585,17.22585,17.22585], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.74265, "x":1.27972, "y":6.0, "heading":0.0, "vx":0.75308, "vy":0.0, "omega":0.0, "ax":1.01214, "ay":0.0, "alpha":0.0, "fx":[17.21625,17.21625,17.21625,17.21625], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.79216, "x":1.31825, "y":6.0, "heading":0.0, "vx":0.80319, "vy":0.0, "omega":0.0, "ax":1.01129, "ay":0.0, "alpha":0.0, "fx":[17.20183,17.20183,17.20183,17.20183], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.84167, "x":1.35925, "y":6.0, "heading":0.0, "vx":0.85326, "vy":0.0, "omega":0.0, "ax":1.00988, "ay":0.0, "alpha":0.0, "fx":[17.17776,17.17776,17.17776,17.17776], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.89118, "x":1.40274, "y":6.0, "heading":0.0, "vx":0.90326, "vy":0.0, "omega":0.0, "ax":1.00705, "ay":0.0, "alpha":0.0, "fx":[17.12958,17.12958,17.12958,17.12958], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.94069, "x":1.44869, "y":6.0, "heading":0.0, "vx":0.95312, "vy":0.0, "omega":0.0, "ax":0.99857, "ay":0.0, "alpha":0.0, "fx":[16.98541,16.98541,16.98541,16.98541], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.9902, "x":1.4971, "y":6.0, "heading":0.0, "vx":1.00256, "vy":0.0, "omega":0.0, "ax":0.42717, "ay":0.0, "alpha":0.0, "fx":[7.26605,7.26605,7.26605,7.26605], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.03971, "x":1.54726, "y":6.0, "heading":0.0, "vx":1.0237, "vy":0.0, "omega":0.0, "ax":0.00029, "ay":0.0, "alpha":0.0, "fx":[0.00487,0.00487,0.00487,0.00487], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.08922, "x":1.59795, "y":6.0, "heading":0.0, "vx":1.02372, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.13873, "x":1.64863, "y":6.0, "heading":0.0, "vx":1.02372, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.18824, "x":1.69932, "y":6.0, "heading":0.0, "vx":1.02372, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.23775, "x":1.75, "y":6.0, "heading":0.0, "vx":1.02372, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.28726, "x":1.80068, "y":6.0, "heading":0.0, "vx":1.02372, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.33677, "x":1.85137, "y":6.0, "heading":0.0, "vx":1.02372, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.38628, "x":1.90205, "y":6.0, "heading":0.0, "vx":1.02372, "vy":0.0, "omega":0.0, "ax":-0.00029, "ay":0.0, "alpha":0.0, "fx":[-0.00487,-0.00487,-0.00487,-0.00487], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.43579, "x":1.95274, "y":6.0, "heading":0.0, "vx":1.0237, "vy":0.0, "omega":0.0, "ax":-0.42717, "ay":0.0, "alpha":0.0, "fx":[-7.26605,-7.26605,-7.26605,-7.26605], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.4853, "x":2.0029, "y":6.0, "heading":0.0, "vx":1.00256, "vy":0.0, "omega":0.0, "ax":-0.99857, "ay":0.0, "alpha":0.0, "fx":[-16.98541,-16.98541,-16.98541,-16.98541], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.53481, "x":2.05131, "y":6.0, "heading":0.0, "vx":0.95312, "vy":0.0, "omega":0.0, "ax":-1.00705, "ay":0.0, "alpha":0.0, "fx":[-17.12958,-17.12958,-17.12958,-17.12958], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.58432, "x":2.09726, "y":6.0, "heading":0.0, "vx":0.90326, "vy":0.0, "omega":0.0, "ax":-1.00988, "ay":0.0, "alpha":0.0, "fx":[-17.17776,-17.17776,-17.17776,-17.17776], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.63383, "x":2.14075, "y":6.0, "heading":0.0, "vx":0.85326, "vy":0.0, "omega":0.0, "ax":-1.01129, "ay":0.0, "alpha":0.0, "fx":[-17.20183,-17.20183,-17.20183,-17.20183], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.68334, "x":2.18175, "y":6.0, "heading":0.0, "vx":0.80319, "vy":0.0, "omega":0.0, "ax":-1.01214, "ay":0.0, "alpha":0.0, "fx":[-17.21625,-17.21625,-17.21625,-17.21625], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.73285, "x":2.22028, "y":6.0, "heading":0.0, "vx":0.75308, "vy":0.0, "omega":0.0, "ax":-1.01271, "ay":0.0, "alpha":0.0, "fx":[-17.22585,-17.22585,-17.22585,-17.22585], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.78236, "x":2.25632, "y":6.0, "heading":0.0, "vx":0.70294, "vy":0.0, "omega":0.0, "ax":-1.01311, "ay":0.0, "alpha":0.0, "fx":[-17.2327,-17.2327,-17.2327,-17.2327], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.83187, "x":2.28988, "y":6.0, "heading":0.0, "vx":0.65278, "vy":0.0, "omega":0.0, "ax":-1.01341, "ay":0.0, "alpha":0.0, "fx":[-17.23784,-17.23784,-17.23784,-17.23784], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.88138, "x":2.32096, "y":6.0, "heading":0.0, "vx":0.60261, "vy":0.0, "omega":0.0, "ax":-1.01365, "ay":0.0, "alpha":0.0, "fx":[-17.24183,-17.24183,-17.24183,-17.24183], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.93089, "x":2.34955, "y":6.0, "heading":0.0, "vx":0.55242, "vy":0.0, "omega":0.0, "ax":-1.01383, "ay":0.0, "alpha":0.0, "fx":[-17.24502,-17.24502,-17.24502,-17.24502], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":1.9804, "x":2.37566, "y":6.0, "heading":0.0, "vx":0.50223, "vy":0.0, "omega":0.0, "ax":-1.01399, "ay":0.0, "alpha":0.0, "fx":[-17.24763,-17.24763,-17.24763,-17.24763], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.02991, "x":2.39928, "y":6.0, "heading":0.0, "vx":0.45202, "vy":0.0, "omega":0.0, "ax":-1.01411, "ay":0.0, "alpha":0.0, "fx":[-17.2498,-17.2498,-17.2498,-17.2498], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.07942, "x":2.42042, "y":6.0, "heading":0.0, "vx":0.40181, "vy":0.0, "omega":0.0, "ax":-1.01422, "ay":0.0, "alpha":0.0, "fx":[-17.25164,-17.25164,-17.25164,-17.25164], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.12893, "x":2.43907, "y":6.0, "heading":0.0, "vx":0.3516, "vy":0.0, "omega":0.0, "ax":-1.01432, "ay":0.0, "alpha":0.0, "fx":[-17.25321,-17.25321,-17.25321,-17.25321], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.17844, "x":2.45523, "y":6.0, "heading":0.0, "vx":0.30138, "vy":0.0, "omega":0.0, "ax":-1.0144, "ay":0.0, "alpha":0.0, "fx":[-17.25458,-17.25458,-17.25458,-17.25458], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.22795, "x":2.46891, "y":6.0, "heading":0.0, "vx":0.25116, "vy":0.0, "omega":0.0, "ax":-1.01447, "ay":0.0, "alpha":0.0, "fx":[-17.25577,-17.25577,-17.25577,-17.25577], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.27746, "x":2.4801, "y":6.0, "heading":0.0, "vx":0.20093, "vy":0.0, "omega":0.0, "ax":-1.01453, "ay":0.0, "alpha":0.0, "fx":[-17.25682,-17.25682,-17.25682,-17.25682], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.32697, "x":2.48881, "y":6.0, "heading":0.0, "vx":0.1507, "vy":0.0, "omega":0.0, "ax":-1.01458, "ay":0.0, "alpha":0.0, "fx":[-17.25776,-17.25776,-17.25776,-17.25776], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.37648, "x":2.49503, "y":6.0, "heading":0.0, "vx":0.10047, "vy":0.0, "omega":0.0, "ax":-1.01463, "ay":0.0, "alpha":0.0, "fx":[-17.2586,-17.2586,-17.2586,-17.2586], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.42599, "x":2.49876, "y":6.0, "heading":0.0, "vx":0.05024, "vy":0.0, "omega":0.0, "ax":-1.01468, "ay":0.0, "alpha":0.0, "fx":[-17.25935,-17.25935,-17.25935,-17.25935], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":2.4755, "x":2.5, "y":6.0, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}

src/main/java/frc/lib/AutoSelectorIO.java

@@ -20,7 +20,7 @@ public static class AutoSelectorIOInputs {
   }
 
   public void updateInputs(AutoSelectorIOInputs inputs) {
-    inputs.autoSwitchPosition = getRotarySwitchPosition();
+    inputs.autoSwitchPosition = getBinarySwitchPosition();
   }
 
   /**

src/main/java/frc/robot/Constants.java

@@ -15,6 +15,7 @@
 
 import static edu.wpi.first.units.Units.*;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj.RobotBase;
 
@@ -75,4 +76,11 @@ public static final class OIConstants {
     public static final int kDefaultDriverPort = 0;
     public static final int kDefaultOperatorPort = 1;
   }
+
+  public static final class FieldConstants {
+    public static final Translation2d kBlueHubCenter =
+        new Translation2d(Inches.of(181.56), Inches.of(158.32));
+    public static final Translation2d kRedHubCenter =
+        new Translation2d(Inches.of(650.12 - 181.56), Inches.of(158.32));
+  }
 }

src/main/java/frc/robot/Robot.java

@@ -19,6 +19,8 @@
 import frc.lib.ControllerSelector.ControllerFunction;
 import frc.lib.ControllerSelector.ControllerType;
 import frc.robot.Constants.AutoConstants;
+import frc.robot.Constants.FieldConstants;
+import frc.robot.auto.B_MoveForward1M;
 import frc.robot.auto.B_Path;
 import frc.robot.auto.R_MoveAndRotate;
 import frc.robot.auto.R_MoveStraight;
@@ -118,15 +120,15 @@ public Robot() {
         vision =
             new Vision(
                 drive::addVisionMeasurement,
-                drive::getOdometryPose,
+                drive::getVisionPose,
                 new VisionIOPhotonVisionSim(
-                    cameraFrontRightName, robotToFrontRightCamera, drive::getOdometryPose),
+                    cameraFrontRightName, robotToFrontRightCamera, drive::getVisionPose),
                 new VisionIOPhotonVisionSim(
-                    cameraFrontLeftName, robotToFrontLeftCamera, drive::getOdometryPose),
+                    cameraFrontLeftName, robotToFrontLeftCamera, drive::getVisionPose),
                 new VisionIOPhotonVisionSim(
-                    cameraBackRightName, robotToBackRightCamera, drive::getOdometryPose),
+                    cameraBackRightName, robotToBackRightCamera, drive::getVisionPose),
                 new VisionIOPhotonVisionSim(
-                    cameraBackLeftName, robotToBackLeftCamera, drive::getOdometryPose));
+                    cameraBackLeftName, robotToBackLeftCamera, drive::getVisionPose));
         break;
 
       case REPLAY: // Replaying a log
@@ -148,7 +150,7 @@ public Robot() {
         vision =
             new Vision(
                 drive::addVisionMeasurement,
-                drive::getOdometryPose,
+                drive::getVisionPose,
                 new VisionIO() {},
                 new VisionIO() {},
                 new VisionIO() {},
@@ -326,12 +328,37 @@ public void bindXboxDriver(int port) {
                     drive)
                 .ignoringDisable(true));
 
-    // Point at target while A button is held
+    // Point at Hub while A button is held
     xboxDriver
         .a()
         .whileTrue(
-            DriveCommands.pointAtTarget(
-                drive, () -> vision.getTargetX(0), allianceSelector::fieldRotated));
+            DriveCommands.joystickDriveAtFixedOrientation(
+                drive,
+                () -> -xboxDriver.getLeftY(),
+                () -> -xboxDriver.getLeftX(),
+                // TODO: Point at the hub of the correct alliance color
+                () ->
+                    FieldConstants.kBlueHubCenter
+                        .minus(drive.getVisionPose().getTranslation())
+                        .getAngle(),
+                allianceSelector::fieldRotated));
+
+    // Point in the direction of the commanded translation while Y button is held
+    xboxDriver
+        .y()
+        .whileTrue(
+            DriveCommands.joystickDrivePointedForward(
+                drive,
+                () -> -xboxDriver.getLeftY(),
+                () -> -xboxDriver.getLeftX(),
+                allianceSelector::fieldRotated));
+
+    // Point at vision target while A button is held
+    // xboxDriver
+    //     .a()
+    //     .whileTrue(
+    //         DriveCommands.pointAtTarget(
+    //             drive, () -> vision.getTargetX(0), allianceSelector::fieldRotated));
 
     // Drive 1m forward while A button is held
     // xboxDriver.a().whileTrue(PathCommands.advanceForward(drive, Meters.of(1)));
@@ -358,6 +385,7 @@ public void bindXboxOperator(int port) {
   }
 
   public void configureAutoOptions() {
+    autoSelector.addAuto(new AutoOption(Alliance.Blue, 1, new B_MoveForward1M(drive)));
     autoSelector.addAuto(new AutoOption(Alliance.Red, 1, new R_MoveStraight(drive)));
     autoSelector.addAuto(new AutoOption(Alliance.Red, 2, new R_MoveAndRotate(drive)));
     autoSelector.addAuto(new AutoOption(Alliance.Blue, 3, new B_Path(drive)));

src/main/java/frc/robot/auto/AutoMode.java

@@ -14,7 +14,7 @@ public abstract class AutoMode {
   public AutoMode(Drive drivetrain) {
     autoFactory =
         new AutoFactory(
-            drivetrain::getOdometryPose,
+            drivetrain::getVisionPose,
             drivetrain::setOdometryPose,
             drivetrain::followTrajectory,
             false,

src/main/java/frc/robot/auto/B_MoveForward1M.java

@@ -0,0 +1,43 @@
+package frc.robot.auto;
+
+import choreo.auto.AutoRoutine;
+import choreo.auto.AutoTrajectory;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.drive.Drive;
+
+public class B_MoveForward1M extends AutoMode {
+
+  public B_MoveForward1M(Drive drivetrain) {
+    super(drivetrain);
+  }
+
+  // Define routine
+  AutoRoutine routine = super.getAutoFactory().newRoutine("bluemoveforward1m");
+
+  // Load the routine's trajectories
+  AutoTrajectory trajectory = routine.trajectory("blueMoveForward1m");
+
+  @Override
+  public String getName() {
+    return "bluemoveforward1m";
+  }
+
+  @Override
+  public AutoTrajectory getInitialTrajectory() {
+    return trajectory;
+  }
+
+  @Override
+  public AutoRoutine getAutoRoutine() {
+
+    // spotless:off
+        // When the routine begins, reset odometry and start the first trajectory
+        routine.active().onTrue(
+            Commands.sequence(
+                trajectory.resetOdometry(), 
+                trajectory.cmd()));
+        // spotless:on
+
+    return routine;
+  }
+}