ok fine i'll do try catch

cachemoney8096 · Jan 20, 2024 · 3e3bc51 · 3e3bc51
1 parent f484549
commit 3e3bc51
Showing 1 changed file with 7 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/utils/PoseBuffer.java b/src/main/java/frc/robot/utils/PoseBuffer.java
@@ -23,14 +23,17 @@ public int size(){
     }
 
     public Optional<Pose2d> getPoseAtTimestamp(double timestamp){
-        if(buffer.size() < 2){
-            return Optional.empty();
-        }
         if(buffer.getFromFirst(0).getFirst() >= timestamp && buffer.getFromLast(0).getFirst() <= timestamp){
             for(int i = 0; i < buffer.size(); i++){
                 if(buffer.getFromFirst(i).getFirst() >= timestamp && buffer.getFromFirst(i+1).getFirst() <= timestamp){
                     Pair<Double, Pose2d> timedPoseA = buffer.getFromFirst(i);
-                    Pair<Double, Pose2d> timedPoseB = buffer.getFromFirst(i+1);
+                    Pair<Double, Pose2d> timedPoseB;
+                    try{
+                        timedPoseB = buffer.getFromFirst(i+1);
+                    }
+                    catch(Exception e){
+                        return Optional.of(timedPoseA.getSecond());
+                    }
                     double percentage = (timedPoseA.getFirst()-timestamp)/(timedPoseA.getFirst()-timedPoseB.getFirst());
                     return Optional.of(timedPoseA.getSecond().interpolate(timedPoseB.getSecond(), percentage));
                 }