refactor: increment multiple keypoints if needed

src/main/java/xbot/common/trajectory/SimpleTimeInterpolator.java

@@ -37,7 +37,7 @@ public class SimpleTimeInterpolator {
     RobotAssertionManager assertionManager;
     private boolean usingKinematics;
 
-    public class InterpolationResult {
+    public static class InterpolationResult {
         public Translation2d chasePoint;
         public boolean isOnFinalPoint;
 
@@ -90,7 +90,7 @@ public SimpleTimeInterpolator(RobotAssertionManager assertionManager) {
         this.assertionManager = assertionManager;
     }
 
-    public void setKeyPoints(List<? extends ProvidesInterpolationData > keyPoints) {
+    public void setKeyPoints(List<? extends ProvidesInterpolationData> keyPoints) {
         this.keyPoints = keyPoints;
     }
 
@@ -126,8 +126,7 @@ public InterpolationResult calculateTarget(Translation2d currentLocation) {
 
         accumulatedProductiveSeconds += secondsSinceLastExecute;
 
-        // If we somehow have no points to visit, don't do anything.
-        if (keyPoints == null || keyPoints.size() == 0) {
+        if (keyPoints == null || keyPoints.isEmpty()) {
             log.warn("No key points to visit!");
             return new InterpolationResult(currentLocation, true, Rotation2d.fromDegrees(0));
         }
@@ -146,69 +145,73 @@ public InterpolationResult calculateTarget(Translation2d currentLocation) {
             );
         }
 
-        // First, assume we are just going to our target. (This is what all trajectories will eventually
-        // settle to - all this interpolation is for intermediate points.)
         Translation2d chasePoint = targetKeyPoint.getTranslation2d();
 
-        // Now, try to find a better point via linear interpolation.
-        // LerpFraction ranges from 0-1, and it is our goal completion progress to chasePoint
-        double lerpFraction = (accumulatedProductiveSeconds) / targetKeyPoint.getSecondsForSegment();
-        aKitLog.record("LerpFraction", lerpFraction);
+        aKitLog.record("LerpFraction", accumulatedProductiveSeconds / targetKeyPoint.getSecondsForSegment());
         aKitLog.record("accumulatedProductiveSeconds", accumulatedProductiveSeconds);
 
+        while (index < keyPoints.size() - 1) {
+            double segmentTime = targetKeyPoint.getSecondsForSegment();
+            if (segmentTime <= 0) {
+                log.warn("Cannot have a keypoint with a time of 0 or less! Skipping to next point.");
+                index++;
+                if (index >= keyPoints.size()) {
+                    break;
+                }
+                targetKeyPoint = keyPoints.get(index);
+                if (usingKinematics) {
+                    calculator = newCalculator(targetKeyPoint.getTranslation2d(), targetKeyPoint.getKinematics());
+                }
+                continue;
+            }
+
+            if (accumulatedProductiveSeconds < segmentTime) {
+                break;
+            }
 
-        // If the fraction is above 1, it's time to set a new baseline point and start LERPing on the next point
-        if (lerpFraction >= 1 && index < keyPoints.size()-1) {
-            // What was our target now becomes our baseline.
+            accumulatedProductiveSeconds -= segmentTime;
             baseline = targetKeyPoint;
-            accumulatedProductiveSeconds = 0;
-            lerpFraction = 0;
-            log.debug("LerpFraction is above one, so advancing to next keypoint");
             index++;
 
-            // And set our new target to the next element of the list
             targetKeyPoint = keyPoints.get(index);
-            log.debug("Baseline is now " + baseline.getTranslation2d()
-                    + " and target is now " + targetKeyPoint.getTranslation2d());
-
+            log.debug("Advancing to next keypoint, new baseline is {} and new target is {}", baseline.getTranslation2d(), targetKeyPoint.getTranslation2d());
             if (usingKinematics) {
                 calculator = newCalculator(targetKeyPoint.getTranslation2d(), targetKeyPoint.getKinematics());
             }
         }
 
-        // Most of the time, the fraction will be less than one.
-        // In that case, we want to interpolate between the baseline and the target.
+        double lerpFraction = Math.max(0, Math.min(accumulatedProductiveSeconds / targetKeyPoint.getSecondsForSegment(), 1.0));
+
         if (lerpFraction < 1) {
             if (usingKinematics) {
-                // This will be a curve as the calculator will do some fancy stuff with acceleration and velocity
                 Distance expectedMagnitudeTravelled = calculator.getDistanceTravelledAtCompletionPercentage(lerpFraction);
                 double multiplier = expectedMagnitudeTravelled.div(calculator.getTotalOperationDistance()).in(Value);
                 chasePoint = baseline.getTranslation2d().interpolate(
                         targetKeyPoint.getTranslation2d(), multiplier);
             } else {
-                // This will be a linear interpolation
                 chasePoint = baseline.getTranslation2d().interpolate(
                         targetKeyPoint.getTranslation2d(), lerpFraction);
             }
         }
 
-        // Now, if that chase point is "too far ahead", we need to freeze the chasePoint to allow the robot to catch up!
-        // Ideally, this should NEVER happen unless there are outside interference, and we will rely on PID to push us near
-        // the chase point again and everything will continue like normal. This effectively "rewinds time" for the next loop.
+        // If the chase point is too far ahead, freeze it so the robot can catch up.
+        // This "rewinds time" so the next loop recalculates from an earlier point on the trajectory.
         if (currentLocation.getDistance(chasePoint) > maximumDistanceFromChasePointInMeters) {
             accumulatedProductiveSeconds -= secondsSinceLastExecute;
+            lerpFraction = Math.max(0, Math.min(accumulatedProductiveSeconds
+                    / targetKeyPoint.getSecondsForSegment(), 1.0));
         }
 
-        // The plannedVector is our speed at the current time, IRL and in simulation, this may be behind
-        // due to friction, but that's what PID is for.
         var plannedVector = targetKeyPoint.getTranslation2d().minus(baseline.getTranslation2d());
 
         if (usingKinematics) {
             Distance expectedMagnitudeTravelled = calculator.getDistanceTravelledAtCompletionPercentage(lerpFraction);
             LinearVelocity velocityScalar = calculator.getVelocityAtDistanceTravelled(expectedMagnitudeTravelled);
 
             // We have a velocity, so we now only need to scale our plannedVector to that
-            plannedVector = plannedVector.times(velocityScalar.in(MetersPerSecond) / plannedVector.getNorm());
+            if (plannedVector.getNorm() > 0) {
+                plannedVector = plannedVector.times(velocityScalar.in(MetersPerSecond) / plannedVector.getNorm());
+            }
         } else {
             plannedVector = plannedVector.div(targetKeyPoint.getSecondsForSegment());
         }