Update Drive to point to use profiled PID

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

@@ -4,10 +4,15 @@
 
 package frc.robot.commands;
 
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 
@@ -17,57 +22,131 @@ public class DriveToPoint extends Command {
 
   private final CommandSwerveDrivetrain m_drivetrain;
   private final Pose2d m_targetPose;
+  private final LinearPath path;
+
+  private double elapsedTime;
+  private LinearPath.State initialState;
+  private double lastUpdateTime;
+  private ChassisSpeeds lastSetpointSpeeds;
+  private LinearPath.State setpoint;
 
   private PIDController xController = new PIDController(10, 0, 0);
   private PIDController yController = new PIDController(10, 0, 0);
   private PIDController thetaController = new PIDController(7, 0, 0);
 
-  private SwerveRequest.FieldCentric driveRequest = new SwerveRequest.FieldCentric();
-  /** Creates a new DriveToPoint. */
+  private SwerveRequest.ApplyFieldSpeeds driveRequest =
+      new SwerveRequest.ApplyFieldSpeeds().withDriveRequestType(DriveRequestType.Velocity);
+
+  private final WheelForceCalculator forceCalculator;
+
+  // Maximum velocities and accelerations for path following
+  private static final double kMaxV = 4.3; // m/s
+  private static final double kMaxA = 4; // m/s^2
+  private static final double kMaxOmega = Math.PI; // rad/s
+  private static final double kMaxAlpha = 2.0 * Math.PI; // rad/s^2
+
+  /**
+   * Creates a new DriveToPoint command.
+   *
+   * @param drivetrain The swerve drivetrain subsystem
+   * @param targetPose Target pose in field coordinates.
+   */
   public DriveToPoint(CommandSwerveDrivetrain drivetrain, Pose2d targetPose) {
     m_drivetrain = drivetrain;
     m_targetPose = targetPose;
 
+    // Use 6 if you aren't sure about MOI
+    forceCalculator = new WheelForceCalculator(m_drivetrain.getModuleLocations(), 40, 6);
+
+    // Create linear path with velocity/acceleration constraints
+    this.path =
+        new LinearPath(
+            new TrapezoidProfile.Constraints(kMaxV, kMaxA),
+            new TrapezoidProfile.Constraints(kMaxOmega, kMaxAlpha));
+
     // We do this so robot will rotate fastest way around
     thetaController.enableContinuousInput(-Math.PI, Math.PI);
 
-    // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(drivetrain);
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    Pose2d currentPose = m_drivetrain.getState().Pose;
+    ChassisSpeeds currentSpeeds = m_drivetrain.getState().Speeds;
+
+    // Save the INITIAL state - this never changes
+    initialState =
+        new LinearPath.State(
+            currentPose,
+            ChassisSpeeds.fromRobotRelativeSpeeds(currentSpeeds, currentPose.getRotation()));
+
+    elapsedTime = 0.0;
+    lastUpdateTime = Utils.getCurrentTimeSeconds();
+
+    setpoint = path.calculate(elapsedTime, initialState, m_targetPose);
+
+    lastSetpointSpeeds = setpoint.speeds;
+  }
 
-  // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    Pose2d currentPose = m_drivetrain.getPose();
+    double currentTime = Utils.getCurrentTimeSeconds();
+    double dt = currentTime - lastUpdateTime;
+    lastUpdateTime = currentTime;
+
+    elapsedTime += dt;
+
+    // Update setpoint to get speed robot should be running at based on current time
+    setpoint = path.calculate(elapsedTime, initialState, m_targetPose);
 
-    double xVelocity = xController.calculate(currentPose.getX(), m_targetPose.getX());
-    double yVelocity = yController.calculate(currentPose.getY(), m_targetPose.getY());
-    double thetaVelocity =
+    Pose2d currentPose = m_drivetrain.getState().Pose;
+
+    // Calculate feedback corrections
+    double xFeedback = xController.calculate(currentPose.getX(), setpoint.pose.getX());
+    double yFeedback = yController.calculate(currentPose.getY(), setpoint.pose.getY());
+    double thetaFeedback =
         thetaController.calculate(
-            currentPose.getRotation().getRadians(), m_targetPose.getRotation().getRadians());
+            currentPose.getRotation().getRadians(), setpoint.pose.getRotation().getRadians());
+
+    ChassisSpeeds feedbackSpeeds = new ChassisSpeeds(xFeedback, yFeedback, thetaFeedback);
 
+    // Calculate feedforward forces based on PLANNED trajectory change
+    Feedforwards feedforwards = forceCalculator.calculate(dt, lastSetpointSpeeds, setpoint.speeds);
+
+    // Combine feedforward + feedback
+    ChassisSpeeds correctedSpeeds =
+        new ChassisSpeeds(
+            setpoint.speeds.vxMetersPerSecond + feedbackSpeeds.vxMetersPerSecond,
+            setpoint.speeds.vyMetersPerSecond + feedbackSpeeds.vyMetersPerSecond,
+            setpoint.speeds.omegaRadiansPerSecond + feedbackSpeeds.omegaRadiansPerSecond);
+
+    // Apply feedforward forces (from planned trajectory) + corrected speeds (planned + feedback)
     m_drivetrain.setControl(
         driveRequest
-            .withVelocityX(xVelocity)
-            .withVelocityY(yVelocity)
-            .withRotationalRate(thetaVelocity));
+            .withSpeeds(correctedSpeeds)
+            .withWheelForceFeedforwardsX(feedforwards.x_newtons)
+            .withWheelForceFeedforwardsY(feedforwards.y_newtons));
+
+    // Save setpoint speeds for next feedforward calculation
+    lastSetpointSpeeds = setpoint.speeds;
+
+    // Telemetry
+    SmartDashboard.putString("DriveToPoint/TargetPose", m_targetPose.toString());
+    SmartDashboard.putString("DriveToPoint/CurrentPose", currentPose.toString());
+    SmartDashboard.putNumber(
+        "DriveToPoint/DistanceToTarget",
+        currentPose.getTranslation().getDistance(m_targetPose.getTranslation()));
   }
 
-  // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
     m_drivetrain.setControl(new SwerveRequest.Idle());
   }
 
-  // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    // Only recommend if you need it to end
-    // return xController.atSetpoint() && yController.atSetpoint() && thetaController.atSetpoint();
-    return false;
+    return path.isFinished(elapsedTime);
   }
 }