Pure pursuit target point calculation (#111)

* Add math to find perpendicular points as well as lookahead points * Start adding point processing * Add unit test * Fix getLookaheadWaypoint to always give a lookahead point toward the end point * Simplify test assertions with Pose2ds * Remove old summing test * Change heuristic logs for multiple path tracking logging * Do more testing for perp and lookahead math * Make a pure pursuit test auto * Start debugging pure pursuit * Change pure pursuit aligning to head toward start pose * Format * reset previous auto point index trailblazer * Add interpolated rotation pure pursuit path tracking * Incorporate a curvature based dynamic lookahead * Make a test auto for in person robot testing with pure pursuit * OP auto segment fix * Add interpolated rotation pure pursuit path tracking * Add rotation check to isFinished() for pure pursuit tracker * Update dependency gradle to v8.12 (#100) Co-authored-by: renovate[bot] <29139614+renovate[bot]@users.noreply.github.com> * Update dependency org.junit.jupiter:junit-jupiter to v5.11.4 (#114) Co-authored-by: renovate[bot] <29139614+renovate[bot]@users.noreply.github.com> * fix typo in expected result for linear velocity test * linear velocity & acceleration constraints * angular acceleration & velocity constraints * Format * Make changes to the test auto * Fix linear acceleration constraint math to return negative and positive values * add rotation element to isFinished() pure pursuit * Fix pure pursuit issue with jumping lookahead * Play with constraints in test auto * dynamic velocity constraint potential change * Format * Make 4 peice auto Kitbot with Trailblazer * Remove segment motion profile and format --------- Co-authored-by: Simon <87281874+simonp17@users.noreply.github.com> Co-authored-by: Jonah Snider <jonah@jonahsnider.com> Co-authored-by: fcuellar13 <105395555+fcuellar13@users.noreply.github.com> Co-authored-by: renovate[bot] <29139614+renovate[bot]@users.noreply.github.com>
team581 · Jan 7, 2025 · 4ba4c17 · 4ba4c17
1 parent 67c4eaf
commit 4ba4c17
Show file tree

Hide file tree

Showing 10 changed files with 509 additions and 46 deletions.
diff --git a/src/main/java/frc/robot/autos/AutoSelection.java b/src/main/java/frc/robot/autos/AutoSelection.java
@@ -1,14 +1,16 @@
 package frc.robot.autos;
 
 import frc.robot.autos.amp.Op345Auto;
+import frc.robot.autos.amp.TestAuto;
 import frc.robot.autos.trailblazer.Trailblazer;
 import frc.robot.robot_manager.RobotManager;
 import java.util.function.BiFunction;
 
 public enum AutoSelection {
   DO_NOTHING(DoNothingAuto::new),
 
-  OP(Op345Auto::new);
+  OP(Op345Auto::new),
+  PURE_PURSUIT_TEST_AUTO(TestAuto::new);
 
   public final BiFunction<RobotManager, Trailblazer, BaseAuto> auto;
 

diff --git a/src/main/java/frc/robot/autos/amp/Op345Auto.java b/src/main/java/frc/robot/autos/amp/Op345Auto.java
@@ -32,13 +32,24 @@ 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()
+                .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(13.68, 6.99, Rotation2d.fromDegrees(0.0))),
-                new AutoPoint(new Pose2d(12.64, 7.06, Rotation2d.fromDegrees(-21.67))))),
+                new AutoPoint(new Pose2d(8.52, 7.46, Rotation2d.fromDegrees(0.0))),
+                new AutoPoint(new Pose2d(12.58, 6.01, Rotation2d.fromDegrees(-6.63))))),
         Commands.sequence(autoCommands.speakerShotWithTimeout(), actions.intakeAssistCommand()),
         trailblazer.followSegment(
+
             new AutoSegment(
                 new AutoPoint(new Pose2d(8.52, 7.46, Rotation2d.fromDegrees(0.0))),
                 new AutoPoint(new Pose2d(12.58, 6.01, Rotation2d.fromDegrees(-6.63))))),

diff --git a/src/main/java/frc/robot/autos/amp/TestAuto.java b/src/main/java/frc/robot/autos/amp/TestAuto.java
@@ -0,0 +1,63 @@
+package frc.robot.autos.amp;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.autos.BaseAuto;
+import frc.robot.autos.trailblazer.AutoPoint;
+import frc.robot.autos.trailblazer.AutoSegment;
+import frc.robot.autos.trailblazer.Trailblazer;
+import frc.robot.autos.trailblazer.constraints.AutoConstraintOptions;
+import frc.robot.robot_manager.RobotManager;
+
+public class TestAuto extends BaseAuto {
+  public TestAuto(RobotManager robotManager, Trailblazer trailblazer) {
+    super(robotManager, trailblazer);
+  }
+
+  @Override
+  protected Command getBlueAutoCommand() {
+    return Commands.none();
+  }
+
+  @Override
+  protected Command getRedAutoCommand() {
+    return Commands.sequence(
+        Commands.print("example command on auto start"),
+        Commands.runOnce(
+            () ->
+                robotManager.localization.resetPose(
+                    new Pose2d(9.0, 4.0, Rotation2d.fromDegrees(0)))),
+        trailblazer.followSegment(
+            new AutoSegment(
+                new AutoConstraintOptions(false, 5, 500, 8.0, 5000),
+                new AutoPoint(new Pose2d(11.8, 4.0, Rotation2d.fromDegrees(0))))),
+        trailblazer.followSegment(
+            new AutoSegment(
+                new AutoConstraintOptions(false, 5, 500, 8.0, 500),
+                new AutoPoint(new Pose2d(10.9, 5.0, Rotation2d.fromDegrees(-10))),
+                new AutoPoint(new Pose2d(16.157, 7.043, Rotation2d.fromDegrees(-125.216))))),
+        trailblazer.followSegment(
+            new AutoSegment(
+                new AutoConstraintOptions(false, 5, 500, 8.0, 500),
+                new AutoPoint(new Pose2d(13.6, 5.2, Rotation2d.fromDegrees(-120.559))))),
+        trailblazer.followSegment(
+            new AutoSegment(
+                new AutoConstraintOptions(false, 5, 500, 8.0, 500),
+                new AutoPoint(new Pose2d(16.157, 7.043, Rotation2d.fromDegrees(-125.216))))),
+        trailblazer.followSegment(
+            new AutoSegment(
+                new AutoConstraintOptions(false, 5, 500, 8.0, 500),
+                new AutoPoint(new Pose2d(13.6, 5.2, Rotation2d.fromDegrees(-120.559))))),
+        trailblazer.followSegment(
+            new AutoSegment(
+                new AutoConstraintOptions(false, 5, 500, 8.0, 500),
+                new AutoPoint(new Pose2d(16.157, 7.043, Rotation2d.fromDegrees(-125.216))))),
+        trailblazer.followSegment(
+            new AutoSegment(
+                new AutoConstraintOptions(false, 5, 500, 8.0, 500),
+                new AutoPoint(new Pose2d(12.2, 5.5, Rotation2d.fromDegrees(-60.0))),
+                new AutoPoint(new Pose2d(12.46, 5.16, Rotation2d.fromDegrees(-59.927))))));
+  }
+}
diff --git a/src/main/java/frc/robot/autos/trailblazer/Trailblazer.java b/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;
 
@@ -23,15 +23,17 @@ public class Trailblazer {
    */
   private static AutoConstraintOptions resolveConstraints(
       AutoPoint point, AutoConstraintOptions segmentConstraints) {
-    return point.constraints.orElse(segmentConstraints);
+    var constraints = point.constraints.orElse(segmentConstraints);
+    return constraints;
   }
 
   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(8.0, 0, 0));
+
   private int previousAutoPointIndex = -1;
   private ChassisSpeeds previousSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0);
   private double previousTimestamp = 0.0;
@@ -62,7 +64,6 @@ public Command followSegment(AutoSegment segment, boolean shouldEnd) {
                     () -> {
                       pathTracker.updateRobotState(
                           localization.getPose(), swerve.getFieldRelativeSpeeds());
-
                       var currentAutoPointIndex = pathTracker.getCurrentPointIndex();
                       var currentAutoPoint = segment.points.get(currentAutoPointIndex);
 
@@ -110,13 +111,18 @@ private ChassisSpeeds getSwerveSetpoint(
     if (previousTimestamp == 0.0) {
       previousTimestamp = currentTimestamp - 0.02;
     }
-    var usedConstraints = resolveConstraints(point, segmentConstraints);
-
+    var robotPose = localization.getPose();
     var originalTargetPose = pathTracker.getTargetPose();
+    var originalVelocityGoal = pathFollower.calculateSpeeds(robotPose, originalTargetPose);
+    var currentVelocity =
+        Math.hypot(originalVelocityGoal.vxMetersPerSecond, originalVelocityGoal.vyMetersPerSecond);
+
+    var usedConstraints = resolveConstraints(point, segmentConstraints);
+    DogLog.log(
+        "Trailblazer/Constraints/VelocityCalculation/CalculatedVelocity",
+        usedConstraints.maxLinearVelocity());
     DogLog.log("Trailblazer/Tracker/RawOutput", originalTargetPose);
 
-    var originalVelocityGoal =
-        pathFollower.calculateSpeeds(localization.getPose(), originalTargetPose);
     DogLog.log("Trailblazer/Follower/RawOutput", originalVelocityGoal);
     var constrainedVelocityGoal =
         AutoConstraintCalculator.constrainVelocityGoal(

diff --git a/src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculator.java b/src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculator.java
@@ -71,6 +71,7 @@ private static ChassisSpeeds constrainLinearAcceleration(
       double timeBetweenPreviousAndInputSpeeds,
       AutoConstraintOptions options) {
 
+
     double currentLinearAcceleration =
         Math.hypot(inputSpeeds.vxMetersPerSecond, inputSpeeds.vyMetersPerSecond);
     double previousLinearAcceleration =
@@ -85,7 +86,6 @@ private static ChassisSpeeds constrainLinearAcceleration(
               + options.maxLinearAcceleration() * timeBetweenPreviousAndInputSpeeds;
       double constrainedVx = finalAcceleration * Math.cos(preserveTheta);
       double constrainedVy = finalAcceleration * Math.sin(preserveTheta);
-
       return new ChassisSpeeds(constrainedVx, constrainedVy, inputSpeeds.omegaRadiansPerSecond);
     }
     return inputSpeeds;

diff --git a/src/main/java/frc/robot/autos/trailblazer/trackers/HeuristicPathTracker.java b/src/main/java/frc/robot/autos/trailblazer/trackers/HeuristicPathTracker.java
@@ -34,16 +34,16 @@ public void updateRobotState(Pose2d currentPose, ChassisSpeeds currentFieldRelat
     if (distanceToTarget < proximityRadius && currentPointIndex < points.size() - 1) {
       currentPointIndex++;
     }
-    DogLog.log("Autos/Trailblazer/CurrentTargetPose", currentTargetPose);
-    DogLog.log("Autos/Trailblazer/DistanceToTarget", distanceToTarget);
-    DogLog.log("Autos/Trailblazer/NextPointIndex", currentPointIndex);
+    DogLog.log("Autos/Trailblazer/HeuristicPathTracker/CurrentTargetPose", currentTargetPose);
+    DogLog.log("Autos/Trailblazer/HeuristicPathTracker/DistanceToTarget", distanceToTarget);
+    DogLog.log("Autos/Trailblazer/HeuristicPathTracker/NextPointIndex", currentPointIndex);
   }
 
   @Override
   public Pose2d getTargetPose() {
     var targetPose = points.get(currentPointIndex).poseSupplier.get();
 
-    DogLog.log("Autos/Trailblazer/TargetPose", targetPose);
+    DogLog.log("Autos/Trailblazer/HeuristicPathTracker/TargetPose", targetPose);
 
     return targetPose;
   }

diff --git a/src/main/java/frc/robot/autos/trailblazer/trackers/PurePursuitPathTracker.java b/src/main/java/frc/robot/autos/trailblazer/trackers/PurePursuitPathTracker.java