Add DriveMotionPlanner.java

- remove DriveTrajectory.java
Add static utility method to LoggedTunableNumber.java
Added to HolonomicDriveController.java
Add simple 4 piece for tomorrow

src/main/java/frc/robot/RobotContainer.java

@@ -22,10 +22,9 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.DriveCommands;
-import frc.robot.commands.DriveTrajectory;
 import frc.robot.commands.FeedForwardCharacterization;
+import frc.robot.commands.TestAutos;
 import frc.robot.subsystems.drive.*;
-import frc.robot.subsystems.kitbotshooter.*;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterIO;
 import frc.robot.subsystems.shooter.ShooterIOSim;
@@ -123,21 +122,23 @@ public RobotContainer() {
             .beforeStarting(() -> shooter.setCharacterizing(true))
             .finallyDo(() -> shooter.setCharacterizing(false)));
 
+    autoChooser.addOption("Three Spike Smooth", TestAutos.threeSpikeSmooth(drive));
+
     // Testing autos paths
     Function<File, Optional<Command>> trajectoryCommandFactory =
         trajectoryFile -> {
           Optional<Trajectory> trajectory = ChoreoTrajectoryReader.generate(trajectoryFile);
           return trajectory.map(
               traj ->
-                  Commands.sequence(
-                      Commands.runOnce(
-                          () ->
-                              robotState.resetPose(
-                                  AllianceFlipUtil.apply(traj.startPose()),
-                                  drive.getWheelPositions(),
-                                  drive.getGyroYaw()),
-                          drive),
-                      new DriveTrajectory(drive, traj)));
+                  Commands.runOnce(
+                      () -> {
+                        robotState.resetPose(
+                            AllianceFlipUtil.apply(traj.getStartPose()),
+                            drive.getWheelPositions(),
+                            drive.getGyroYaw());
+                        drive.getMotionPlanner().setTrajectory(traj);
+                      },
+                      drive));
         };
     final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo");
     for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) {
@@ -166,7 +167,7 @@ private void configureButtonBindings() {
             () -> -controller.getLeftY(),
             () -> -controller.getLeftX(),
             () -> -controller.getRightX()));
-    controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
+    controller.x().onTrue(Commands.runOnce(drive.getMotionPlanner()::requestEnableXMode, drive));
     controller
         .b()
         .onTrue(

src/main/java/frc/robot/commands/DriveCommands.java

@@ -60,12 +60,14 @@ public static Command joystickDrive(
                   .getTranslation();
 
           // Convert to field relative speeds & send command
-          drive.runVelocity(
-              ChassisSpeeds.fromFieldRelativeSpeeds(
-                  linearVelocity.getX() * DriveConstants.drivetrainConfig.maxLinearVelocity(),
-                  linearVelocity.getY() * DriveConstants.drivetrainConfig.maxLinearVelocity(),
-                  omega * DriveConstants.drivetrainConfig.maxAngularVelocity(),
-                  RobotState.getInstance().getEstimatedPose().getRotation()));
+          drive
+              .getMotionPlanner()
+              .acceptDriveInput(
+                  ChassisSpeeds.fromFieldRelativeSpeeds(
+                      linearVelocity.getX() * DriveConstants.drivetrainConfig.maxLinearVelocity(),
+                      linearVelocity.getY() * DriveConstants.drivetrainConfig.maxLinearVelocity(),
+                      omega * DriveConstants.drivetrainConfig.maxAngularVelocity(),
+                      RobotState.getInstance().getEstimatedPose().getRotation()));
         },
         drive);
   }

src/main/java/frc/robot/commands/TestAutos.java

@@ -0,0 +1,59 @@
+package frc.robot.commands;
+
+import static frc.robot.util.trajectory.ChoreoTrajectoryReader.generate;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.FieldConstants;
+import frc.robot.RobotState;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.drive.DriveMotionPlanner;
+import frc.robot.util.trajectory.Trajectory;
+import java.io.File;
+
+public class TestAutos {
+  public static Command threeSpikeSmooth(Drive drive) {
+    File trajFile = new File(Filesystem.getDeployDirectory(), "/choreo/3SpikeSmooth.traj");
+    Trajectory trajectory = generate(trajFile).orElseThrow();
+    DriveMotionPlanner motionPlanner = drive.getMotionPlanner();
+
+    // Waiting inbetween shots is to fake intaking
+    // Would not be time based on real robot
+    return Commands.sequence(
+        reset(trajectory.getStartPose(), drive),
+        Commands.waitSeconds(1.5), // Shoot first 1.5 secs
+        Commands.runOnce(() -> motionPlanner.setTrajectory(trajectory)),
+        Commands.waitSeconds(0.7), // 2.2 secs
+        moveWhileShooting(motionPlanner), // 2.9 secs
+        Commands.waitSeconds(0.3), // 3.2 secs
+        moveWhileShooting(motionPlanner), // 3.9 secs
+        Commands.waitSeconds(0.61), // 4.51 secs
+        moveWhileShooting(motionPlanner)); // 5.21 secs
+  }
+
+  private static Command moveWhileShooting(DriveMotionPlanner motionPlanner) {
+    return Commands.runOnce(() -> motionPlanner.setHeadingSupplier(TestAutos::getShootHeading))
+        .andThen(
+            Commands.waitSeconds(0.7)
+                .andThen(
+                    Commands.runOnce(motionPlanner::disableHeadingSupplier))); // Time to shoot ig
+  }
+
+  private static Rotation2d getShootHeading() {
+    return FieldConstants.Speaker.centerSpeakerOpening
+        .getTranslation()
+        .minus(RobotState.getInstance().getEstimatedPose().getTranslation())
+        .getAngle()
+        .plus(Rotation2d.fromRadians(Math.PI));
+  }
+
+  private static Command reset(Pose2d pose, Drive drive) {
+    return Commands.runOnce(
+        () ->
+            RobotState.getInstance()
+                .resetPose(pose, drive.getWheelPositions(), drive.getGyroYaw()));
+  }
+}

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -30,11 +30,13 @@
 
 public class Drive extends SubsystemBase {
   public static final Lock odometryLock = new ReentrantLock();
+  // TODO: DO THIS BETTER!
   public static final Queue<Double> timestampQueue = new ArrayBlockingQueue<>(100);
 
   private final GyroIO gyroIO;
   private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
   private final Module[] modules = new Module[4];
+  private final DriveMotionPlanner motionPlanner = new DriveMotionPlanner();
   private ChassisSpeeds robotVelocity = new ChassisSpeeds();
   private ChassisSpeeds fieldVelocity = new ChassisSpeeds();
 
@@ -100,9 +102,13 @@ public void periodic() {
       Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
     }
 
-    // update current velocities
+    // update current velocities use gyro when possible
     ChassisSpeeds robotRelativeVelocity =
         DriveConstants.kinematics.toChassisSpeeds(getModuleStates());
+    robotRelativeVelocity.omegaRadiansPerSecond =
+        gyroInputs.connected
+            ? gyroInputs.yawVelocityRadPerSec
+            : robotRelativeVelocity.omegaRadiansPerSecond;
     robotVelocity = robotRelativeVelocity;
     Translation2d linearFieldVel =
         new Translation2d(
@@ -112,17 +118,22 @@ public void periodic() {
         new ChassisSpeeds(
             linearFieldVel.getX(),
             linearFieldVel.getY(),
-            gyroInputs.connected
-                ? gyroInputs.yawVelocityRadPerSec
-                : robotRelativeVelocity.omegaRadiansPerSecond);
+            robotRelativeVelocity.omegaRadiansPerSecond);
+
+    // Get motion planner velocity and run
+    ChassisSpeeds speeds =
+        motionPlanner.update(
+            Timer.getFPGATimestamp(), RobotState.getInstance().getEstimatedPose(), fieldVelocity);
+    if (motionPlanner.xModeEnabled()) stopWithX();
+    else runVelocity(speeds);
   }
 
   /**
    * Runs the drive at the desired velocity.
    *
    * @param speeds Speeds in meters/sec
    */
-  public void runVelocity(ChassisSpeeds speeds) {
+  private void runVelocity(ChassisSpeeds speeds) {
     // Calculate module setpoints
     ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
     SwerveModuleState[] setpointStates =
@@ -142,15 +153,15 @@ public void runVelocity(ChassisSpeeds speeds) {
   }
 
   /** Stops the drive. */
-  public void stop() {
+  private void stop() {
     runVelocity(new ChassisSpeeds());
   }
 
   /**
    * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will
    * return to their normal orientations the next time a nonzero velocity is requested.
    */
-  public void stopWithX() {
+  private void stopWithX() {
     Rotation2d[] headings = new Rotation2d[4];
     for (int i = 0; i < 4; i++) {
       headings[i] = DriveConstants.moduleTranslations[i].getAngle();
@@ -166,6 +177,10 @@ public void runCharacterizationVolts(double volts) {
     }
   }
 
+  public DriveMotionPlanner getMotionPlanner() {
+    return motionPlanner;
+  }
+
   /** Returns the average drive velocity in radians/sec. */
   public double getCharacterizationVelocity() {
     double driveVelocityAverage = 0.0;