add logging for heursitic path tracker

Author: fcuellar13

src/main/java/frc/robot/autos/AutoChooser.java

@@ -17,8 +17,8 @@ public AutoChooser(RobotManager robotManager, Trailblazer trailblazer) {
     }
 
     chooser.setDefaultOption(
-        AutoSelection.DO_NOTHING.toString(),
-        AutoSelection.DO_NOTHING.auto.apply(robotManager, trailblazer));
+        AutoSelection.OP.toString(),
+        AutoSelection.OP.auto.apply(robotManager, trailblazer));
   }
 
   public BaseAuto getSelectedAuto() {

src/main/java/frc/robot/autos/trailblazer/trackers/HeuristicPathTracker.java

@@ -3,15 +3,19 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import frc.robot.autos.trailblazer.AutoPoint;
+
 import java.util.List;
 
+import dev.doglog.DogLog;
+
 // TODO: Implement https://github.com/team581/2024-offseason-bot/issues/94
 public class HeuristicPathTracker implements PathTracker {
   private List<AutoPoint> points = List.of();
   private Pose2d currentPose = new Pose2d();
   private double proximityRadius = 0.5;
   private int currentPointIndex = 0;
 
+
   @Override
   public void resetAndSetPoints(List<AutoPoint> points) {
     this.points = points;
@@ -29,14 +33,24 @@ public Pose2d getTargetPose() {
     }
     AutoPoint currentPoint = points.get(getCurrentPointIndex());
     Pose2d currentTargetPose = currentPoint.poseSupplier.get();
+
+   /// DogLog.log("Autos/Trailblazer/CurrentWaypoint", currentPoint);
+    DogLog.log("Autos/Trailblazer/NextPointIndex", currentPointIndex);
+    DogLog.log("Autos/Trailblazer/CurrentTargetPose", currentTargetPose);
+
     double distanceToTarget =
         currentPose.getTranslation().getDistance(currentTargetPose.getTranslation());
 
     if (distanceToTarget < proximityRadius && currentPointIndex < points.size() - 1) {
       currentPointIndex++;
     }
 
-    return points.get(currentPointIndex).poseSupplier.get();
+    var targetPose = points.get(currentPointIndex).poseSupplier.get();
+
+    DogLog.log("Autos/Trailblazer/TargetPose", targetPose);
+    DogLog.log("Autos/Trailblazer/DistanceToTarget", distanceToTarget);
+
+    return targetPose;
   }
 
   @Override
@@ -48,4 +62,5 @@ public int getCurrentPointIndex() {
   public boolean isFinished() {
     return false;
   }
+
 }