From 669f0d18abd6c820bf9771b783e6db55094c8aa6 Mon Sep 17 00:00:00 2001 From: ariakrishnamoorthy Date: Sun, 26 Oct 2025 13:45:34 -0700 Subject: [PATCH] change isPoseJumpy to be based on speed instead of distance --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 3 ++- .../robot/subsystems/LocalizationCamera.java | 19 +++++++++++++++---- 3 files changed, 18 insertions(+), 6 deletions(-) diff --git a/build.gradle b/build.gradle index edd690d3..fd7cdb36 100644 --- a/build.gradle +++ b/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" } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b228fe29..f04b2209 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; } diff --git a/src/main/java/frc/robot/subsystems/LocalizationCamera.java b/src/main/java/frc/robot/subsystems/LocalizationCamera.java index ec0c44e6..16e985c4 100644 --- a/src/main/java/frc/robot/subsystems/LocalizationCamera.java +++ b/src/main/java/frc/robot/subsystems/LocalizationCamera.java @@ -61,7 +61,7 @@ public class LocalizationCamera { private PhotonPoseEstimator poseEstimator; private PhotonTrackedTarget m_target; - private ArrayList m_lastEstPoses = new ArrayList<>(); + private ArrayList 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; } }