change isPoseJumpy to be based on speed instead of distance

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.3.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.2"
     id "com.peterabeles.gversion" version "1.10"
 }
 

src/main/java/frc/robot/Constants.java

@@ -263,7 +263,8 @@ public static class VisionConstants {
     public static final double TARGET_HEIGHT = 0.3048; // in meters to the middle of the apriltag on reef
     public static final double TARGET_PITCH = 0;
 
-    public static final double MAX_AVG_DIST_BETWEEN_LAST_EST_POSES = 0.3; // in meters 
+    public static final double MAX_AVG_DIST_BETWEEN_LAST_EST_POSES = 0.3;
+    public static final double MAX_AVG_SPEED_BETWEEN_LAST_EST_POSES = MAX_AVG_DIST_BETWEEN_LAST_EST_POSES * 50.; // in meters 
     public static final int NUM_LAST_EST_POSES = 3;
   }
 

src/main/java/frc/robot/subsystems/LocalizationCamera.java

@@ -61,7 +61,7 @@ public class LocalizationCamera {
   private PhotonPoseEstimator poseEstimator;
 
   private PhotonTrackedTarget m_target;
-  private ArrayList<Pose2d> m_lastEstPoses = new ArrayList<>();
+  private ArrayList<EstimatedRobotPose> m_lastEstPoses = new ArrayList<>();
 
 
   // LocalizationCamera is a camera object (each camera = separate LocalizationCamera object)
@@ -171,7 +171,7 @@ public void findTarget() {
                 estimatedRobotPose = poseEstimatorOutput.get(); 
 
                 // update our last n poses
-                m_lastEstPoses.add(estimatedRobotPose.estimatedPose.toPose2d());
+                m_lastEstPoses.add(estimatedRobotPose);
                 if (m_lastEstPoses.size() > VisionConstants.NUM_LAST_EST_POSES) {
                   m_lastEstPoses.remove(0);
                 }
@@ -255,15 +255,26 @@ public boolean isEstPoseJumpy() {
     }
 
     double totalDistance = 0;
+    double totalTime = 0;
 
     for (int i = 0; i < m_lastEstPoses.size() - 1; i++) {
       // add distance between ith pose and i+1th pose
-      totalDistance += Math.abs(m_lastEstPoses.get(i).minus(m_lastEstPoses.get(i + 1)).getTranslation().getNorm());
+      totalDistance += Math.abs(m_lastEstPoses.get(i).estimatedPose.toPose2d().minus(m_lastEstPoses.get(i + 1).estimatedPose.toPose2d()).getTranslation().getNorm());
+      totalTime += Math.abs(m_lastEstPoses.get(i).timestampSeconds - m_lastEstPoses.get(i+1).timestampSeconds);
     }
 
     double avgDist = totalDistance / m_lastEstPoses.size();
+    double avgTime = totalTime / m_lastEstPoses.size();
+    if (avgTime == 0){
+      return true;
+    }
+    double avgSpeed = avgDist/avgTime;
+
     SmartDashboard.putNumber("vision/" + m_cameraName + "/avgDistBetweenLastEstPoses", avgDist);
+    SmartDashboard.putNumber("vision/" + m_cameraName + "/avgSpeedBetweenLastEstPoses", avgSpeed);
+    SmartDashboard.putNumber("vision/" + m_cameraName + "/avgTimeBetweenLastEstPoses", avgTime);
+
 
-    return avgDist > VisionConstants.MAX_AVG_DIST_BETWEEN_LAST_EST_POSES;
+    return avgSpeed > VisionConstants.MAX_AVG_SPEED_BETWEEN_LAST_EST_POSES;
   }
 }