Add interpolated rotation pure pursuit path tracking

src/main/java/frc/robot/autos/amp/Op345Auto.java

@@ -32,11 +32,16 @@ protected Command getRedAutoCommand() {
             new AutoSegment(
                 new AutoPoint(new Pose2d(15.78, 6.67, Rotation2d.fromDegrees(-60.48))),
                 new AutoPoint(new Pose2d(14.676, 6.767, Rotation2d.fromDegrees(-26.85))))),
-        Commands.sequence(autoCommands.speakerShotWithTimeout(), actions.intakeAssistCommand()),
-        trailblazer.followSegment(
-            new AutoSegment(
-                new AutoPoint(new Pose2d(13.68, 6.99, Rotation2d.fromDegrees(0.0))),
-                new AutoPoint(new Pose2d(12.64, 7.06, Rotation2d.fromDegrees(-21.67))))),
+        Commands.sequence(
+            autoCommands.speakerShotWithTimeout(),
+            actions
+                .intakeAssistCommand()
+                .alongWith(
+                    trailblazer.followSegment(
+                        new AutoSegment(
+                            new AutoPoint(new Pose2d(13.68, 6.99, Rotation2d.fromDegrees(0.0))),
+                            new AutoPoint(
+                                new Pose2d(12.64, 7.06, Rotation2d.fromDegrees(-21.67))))))),
         Commands.sequence(autoCommands.speakerShotWithTimeout(), actions.intakeAssistCommand()),
         trailblazer.followSegment(
             new AutoSegment(new AutoPoint(new Pose2d(8.52, 7.46, Rotation2d.fromDegrees(0.0))))),

src/main/java/frc/robot/autos/amp/TestAuto.java

@@ -30,19 +30,22 @@ protected Command getRedAutoCommand() {
                     new Pose2d(15.18, 6.41, Rotation2d.fromDegrees(0.0)))),
         trailblazer.followSegment(
             new AutoSegment(
-                new AutoPoint(new Pose2d(12.00, 6.28, Rotation2d.fromDegrees(0.0))),
-                new AutoPoint(new Pose2d(10.63, 7.42, Rotation2d.fromDegrees(0.0))),
-                new AutoPoint(new Pose2d(8.63, 7.42, Rotation2d.fromDegrees(0.0))))),
+                new AutoPoint(new Pose2d(12.00, 6.28, Rotation2d.fromDegrees(-100))),
+                new AutoPoint(new Pose2d(10.00, 5.28, Rotation2d.fromDegrees(40))),
+                new AutoPoint(new Pose2d(8.00, 6.28, Rotation2d.fromDegrees(-50))),
+                new AutoPoint(new Pose2d(6.00, 5.28, Rotation2d.fromDegrees(100))),
+                new AutoPoint(new Pose2d(4.00, 6.28, Rotation2d.fromDegrees(0)))
+
+                )),
         trailblazer.followSegment(
-            new AutoSegment(
-                new AutoPoint(new Pose2d(11.82, 6.59, Rotation2d.fromDegrees(-12.24))))),
+            new AutoSegment(new AutoPoint(new Pose2d(11.82, 6.59, Rotation2d.fromDegrees(24.0))))),
         trailblazer.followSegment(
             new AutoSegment(
-                new AutoPoint(new Pose2d(9.98, 6.55, Rotation2d.fromDegrees(14.02))),
-                new AutoPoint(new Pose2d(8.60, 5.85, Rotation2d.fromDegrees(14.02))))),
+                new AutoPoint(new Pose2d(9.98, 6.55, Rotation2d.fromDegrees(-5.0))),
+                new AutoPoint(new Pose2d(8.60, 5.85, Rotation2d.fromDegrees(100.0))))),
         trailblazer.followSegment(
             new AutoSegment(
-                new AutoPoint(new Pose2d(10.78, 7.34, Rotation2d.fromDegrees(0))),
-                new AutoPoint(new Pose2d(11.82, 6.59, Rotation2d.fromDegrees(-12.24))))));
+                new AutoPoint(new Pose2d(10.78, 7.34, Rotation2d.fromDegrees(-80.0))),
+                new AutoPoint(new Pose2d(13.3, 6.59, Rotation2d.fromDegrees(40.0))))));
   }
 }

src/main/java/frc/robot/autos/trailblazer/Trailblazer.java

@@ -11,8 +11,8 @@
 import frc.robot.autos.trailblazer.constraints.AutoConstraintOptions;
 import frc.robot.autos.trailblazer.followers.PathFollower;
 import frc.robot.autos.trailblazer.followers.PidPathFollower;
-import frc.robot.autos.trailblazer.trackers.HeuristicPathTracker;
 import frc.robot.autos.trailblazer.trackers.PathTracker;
+import frc.robot.autos.trailblazer.trackers.pure_pursuit.PurePursuitPathTracker;
 import frc.robot.localization.LocalizationSubsystem;
 import frc.robot.swerve.SwerveSubsystem;
 
@@ -28,10 +28,10 @@ private static AutoConstraintOptions resolveConstraints(
 
   private final SwerveSubsystem swerve;
   private final LocalizationSubsystem localization;
-  private final PathTracker pathTracker = new HeuristicPathTracker();
+  private final PathTracker pathTracker = new PurePursuitPathTracker();
   private final PathFollower pathFollower =
       new PidPathFollower(
-          new PIDController(4, 0, 0), new PIDController(4, 0, 0), new PIDController(2.5, 0, 0));
+          new PIDController(4, 0, 0), new PIDController(4, 0, 0), new PIDController(8, 0, 0));
   private int previousAutoPointIndex = -1;
   private ChassisSpeeds previousSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0);
   private double previousTimestamp = 0.0;
@@ -91,7 +91,14 @@ public Command followSegment(AutoSegment segment, boolean shouldEnd) {
             .withName("FollowSegmentIndefinitely");
 
     if (shouldEnd) {
-      return command.until(pathTracker::isFinished).withName("FollowSegmentUntilFinished");
+      return command
+          .until(pathTracker::isFinished)
+          .withName("FollowSegmentUntilFinished")
+          .andThen(
+              Commands.runOnce(
+                  () -> {
+                    swerve.setFieldRelativeAutoSpeeds(new ChassisSpeeds());
+                  }));
     }
 
     return command;

src/main/java/frc/robot/autos/trailblazer/trackers/pure_pursuit/PurePursuitPathTracker.java

@@ -1,12 +1,13 @@
 package frc.robot.autos.trailblazer.trackers.pure_pursuit;
 
+import java.util.List;
+
 import dev.doglog.DogLog;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import frc.robot.autos.trailblazer.AutoPoint;
 import frc.robot.autos.trailblazer.trackers.PathTracker;
-import java.util.List;
 
 // TODO: Implement https://github.com/team581/2024-offseason-bot/issues/95
 public class PurePursuitPathTracker implements PathTracker {
@@ -22,9 +23,10 @@ public class PurePursuitPathTracker implements PathTracker {
 
   @Override
   public void resetAndSetPoints(List<AutoPoint> points) {
+    startingRobotPose = new Pose2d();
+    startingRobotPoseUpdated = false;
     currentPointIndex = 0;
     this.points = points;
-    startingRobotPoseUpdated = false;
   }
 
   @Override
@@ -83,12 +85,8 @@ public Pose2d getTargetPose() {
           Math.sqrt(Math.pow(lookaheadX - startX, 2) + Math.pow(lookaheadY - startY, 2));
       var distanceToEnd =
           Math.sqrt(Math.pow(lookaheadX - endX, 2) + Math.pow(lookaheadY - endY, 2));
-      // Check if lookaheadpoint is outside on the starting side of the line
+      // Check if lookaheadpoint is outside on the ending side of the line
       if (distanceToStart > distanceToEnd) {
-
-        // Otherwise, the lookahead point is past the end point, which means we need to go around
-        // corner.
-        // TODO: Fix for case of perp point is also past end point
         var perpDistanceToEnd =
             Math.sqrt(
                 Math.pow(perpendicularPoint.getX() - endX, 2)
@@ -109,17 +107,16 @@ public Pose2d getTargetPose() {
         }
         // otherwise just return the end point
         else {
-          var targetPose = new Pose2d(endX, endY, new Rotation2d());
+          var targetPose = new Pose2d(endX, endY, getPointToPointInterpolatedRotation(currentTargetWaypoint, nextTargetWaypoint,  currentRobotPose));
           DogLog.log("Autos/Trailblazer/PurePursuitPathTracker/TargetPose", targetPose);
           return targetPose;
         }
       } else {
-
         var targetPose =
             getLookaheadPoint(
                 startingRobotPose,
-                new Pose2d(startX, startY, new Rotation2d()),
-                currentRobotPose,
+                currentTargetWaypoint,
+                getPerpendicularPoint(startingRobotPose, currentTargetWaypoint, currentRobotPose),
                 LOOKAHEAD_DISTANCE);
         DogLog.log("Autos/Trailblazer/PurePursuitPathTracker/TargetPose", targetPose);
         return targetPose;
@@ -140,12 +137,10 @@ public boolean isFinished() {
     if (points.isEmpty()) {
       return true;
     }
-    DogLog.timestamp("aaaaa");
     if (currentRobotPose
             .getTranslation()
             .getDistance(points.get(points.size() - 1).poseSupplier.get().getTranslation())
         < FINISHED_THRESHOLD) {
-      DogLog.timestamp("aaaaal,");
       return true;
     }
     return false;
@@ -201,6 +196,32 @@ private Pose2d getLookaheadPoint(
         y
             + lookaheadDistance
                 * ((y2 - y1) / (Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2))));
-    return new Pose2d(xLookahead, yLookahead, new Rotation2d());
+    return new Pose2d(xLookahead, yLookahead, getPointToPointInterpolatedRotation(startPoint,  endPoint,  pointOnPath));
   }
+
+
+  private Rotation2d getPointToPointInterpolatedRotation(Pose2d startPoint, Pose2d endPoint, Pose2d pointOnPath) {
+    //TODO: do unit tests for interpolated rotation
+  var totalDistance = startPoint.getTranslation().getDistance(endPoint.getTranslation());
+  var pointToStart = pointOnPath.getTranslation().getDistance(startPoint.getTranslation());
+  var pointToEnd = pointOnPath.getTranslation().getDistance(endPoint.getTranslation());
+
+  if (!((pointOnPath.getX() - startPoint.getX()) * (pointOnPath.getX() - endPoint.getX()) <= 0
+      && (pointOnPath.getY() - startPoint.getY()) * (pointOnPath.getY() - endPoint.getY()) <= 0)) {
+    if (pointToEnd > pointToStart) {
+      return startPoint.getRotation();
+    } else {
+      return endPoint.getRotation();
+    }
+  }
+  var progressPercent = Math.abs((pointToStart / totalDistance));
+  if (progressPercent > 0.8) {
+    progressPercent = 1.0;
+  }
+
+  var interpolatedRotation = startPoint.getRotation().interpolate(endPoint.getRotation(), progressPercent);
+
+  return interpolatedRotation;
+
+   }
 }