From 24cbe2fc9b3a0899d4afee59fa9919eeb3285145 Mon Sep 17 00:00:00 2001
From: fcuellar13 <105395555+fcuellar13@users.noreply.github.com>
Date: Sat, 23 Nov 2024 12:55:40 -0800
Subject: [PATCH] heuristic path tracker code continued (need to debug)

---
 .../trackers/HeuristicPathTracker.java        | 28 +++++++++++--------
 1 file changed, 17 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/autos/trailblazer/trackers/HeuristicPathTracker.java b/src/main/java/frc/robot/autos/trailblazer/trackers/HeuristicPathTracker.java
index aa05220..8fb2891 100644
--- a/src/main/java/frc/robot/autos/trailblazer/trackers/HeuristicPathTracker.java
+++ b/src/main/java/frc/robot/autos/trailblazer/trackers/HeuristicPathTracker.java
@@ -12,6 +12,9 @@ public class HeuristicPathTracker implements PathTracker {
   private Pose2d currentPose = new Pose2d();
   private double proximityRadius = 0.5;
   private int currentPointIndex = 0;
+  private double endPointProximityRadius = 0.1;
+ // private double distanceToTarget;
+ // private Pose2d currentTargetPose = new Pose2d();
 
   @Override
   public void resetAndSetPoints(List<AutoPoint> points) {
@@ -21,31 +24,30 @@ public void resetAndSetPoints(List<AutoPoint> points) {
   @Override
   public void updateRobotState(Pose2d currentPose, ChassisSpeeds currentFieldRelativeRobotSpeeds) {
     this.currentPose = currentPose;
-  }
 
-  @Override
-  public Pose2d getTargetPose() {
-    if (isFinished() == true) {
-      return null;
-    }
     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++;
     }
+    DogLog.log("Autos/Trailblazer/CurrentTargetPose", currentTargetPose);
+    DogLog.log("Autos/Trailblazer/DistanceToTarget", distanceToTarget);
+    DogLog.log("Autos/Trailblazer/NextPointIndex", currentPointIndex);
+  }
+
+  @Override
+  public Pose2d getTargetPose() {
+    //if (isFinished() == true) {
+   //   return currentTargetPose;
+   // }
 
     var targetPose = points.get(currentPointIndex).poseSupplier.get();
 
     DogLog.log("Autos/Trailblazer/TargetPose", targetPose);
-    DogLog.log("Autos/Trailblazer/DistanceToTarget", distanceToTarget);
 
     return targetPose;
   }
@@ -57,6 +59,10 @@ public int getCurrentPointIndex() {
 
   @Override
   public boolean isFinished() {
+    // if (distanceToTarget <= endPointProximityRadius && currentPointIndex >= points.size() - 1) {
+    //   DogLog.log("Autos/Trailblazer/isFinished", isFinished());
+    //   return true;
+    // }
     return false;
   }
 }