Creates basic trajectory following commands

src/main/java/frc/robot/subsystems/swerveIO/SwerveSubsystem.java

@@ -1,13 +1,18 @@
 package frc.robot.subsystems.swerveIO;
 
+import com.choreo.lib.Choreo;
+import com.choreo.lib.ChoreoControlFunction;
+import com.choreo.lib.ChoreoTrajectory;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
@@ -24,9 +29,13 @@
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import frc.robot.Constants;
 import frc.robot.Constants.DriveConstants;
+import frc.robot.Robot;
+import frc.robot.rhr.auto.RHRPathPlannerAuto;
 import frc.robot.subsystems.swerveIO.module.SwerveModule;
 import frc.robot.subsystems.swerveIO.module.SwerveModuleIO;
+import frc.robot.util.ErrorTracker;
 import frc.robot.util.MotionHandler;
+import frc.robot.util.PIDFFGains;
 import frc.robot.util.SwerveHeadingController;
 import org.littletonrobotics.junction.Logger;
 
@@ -405,6 +414,41 @@ public void periodic() {
   }
 
   public static class Commands {
+    public ChoreoControlFunction modifiedChoreoSwerveController(
+        PIDController xController, PIDController yController, PIDController rotationController) {
+      rotationController.enableContinuousInput(-Math.PI, Math.PI);
+      ErrorTracker errorTracker =
+          new ErrorTracker(
+              10,
+              PIDFFGains.fromPIDGains(xController),
+              PIDFFGains.fromPIDGains(rotationController));
+      return (pose, referenceState) -> {
+        Logger.recordOutput(
+            "Choreo/Target Pose",
+            new Pose2d(
+                new Translation2d(referenceState.x, referenceState.y),
+                Rotation2d.fromRadians(referenceState.heading)));
+
+        var error =
+            new Pose2d(
+                new Translation2d(referenceState.x - pose.getX(), referenceState.y - pose.getY()),
+                Rotation2d.fromRadians(referenceState.heading - pose.getRotation().getRadians()));
+
+        errorTracker.addObservation(error);
+        double xFF = referenceState.velocityX;
+        double yFF = referenceState.velocityY;
+        double rotationFF = referenceState.angularVelocity;
+
+        double xFeedback = xController.calculate(pose.getX(), referenceState.x);
+        double yFeedback = yController.calculate(pose.getY(), referenceState.y);
+        double rotationFeedback =
+            rotationController.calculate(pose.getRotation().getRadians(), referenceState.heading);
+
+        return ChassisSpeeds.fromFieldRelativeSpeeds(
+            xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, pose.getRotation());
+      };
+    }
+
     public static Command setHeading(Rotation2d degrees) {
       return new InstantCommand(() -> SwerveHeadingController.getInstance().setSetpoint(degrees));
     }
@@ -414,5 +458,33 @@ public static Command setHeadingandWait(Rotation2d degrees) {
           setHeading(degrees),
           new WaitUntilCommand(SwerveHeadingController.getInstance()::atSetpoint));
     }
+
+    public static Command getAutonomousCommandChoreo(String trajectory) {
+      return new RHRPathPlannerAuto(trajectory);
+    }
+
+    public Command choreoCommandBuilder(ChoreoTrajectory traj) {
+      var alliance = DriverStation.getAlliance();
+      boolean useAllianceColour = false;
+      if (alliance.isPresent()) {
+        useAllianceColour = alliance.get() == DriverStation.Alliance.Red;
+      }
+
+      return new SequentialCommandGroup(
+          Choreo.choreoSwerveCommand(
+              traj, //
+              Robot.swerveDrive::getUsablePose, //
+              modifiedChoreoSwerveController(
+                  new PIDController(3, 0.0, 0.0), //
+                  new PIDController(3, 0.0, 0.0), //
+                  new PIDController(3, 0.0, 0.0)),
+              (ChassisSpeeds speeds) -> {
+                Robot.swerveDrive.setDesiredChassisSpeeds(speeds);
+              },
+              useAllianceColour,
+              Robot.swerveDrive //
+              ),
+          new InstantCommand(() -> Robot.swerveDrive.setDesiredChassisSpeeds(new ChassisSpeeds())));
+    }
   }
 }