From bee6eab6e5221a320aa3564746f3f625254fb980 Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Fri, 20 Mar 2026 18:30:46 +0200 Subject: [PATCH 01/12] estimated pose velocity calculation --- src/main/java/frc/robot/Robot.java | 1 + .../robot/poseestimator/IPoseEstimator.java | 2 ++ .../poseestimator/RobotPoseEstimation.java | 31 +++++++++++++++++++ .../WPILibPoseEstimatorWrapper.java | 22 +++++++++++++ 4 files changed, 56 insertions(+) create mode 100644 src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f60032edd5..c4404a3d56 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -260,6 +260,7 @@ public void periodic() { robotCommander.update(); poseEstimator.updateOdometry(swerve.getAllOdometryData()); + poseEstimator.updateEstimatedPose(); limelightFront.updateIsConnected(); limelightRight.updateIsConnected(); diff --git a/src/main/java/frc/robot/poseestimator/IPoseEstimator.java b/src/main/java/frc/robot/poseestimator/IPoseEstimator.java index de5d0854ee..1960a226ee 100644 --- a/src/main/java/frc/robot/poseestimator/IPoseEstimator.java +++ b/src/main/java/frc/robot/poseestimator/IPoseEstimator.java @@ -17,4 +17,6 @@ public interface IPoseEstimator extends IVisionEstimator, IOdometryEstimator { void log(); + void updateEstimatedPose(); + } diff --git a/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java b/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java new file mode 100644 index 0000000000..b21ea38834 --- /dev/null +++ b/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java @@ -0,0 +1,31 @@ +package frc.robot.poseestimator; + +import edu.wpi.first.math.geometry.Pose2d; + +public class RobotPoseEstimation { + + private double timestampSeconds = 0; + private Pose2d estimatedPose; + + public RobotPoseEstimation(double initialTimestampSeconds, Pose2d estimatedPose) { + this.timestampSeconds = initialTimestampSeconds; + this.estimatedPose = estimatedPose; + } + + public double getTimestampSeconds() { + return timestampSeconds; + } + + public Pose2d getEstimatedPose() { + return estimatedPose; + } + + public void setTimestampSeconds(double timestampSeconds) { + this.timestampSeconds = timestampSeconds; + } + + public void setEstimatedPose(Pose2d estimatedPose) { + this.estimatedPose = estimatedPose; + } + +} diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 3b2ca34d75..9fd1af4f17 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -14,12 +14,14 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import frc.robot.poseestimator.RobotPoseEstimation; import frc.robot.vision.RobotPoseObservation; import frc.robot.poseestimator.IPoseEstimator; import frc.robot.poseestimator.OdometryData; import frc.utils.buffers.RingBuffer.RingBuffer; import frc.utils.math.StatisticsMath; import frc.utils.pose.PoseUtil; +import frc.utils.time.TimeUtil; import org.littletonrobotics.junction.Logger; import java.util.Optional; @@ -35,6 +37,7 @@ public class WPILibPoseEstimatorWrapper implements IPoseEstimator { private final TimeInterpolatableBuffer imuXYAccelerationGBuffer; private RobotPoseObservation lastVisionObservation; private OdometryData lastOdometryData; + private RobotPoseEstimation lastEstimatedPose; private boolean isIMUOffsetCalibrated; public WPILibPoseEstimatorWrapper( @@ -72,6 +75,7 @@ public WPILibPoseEstimatorWrapper( this.imuYawBuffer = TimeInterpolatableBuffer.createBuffer(WPILibPoseEstimatorConstants.IMU_YAW_BUFFER_SIZE_SECONDS); this.imuXYAccelerationGBuffer = TimeInterpolatableBuffer .createBuffer(WPILibPoseEstimatorConstants.IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS); + this.lastEstimatedPose = new RobotPoseEstimation(initialTimestampSeconds, new Pose2d()); } @@ -131,6 +135,12 @@ public void updateVision(RobotPoseObservation... visionRobotPoseObservations) { } } + @Override + public void updateEstimatedPose() { + lastEstimatedPose.setTimestampSeconds(TimeUtil.getCurrentTimeSeconds()); + lastEstimatedPose.setEstimatedPose(getEstimatedPose()); + } + @Override public void resetPose(OdometryData odometryData, Pose2d poseMeters) { Logger.recordOutput(logPath + "/lastPoseResetTo", poseMeters); @@ -204,6 +214,8 @@ public void log() { WPILibPoseEstimatorConstants.MINIMUM_SKID_ROBOT_TO_MODULE_VELOCITY_DIFFERENCE_METERS_PER_SECOND ) ); + + Logger.recordOutput(logPath + "estimatedPoseVelocity", getEstimatedPoseVelocity(TimeUtil.getCurrentTimeSeconds())); } public void resetIsIMUOffsetCalibrated() { @@ -262,4 +274,14 @@ private Optional getEstimatedPoseToIMUYawDifference(Optional gyroYaw.map(yaw -> estimatedPose.getRotation().minus(yaw))); } + private Translation2d getEstimatedPoseVelocity(double timestamp) { + if (!lastEstimatedPose.getEstimatedPose().equals(new Pose2d())) + return new Translation2d( + (getEstimatedPose().getX() - lastEstimatedPose.getEstimatedPose().getX()) + / (timestamp - lastEstimatedPose.getTimestampSeconds()), + (getEstimatedPose().getY() - lastEstimatedPose.getEstimatedPose().getY()) / (timestamp - lastEstimatedPose.getTimestampSeconds()) + ); + return null; + } + } From 8e4127f20d83f82454286e514703b090aad4671a Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Sun, 22 Mar 2026 15:41:49 +0200 Subject: [PATCH 02/12] PE velocity --- src/main/java/frc/robot/Robot.java | 16 +++++-- .../robot/poseestimator/IPoseEstimator.java | 4 +- .../WPILibPoseEstimatorConstants.java | 2 + .../WPILibPoseEstimatorWrapper.java | 46 ++++++++++++++----- 4 files changed, 52 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a0cda7a192..44159357ff 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import com.pathplanner.lib.config.ModuleConfig; import com.pathplanner.lib.config.RobotConfig; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -55,6 +56,7 @@ import frc.utils.brakestate.BrakeMode; import frc.utils.brakestate.BrakeStateManager; import frc.utils.math.StandardDeviations2D; +import frc.utils.time.TimeUtil; import java.util.function.Supplier; @@ -273,7 +275,7 @@ public void periodic() { robotCommander.update(); poseEstimator.updateOdometry(swerve.getAllOdometryData()); - poseEstimator.updateEstimatedPose(); + poseEstimator.updateLastEstimatedPose(); limelightFront.updateIsConnected(); limelightRight.updateIsConnected(); @@ -288,8 +290,16 @@ public void periodic() { limelightLeft.getIndependentRobotPose().ifPresent(poseEstimator::updateVision); poseEstimator.log(); - ShootingCalculations - .updateShootingParams(poseEstimator.getEstimatedPose(), swerve.getFieldRelativeVelocity(), swerve.getIMUAngularVelocityRPS()[2]); + Pose2d currentEstimatedVelocity = poseEstimator.getFieldRelativeEstimatedPoseVelocity(TimeUtil.getCurrentTimeSeconds()); + ShootingCalculations.updateShootingParams( + poseEstimator.getEstimatedPose(), + new ChassisSpeeds( + currentEstimatedVelocity.getX(), + currentEstimatedVelocity.getY(), + currentEstimatedVelocity.getRotation().getRadians() + ), + swerve.getIMUAngularVelocityRPS()[2] + ); BatteryUtil.logStatus(); BusChain.logChainsStatuses(); diff --git a/src/main/java/frc/robot/poseestimator/IPoseEstimator.java b/src/main/java/frc/robot/poseestimator/IPoseEstimator.java index 1960a226ee..740308c4aa 100644 --- a/src/main/java/frc/robot/poseestimator/IPoseEstimator.java +++ b/src/main/java/frc/robot/poseestimator/IPoseEstimator.java @@ -17,6 +17,8 @@ public interface IPoseEstimator extends IVisionEstimator, IOdometryEstimator { void log(); - void updateEstimatedPose(); + void updateLastEstimatedPose(); + + Pose2d getFieldRelativeEstimatedPoseVelocity(double timestamp); } diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index 70c4fde1f3..1e75218955 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -35,4 +35,6 @@ public class WPILibPoseEstimatorConstants { public static double IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS = 2; + public static double ESTIMATED_POSE_VELOCITY_TOLERANCE = 0.2; + } diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 9fd1af4f17..0ddbc7ecda 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -20,6 +20,7 @@ import frc.robot.poseestimator.OdometryData; import frc.utils.buffers.RingBuffer.RingBuffer; import frc.utils.math.StatisticsMath; +import frc.utils.math.ToleranceMath; import frc.utils.pose.PoseUtil; import frc.utils.time.TimeUtil; import org.littletonrobotics.junction.Logger; @@ -37,7 +38,8 @@ public class WPILibPoseEstimatorWrapper implements IPoseEstimator { private final TimeInterpolatableBuffer imuXYAccelerationGBuffer; private RobotPoseObservation lastVisionObservation; private OdometryData lastOdometryData; - private RobotPoseEstimation lastEstimatedPose; + private final RobotPoseEstimation lastEstimatedPose; + private Pose2d lastEstimatedPoseVelocity; private boolean isIMUOffsetCalibrated; public WPILibPoseEstimatorWrapper( @@ -76,6 +78,7 @@ public WPILibPoseEstimatorWrapper( this.imuXYAccelerationGBuffer = TimeInterpolatableBuffer .createBuffer(WPILibPoseEstimatorConstants.IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS); this.lastEstimatedPose = new RobotPoseEstimation(initialTimestampSeconds, new Pose2d()); + this.lastEstimatedPoseVelocity = new Pose2d(); } @@ -116,6 +119,8 @@ public void updateOdometry(OdometryData data) { ); } + lastEstimatedPoseVelocity = getFieldRelativeEstimatedPoseVelocity(TimeUtil.getCurrentTimeSeconds()); + poseEstimator .updateWithTime(data.getTimestampSeconds(), Rotation2d.fromRadians(data.getIMUOrientation().get().getZ()), data.getWheelPositions()); imuYawBuffer.addSample(data.getTimestampSeconds(), Rotation2d.fromRadians(data.getIMUOrientation().get().getZ())); @@ -136,7 +141,7 @@ public void updateVision(RobotPoseObservation... visionRobotPoseObservations) { } @Override - public void updateEstimatedPose() { + public void updateLastEstimatedPose() { lastEstimatedPose.setTimestampSeconds(TimeUtil.getCurrentTimeSeconds()); lastEstimatedPose.setEstimatedPose(getEstimatedPose()); } @@ -177,6 +182,8 @@ public boolean isIMUOffsetCalibrated() { @Override public void log() { +// Logger.recordOutput(logPath + "estimatedPoseVelocity", getEstimatedPoseVelocity(TimeUtil.getCurrentTimeSeconds())); + Logger.recordOutput(logPath + "/estimatedPose", getEstimatedPose()); Logger.recordOutput(logPath + "/odometryPose", getOdometryPose()); Logger.recordOutput(logPath + "/lastOdometryUpdate", lastOdometryData.getTimestampSeconds()); @@ -214,8 +221,6 @@ public void log() { WPILibPoseEstimatorConstants.MINIMUM_SKID_ROBOT_TO_MODULE_VELOCITY_DIFFERENCE_METERS_PER_SECOND ) ); - - Logger.recordOutput(logPath + "estimatedPoseVelocity", getEstimatedPoseVelocity(TimeUtil.getCurrentTimeSeconds())); } public void resetIsIMUOffsetCalibrated() { @@ -238,6 +243,10 @@ private void updateVision(RobotPoseObservation visionRobotPoseObservation) { }); } + public Pose2d getLastEstimatedPoseVelocity() { + return lastEstimatedPoseVelocity; + } + private void addVisionMeasurement(RobotPoseObservation visionObservation) { poseEstimator.addVisionMeasurement( visionObservation.robotPose(), @@ -274,14 +283,27 @@ private Optional getEstimatedPoseToIMUYawDifference(Optional gyroYaw.map(yaw -> estimatedPose.getRotation().minus(yaw))); } - private Translation2d getEstimatedPoseVelocity(double timestamp) { - if (!lastEstimatedPose.getEstimatedPose().equals(new Pose2d())) - return new Translation2d( - (getEstimatedPose().getX() - lastEstimatedPose.getEstimatedPose().getX()) - / (timestamp - lastEstimatedPose.getTimestampSeconds()), - (getEstimatedPose().getY() - lastEstimatedPose.getEstimatedPose().getY()) / (timestamp - lastEstimatedPose.getTimestampSeconds()) - ); - return null; + @Override + public Pose2d getFieldRelativeEstimatedPoseVelocity(double timestamp) { + Twist2d changeInPose = new Twist2d( + getEstimatedPose().getX() - lastEstimatedPose.getEstimatedPose().getX(), + getEstimatedPose().getY() - lastEstimatedPose.getEstimatedPose().getY(), + getEstimatedPose().getRotation().getRadians() - lastEstimatedPose.getEstimatedPose().getRotation().getRadians() + ); + double dt = (timestamp - lastEstimatedPose.getTimestampSeconds()); + if (dt != 0) { + Pose2d poseVelocity = new Pose2d((changeInPose.dx / dt), (changeInPose.dy / dt), Rotation2d.fromRadians(changeInPose.dtheta / dt)); + if ( + ToleranceMath.isNear(0, poseVelocity.getX(), WPILibPoseEstimatorConstants.ESTIMATED_POSE_VELOCITY_TOLERANCE) + && ToleranceMath.isNear(0, poseVelocity.getY(), WPILibPoseEstimatorConstants.ESTIMATED_POSE_VELOCITY_TOLERANCE) + ) + return new Pose2d(); + Logger.recordOutput(logPath + "estimatedPoseVelocity", poseVelocity); + Logger.recordOutput(logPath + "/poseVelocity/dt", dt); + return poseVelocity; + } + return lastEstimatedPoseVelocity; } + } From d9f021c9c0cd8d52f1727c581f2fd2a2b46d7318 Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Sun, 22 Mar 2026 16:49:16 +0200 Subject: [PATCH 03/12] estimated velocity --- .../WPILibPoseEstimatorConstants.java | 7 +++- .../WPILibPoseEstimatorWrapper.java | 37 +++++++++++++++---- 2 files changed, 35 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index 1e75218955..084259f875 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -35,6 +35,11 @@ public class WPILibPoseEstimatorConstants { public static double IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS = 2; - public static double ESTIMATED_POSE_VELOCITY_TOLERANCE = 0.2; + public static double ESTIMATED_POSE_VELOCITY_X_TOLERANCE = 0; + + public static double ESTIMATED_POSE_VELOCITY_Y_TOLERANCE = 0; + + public static double ESTIMATED_POSE_VELOCITY_ANGLE_IN_RADIANS_TOLERANCE = 0; + } diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 0ddbc7ecda..1c42d810a3 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -285,21 +285,42 @@ private Optional getEstimatedPoseToIMUYawDifference(Optional Date: Thu, 26 Mar 2026 18:47:50 +0200 Subject: [PATCH 04/12] velocity calculation --- src/main/java/frc/robot/Robot.java | 2 +- .../robot/poseestimator/IPoseEstimator.java | 2 ++ .../WPILibPoseEstimatorConstants.java | 6 +++--- .../WPILibPoseEstimatorWrapper.java | 19 ++++++++++--------- 4 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 44159357ff..59fe2cb240 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -290,7 +290,7 @@ public void periodic() { limelightLeft.getIndependentRobotPose().ifPresent(poseEstimator::updateVision); poseEstimator.log(); - Pose2d currentEstimatedVelocity = poseEstimator.getFieldRelativeEstimatedPoseVelocity(TimeUtil.getCurrentTimeSeconds()); + Pose2d currentEstimatedVelocity = poseEstimator.getLastEstimatedPoseVelocity(); ShootingCalculations.updateShootingParams( poseEstimator.getEstimatedPose(), new ChassisSpeeds( diff --git a/src/main/java/frc/robot/poseestimator/IPoseEstimator.java b/src/main/java/frc/robot/poseestimator/IPoseEstimator.java index 740308c4aa..45ba38c9aa 100644 --- a/src/main/java/frc/robot/poseestimator/IPoseEstimator.java +++ b/src/main/java/frc/robot/poseestimator/IPoseEstimator.java @@ -17,6 +17,8 @@ public interface IPoseEstimator extends IVisionEstimator, IOdometryEstimator { void log(); + Pose2d getLastEstimatedPoseVelocity(); + void updateLastEstimatedPose(); Pose2d getFieldRelativeEstimatedPoseVelocity(double timestamp); diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index 084259f875..361e494147 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -35,11 +35,11 @@ public class WPILibPoseEstimatorConstants { public static double IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS = 2; - public static double ESTIMATED_POSE_VELOCITY_X_TOLERANCE = 0; + public static double ESTIMATED_POSE_VELOCITY_X_TOLERANCE = 0.02; - public static double ESTIMATED_POSE_VELOCITY_Y_TOLERANCE = 0; + public static double ESTIMATED_POSE_VELOCITY_Y_TOLERANCE = 0.02; - public static double ESTIMATED_POSE_VELOCITY_ANGLE_IN_RADIANS_TOLERANCE = 0; + public static double ESTIMATED_POSE_VELOCITY_ANGLE_IN_RADIANS_TOLERANCE = 0.02; } diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 1c42d810a3..b2309e1212 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -288,10 +288,10 @@ public Pose2d getFieldRelativeEstimatedPoseVelocity(double timestamp) { double dt = (timestamp - lastEstimatedPose.getTimestampSeconds()); if (dt != 0) { Pose2d poseVelocity = new Pose2d( - (getEstimatedPose().getX() - lastEstimatedPose.getEstimatedPose().getX() / dt), - (getEstimatedPose().getY() - lastEstimatedPose.getEstimatedPose().getY() / dt), + (getEstimatedPose().getX() - lastEstimatedPose.getEstimatedPose().getX()) / dt, + (getEstimatedPose().getY() - lastEstimatedPose.getEstimatedPose().getY()) / dt, Rotation2d.fromRadians( - getEstimatedPose().getRotation().getRadians() - lastEstimatedPose.getEstimatedPose().getRotation().getRadians() / dt + (getEstimatedPose().getRotation().getRadians() - lastEstimatedPose.getEstimatedPose().getRotation().getRadians()) / dt ) ); if ( @@ -304,8 +304,6 @@ public Pose2d getFieldRelativeEstimatedPoseVelocity(double timestamp) { ) ) return new Pose2d(); - Logger.recordOutput(logPath + "estimatedPoseVelocity", poseVelocity); - Logger.recordOutput(logPath + "/poseVelocity/dt", dt); boolean isXNearLast = ToleranceMath .isNear(lastEstimatedPoseVelocity.getX(), poseVelocity.getX(), WPILibPoseEstimatorConstants.ESTIMATED_POSE_VELOCITY_X_TOLERANCE); boolean isYNearLast = ToleranceMath @@ -316,12 +314,15 @@ public Pose2d getFieldRelativeEstimatedPoseVelocity(double timestamp) { WPILibPoseEstimatorConstants.ESTIMATED_POSE_VELOCITY_ANGLE_IN_RADIANS_TOLERANCE ); if (isXNearLast && isYNearLast && isRotationNearLast) - return new Pose2d( - (poseVelocity.getX() + lastEstimatedPoseVelocity.getX()) / 2, - (poseVelocity.getY() + lastEstimatedPoseVelocity.getY()) / 2, - Rotation2d.fromRadians(poseVelocity.getRotation().getRadians() + lastEstimatedPoseVelocity.getRotation().getRadians()) + poseVelocity = new Pose2d( + ((poseVelocity.getX() + lastEstimatedPoseVelocity.getX()) / 2), + ((poseVelocity.getY() + lastEstimatedPoseVelocity.getY()) / 2), + Rotation2d.fromRadians((poseVelocity.getRotation().getRadians() + lastEstimatedPoseVelocity.getRotation().getRadians())/2) ); + Logger.recordOutput(logPath + "estimatedPoseVelocity", poseVelocity); + Logger.recordOutput(logPath + "/poseVelocity/dt", dt); return poseVelocity; + } return lastEstimatedPoseVelocity; } From d0fa855ca350be47af55dcf853f7de0f4f7e5881 Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Thu, 26 Mar 2026 19:23:08 +0200 Subject: [PATCH 05/12] calculate velocity --- .../robot/poseestimator/IPoseEstimator.java | 3 +- .../poseestimator/RobotPoseEstimation.java | 12 +++ .../WPILibPoseEstimatorConstants.java | 6 +- .../WPILibPoseEstimatorWrapper.java | 74 ++++++++----------- src/main/java/frc/utils/pose/PoseUtil.java | 14 ++++ 5 files changed, 60 insertions(+), 49 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/IPoseEstimator.java b/src/main/java/frc/robot/poseestimator/IPoseEstimator.java index 45ba38c9aa..7ef5b6b966 100644 --- a/src/main/java/frc/robot/poseestimator/IPoseEstimator.java +++ b/src/main/java/frc/robot/poseestimator/IPoseEstimator.java @@ -1,6 +1,7 @@ package frc.robot.poseestimator; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import java.util.Optional; @@ -21,6 +22,6 @@ public interface IPoseEstimator extends IVisionEstimator, IOdometryEstimator { void updateLastEstimatedPose(); - Pose2d getFieldRelativeEstimatedPoseVelocity(double timestamp); + RobotPoseEstimation getFieldRelativeEstimatedPoseVelocity(double timestamp, ChassisSpeeds swerveVelocity); } diff --git a/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java b/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java index b21ea38834..f41396fc4b 100644 --- a/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java +++ b/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java @@ -1,6 +1,7 @@ package frc.robot.poseestimator; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; public class RobotPoseEstimation { @@ -12,6 +13,11 @@ public RobotPoseEstimation(double initialTimestampSeconds, Pose2d estimatedPose) this.estimatedPose = estimatedPose; } + public RobotPoseEstimation(double timestampSeconds){ + this.timestampSeconds = timestampSeconds; + this.estimatedPose = new Pose2d(); + } + public double getTimestampSeconds() { return timestampSeconds; } @@ -20,6 +26,12 @@ public Pose2d getEstimatedPose() { return estimatedPose; } + public double getX(){ return estimatedPose.getX(); } + + public double getY(){ return estimatedPose.getY();} + + public Rotation2d getRotation(){ return estimatedPose.getRotation(); } + public void setTimestampSeconds(double timestampSeconds) { this.timestampSeconds = timestampSeconds; } diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index 361e494147..6e42644451 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -35,11 +35,7 @@ public class WPILibPoseEstimatorConstants { public static double IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS = 2; - public static double ESTIMATED_POSE_VELOCITY_X_TOLERANCE = 0.02; - - public static double ESTIMATED_POSE_VELOCITY_Y_TOLERANCE = 0.02; - - public static double ESTIMATED_POSE_VELOCITY_ANGLE_IN_RADIANS_TOLERANCE = 0.02; + public static Pose2d ESTIMATED_POSE_VELOCITY_TOLERANCE = new Pose2d(); } diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index b2309e1212..3a9f297196 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -8,10 +8,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.Odometry; -import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.kinematics.*; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import frc.robot.poseestimator.RobotPoseEstimation; @@ -39,7 +36,7 @@ public class WPILibPoseEstimatorWrapper implements IPoseEstimator { private RobotPoseObservation lastVisionObservation; private OdometryData lastOdometryData; private final RobotPoseEstimation lastEstimatedPose; - private Pose2d lastEstimatedPoseVelocity; + private RobotPoseEstimation lastEstimatedPoseVelocity; private boolean isIMUOffsetCalibrated; public WPILibPoseEstimatorWrapper( @@ -78,7 +75,7 @@ public WPILibPoseEstimatorWrapper( this.imuXYAccelerationGBuffer = TimeInterpolatableBuffer .createBuffer(WPILibPoseEstimatorConstants.IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS); this.lastEstimatedPose = new RobotPoseEstimation(initialTimestampSeconds, new Pose2d()); - this.lastEstimatedPoseVelocity = new Pose2d(); + this.lastEstimatedPoseVelocity = new RobotPoseEstimation(initialTimestampSeconds, new Pose2d()); } @@ -119,7 +116,7 @@ public void updateOdometry(OdometryData data) { ); } - lastEstimatedPoseVelocity = getFieldRelativeEstimatedPoseVelocity(TimeUtil.getCurrentTimeSeconds()); + lastEstimatedPoseVelocity = calculateEstimatedVelocity(TimeUtil.getCurrentTimeSeconds()); poseEstimator .updateWithTime(data.getTimestampSeconds(), Rotation2d.fromRadians(data.getIMUOrientation().get().getZ()), data.getWheelPositions()); @@ -244,7 +241,7 @@ private void updateVision(RobotPoseObservation visionRobotPoseObservation) { } public Pose2d getLastEstimatedPoseVelocity() { - return lastEstimatedPoseVelocity; + return lastEstimatedPoseVelocity.getEstimatedPose(); } private void addVisionMeasurement(RobotPoseObservation visionObservation) { @@ -284,48 +281,39 @@ private Optional getEstimatedPoseToIMUYawDifference(Optional= minimumCollisionIMUAccelerationG; } From 355c52c1655707ed74415af6e81365bd87b48880 Mon Sep 17 00:00:00 2001 From: DanielleCarmi09 Date: Thu, 26 Mar 2026 19:29:59 +0200 Subject: [PATCH 06/12] spotless --- src/main/java/frc/robot/Robot.java | 1 - .../poseestimator/RobotPoseEstimation.java | 14 +++-- .../WPILibPoseEstimatorWrapper.java | 62 ++++++++++++++----- .../java/frc/utils/math/ToleranceMath.java | 8 +++ src/main/java/frc/utils/pose/PoseUtil.java | 14 ----- 5 files changed, 63 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 59fe2cb240..d98e4f2f6e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -56,7 +56,6 @@ import frc.utils.brakestate.BrakeMode; import frc.utils.brakestate.BrakeStateManager; import frc.utils.math.StandardDeviations2D; -import frc.utils.time.TimeUtil; import java.util.function.Supplier; diff --git a/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java b/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java index f41396fc4b..fe6576f286 100644 --- a/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java +++ b/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java @@ -13,7 +13,7 @@ public RobotPoseEstimation(double initialTimestampSeconds, Pose2d estimatedPose) this.estimatedPose = estimatedPose; } - public RobotPoseEstimation(double timestampSeconds){ + public RobotPoseEstimation(double timestampSeconds) { this.timestampSeconds = timestampSeconds; this.estimatedPose = new Pose2d(); } @@ -26,11 +26,17 @@ public Pose2d getEstimatedPose() { return estimatedPose; } - public double getX(){ return estimatedPose.getX(); } + public double getX() { + return estimatedPose.getX(); + } - public double getY(){ return estimatedPose.getY();} + public double getY() { + return estimatedPose.getY(); + } - public Rotation2d getRotation(){ return estimatedPose.getRotation(); } + public Rotation2d getRotation() { + return estimatedPose.getRotation(); + } public void setTimestampSeconds(double timestampSeconds) { this.timestampSeconds = timestampSeconds; diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 3a9f297196..7759c3ca3f 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -283,37 +283,65 @@ private Optional getEstimatedPoseToIMUYawDifference(Optional= minimumCollisionIMUAccelerationG; } From dfda23655e530c95f9529cd4fa91237353acfc67 Mon Sep 17 00:00:00 2001 From: daniel Date: Sun, 7 Jun 2026 18:57:19 +0300 Subject: [PATCH 07/12] changes --- .../WPILibPoseEstimator/WPILibPoseEstimatorConstants.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index 6e42644451..edeb3be6f4 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -37,5 +37,4 @@ public class WPILibPoseEstimatorConstants { public static Pose2d ESTIMATED_POSE_VELOCITY_TOLERANCE = new Pose2d(); - } From 493e27f94240be904902c56db69d0f3d5d9328b0 Mon Sep 17 00:00:00 2001 From: daniel Date: Sun, 7 Jun 2026 19:01:32 +0300 Subject: [PATCH 08/12] more changes --- .../WPILibPoseEstimatorWrapper.java | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 7759c3ca3f..1b6ce224cf 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -78,7 +78,6 @@ public WPILibPoseEstimatorWrapper( this.lastEstimatedPoseVelocity = new RobotPoseEstimation(initialTimestampSeconds, new Pose2d()); } - @Override public Pose2d getEstimatedPose() { return poseEstimator.getEstimatedPosition(); @@ -179,8 +178,6 @@ public boolean isIMUOffsetCalibrated() { @Override public void log() { -// Logger.recordOutput(logPath + "estimatedPoseVelocity", getEstimatedPoseVelocity(TimeUtil.getCurrentTimeSeconds())); - Logger.recordOutput(logPath + "/estimatedPose", getEstimatedPose()); Logger.recordOutput(logPath + "/odometryPose", getOdometryPose()); Logger.recordOutput(logPath + "/lastOdometryUpdate", lastOdometryData.getTimestampSeconds()); @@ -296,14 +293,14 @@ public RobotPoseEstimation getFieldRelativeEstimatedPoseVelocity(double timestam } private RobotPoseEstimation calculateEstimatedVelocity(double timestamp) { - double dt = (timestamp - lastEstimatedPoseVelocity.getTimestampSeconds()); - if (dt != 0) { + double deltaTime = (timestamp - lastEstimatedPoseVelocity.getTimestampSeconds()); + if (deltaTime != 0) { RobotPoseEstimation poseVelocity = new RobotPoseEstimation( timestamp, new Pose2d( - (getEstimatedPose().getX() - lastEstimatedPose.getX()) / dt, - (getEstimatedPose().getY() - lastEstimatedPose.getY()) / dt, - Rotation2d.fromRadians((getEstimatedPose().getRotation().getRadians() - lastEstimatedPose.getRotation().getRadians()) / dt) + (getEstimatedPose().getX() - lastEstimatedPose.getX()) / deltaTime, + (getEstimatedPose().getY() - lastEstimatedPose.getY()) / deltaTime, + Rotation2d.fromRadians((getEstimatedPose().getRotation().getRadians() - lastEstimatedPose.getRotation().getRadians()) / deltaTime) ) ); if ( @@ -343,5 +340,4 @@ private boolean useEstimatedVelocity(ChassisSpeeds swerveVelocity, RobotPoseEsti ); } - -} +} \ No newline at end of file From c1b74876359f65c2bded79326f5529752dcfcb78 Mon Sep 17 00:00:00 2001 From: daniel Date: Sun, 7 Jun 2026 19:03:10 +0300 Subject: [PATCH 09/12] spotless --- .../WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index 1b6ce224cf..e131c5490f 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -300,7 +300,8 @@ private RobotPoseEstimation calculateEstimatedVelocity(double timestamp) { new Pose2d( (getEstimatedPose().getX() - lastEstimatedPose.getX()) / deltaTime, (getEstimatedPose().getY() - lastEstimatedPose.getY()) / deltaTime, - Rotation2d.fromRadians((getEstimatedPose().getRotation().getRadians() - lastEstimatedPose.getRotation().getRadians()) / deltaTime) + Rotation2d + .fromRadians((getEstimatedPose().getRotation().getRadians() - lastEstimatedPose.getRotation().getRadians()) / deltaTime) ) ); if ( @@ -340,4 +341,4 @@ private boolean useEstimatedVelocity(ChassisSpeeds swerveVelocity, RobotPoseEsti ); } -} \ No newline at end of file +} From 267a338c40011a17be59e3e47a3a71e98cee4443 Mon Sep 17 00:00:00 2001 From: daniel Date: Fri, 19 Jun 2026 23:43:50 +0300 Subject: [PATCH 10/12] many changes need testing --- src/main/java/frc/robot/Robot.java | 17 +-- .../robot/poseestimator/IPoseEstimator.java | 4 +- .../poseestimator/RobotPoseEstimation.java | 49 -------- .../WPILibPoseEstimatorConstants.java | 7 +- .../WPILibPoseEstimatorWrapper.java | 110 +++++++----------- .../java/frc/utils/math/ToleranceMath.java | 6 + 6 files changed, 61 insertions(+), 132 deletions(-) delete mode 100644 src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 30f129a830..67d343fc45 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -349,7 +349,6 @@ public void periodic() { robotCommander.update(); poseEstimator.updateOdometry(swerve.getAllOdometryData()); - poseEstimator.updateLastEstimatedPose(); limelightFront.updateIsConnected(); limelightRight.updateIsConnected(); @@ -363,17 +362,13 @@ public void periodic() { limelightRight.getIndependentRobotPose().ifPresent(poseEstimator::updateVision); limelightLeft.getIndependentRobotPose().ifPresent(poseEstimator::updateVision); + poseEstimator.updateLastEstimatedPose(); poseEstimator.log(); - Pose2d currentEstimatedVelocity = poseEstimator.getLastEstimatedPoseVelocity(); - ShootingCalculations.updateShootingParams( - poseEstimator.getEstimatedPose(), - new ChassisSpeeds( - currentEstimatedVelocity.getX(), - currentEstimatedVelocity.getY(), - currentEstimatedVelocity.getRotation().getRadians() - ), - swerve.getIMUAngularVelocityRPS()[2] - ); + + ChassisSpeeds currentEstimatedVelocity = poseEstimator.getFieldRelativeEstimatedVelocity(swerve.getFieldRelativeVelocity()); + + ShootingCalculations + .updateShootingParams(poseEstimator.getEstimatedPose(), currentEstimatedVelocity, swerve.getIMUAngularVelocityRPS()[2]); Logger.recordOutput("isRobotAutoWinningAlliance", HubUtil.isRobotAllianceAutoWinnerForLog()); Logger.recordOutput("lastBallThrownTimestamp", lastBallThrownTimestamp.get()); diff --git a/src/main/java/frc/robot/poseestimator/IPoseEstimator.java b/src/main/java/frc/robot/poseestimator/IPoseEstimator.java index 7ef5b6b966..df3859bb55 100644 --- a/src/main/java/frc/robot/poseestimator/IPoseEstimator.java +++ b/src/main/java/frc/robot/poseestimator/IPoseEstimator.java @@ -18,10 +18,10 @@ public interface IPoseEstimator extends IVisionEstimator, IOdometryEstimator { void log(); - Pose2d getLastEstimatedPoseVelocity(); + ChassisSpeeds getLastEstimatedVelocity(); void updateLastEstimatedPose(); - RobotPoseEstimation getFieldRelativeEstimatedPoseVelocity(double timestamp, ChassisSpeeds swerveVelocity); + ChassisSpeeds getFieldRelativeEstimatedVelocity(ChassisSpeeds swerveVelocity); } diff --git a/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java b/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java deleted file mode 100644 index fe6576f286..0000000000 --- a/src/main/java/frc/robot/poseestimator/RobotPoseEstimation.java +++ /dev/null @@ -1,49 +0,0 @@ -package frc.robot.poseestimator; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; - -public class RobotPoseEstimation { - - private double timestampSeconds = 0; - private Pose2d estimatedPose; - - public RobotPoseEstimation(double initialTimestampSeconds, Pose2d estimatedPose) { - this.timestampSeconds = initialTimestampSeconds; - this.estimatedPose = estimatedPose; - } - - public RobotPoseEstimation(double timestampSeconds) { - this.timestampSeconds = timestampSeconds; - this.estimatedPose = new Pose2d(); - } - - public double getTimestampSeconds() { - return timestampSeconds; - } - - public Pose2d getEstimatedPose() { - return estimatedPose; - } - - public double getX() { - return estimatedPose.getX(); - } - - public double getY() { - return estimatedPose.getY(); - } - - public Rotation2d getRotation() { - return estimatedPose.getRotation(); - } - - public void setTimestampSeconds(double timestampSeconds) { - this.timestampSeconds = timestampSeconds; - } - - public void setEstimatedPose(Pose2d estimatedPose) { - this.estimatedPose = estimatedPose; - } - -} diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index edeb3be6f4..2514e8bd56 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc.utils.math.StandardDeviations2D; @@ -35,6 +36,10 @@ public class WPILibPoseEstimatorConstants { public static double IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS = 2; - public static Pose2d ESTIMATED_POSE_VELOCITY_TOLERANCE = new Pose2d(); + /** + * Tolerance for detecting swerve wheel slip and smoothing noise. 0.5 m/s matches the skid tolerance threshold. If the difference between raw + * swerve velocity and estimated velocity exceeds this, the wheels are slipping. + */ + public static final ChassisSpeeds ESTIMATED_VELOCITY_TOLERANCE = new ChassisSpeeds(0.5, 0.5, Math.toRadians(15.0)); } diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index e131c5490f..eb7dfc099d 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -11,10 +11,10 @@ import edu.wpi.first.math.kinematics.*; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import frc.robot.poseestimator.RobotPoseEstimation; import frc.robot.vision.RobotPoseObservation; import frc.robot.poseestimator.IPoseEstimator; import frc.robot.poseestimator.OdometryData; +import frc.utils.TimedValue; import frc.utils.buffers.RingBuffer.RingBuffer; import frc.utils.math.StatisticsMath; import frc.utils.math.ToleranceMath; @@ -35,8 +35,8 @@ public class WPILibPoseEstimatorWrapper implements IPoseEstimator { private final TimeInterpolatableBuffer imuXYAccelerationGBuffer; private RobotPoseObservation lastVisionObservation; private OdometryData lastOdometryData; - private final RobotPoseEstimation lastEstimatedPose; - private RobotPoseEstimation lastEstimatedPoseVelocity; + private final TimedValue lastEstimatedPose; + private final TimedValue lastEstimatedVelocity; private boolean isIMUOffsetCalibrated; public WPILibPoseEstimatorWrapper( @@ -74,8 +74,8 @@ public WPILibPoseEstimatorWrapper( this.imuYawBuffer = TimeInterpolatableBuffer.createBuffer(WPILibPoseEstimatorConstants.IMU_YAW_BUFFER_SIZE_SECONDS); this.imuXYAccelerationGBuffer = TimeInterpolatableBuffer .createBuffer(WPILibPoseEstimatorConstants.IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS); - this.lastEstimatedPose = new RobotPoseEstimation(initialTimestampSeconds, new Pose2d()); - this.lastEstimatedPoseVelocity = new RobotPoseEstimation(initialTimestampSeconds, new Pose2d()); + this.lastEstimatedPose = new TimedValue<>(new Pose2d(), initialTimestampSeconds); + this.lastEstimatedVelocity = new TimedValue<>(new ChassisSpeeds(), initialTimestampSeconds); } @Override @@ -115,8 +115,6 @@ public void updateOdometry(OdometryData data) { ); } - lastEstimatedPoseVelocity = calculateEstimatedVelocity(TimeUtil.getCurrentTimeSeconds()); - poseEstimator .updateWithTime(data.getTimestampSeconds(), Rotation2d.fromRadians(data.getIMUOrientation().get().getZ()), data.getWheelPositions()); imuYawBuffer.addSample(data.getTimestampSeconds(), Rotation2d.fromRadians(data.getIMUOrientation().get().getZ())); @@ -138,8 +136,34 @@ public void updateVision(RobotPoseObservation... visionRobotPoseObservations) { @Override public void updateLastEstimatedPose() { - lastEstimatedPose.setTimestampSeconds(TimeUtil.getCurrentTimeSeconds()); - lastEstimatedPose.setEstimatedPose(getEstimatedPose()); + double timestamp = TimeUtil.getCurrentTimeSeconds(); + double deltaTime = timestamp - lastEstimatedVelocity.getTimestamp(); + if (deltaTime > 0) { + Pose2d currentPose = getEstimatedPose(); + ChassisSpeeds rawVelocity = new ChassisSpeeds( + (currentPose.getX() - lastEstimatedPose.getValue().getX()) / deltaTime, + (currentPose.getY() - lastEstimatedPose.getValue().getY()) / deltaTime, + (currentPose.getRotation().getRadians() - lastEstimatedPose.getValue().getRotation().getRadians()) / deltaTime + ); + + if (ToleranceMath.isNear(new ChassisSpeeds(), rawVelocity, WPILibPoseEstimatorConstants.ESTIMATED_VELOCITY_TOLERANCE)) { + lastEstimatedVelocity.setValue(new ChassisSpeeds()); + } else if ( + ToleranceMath.isNear(lastEstimatedVelocity.getValue(), rawVelocity, WPILibPoseEstimatorConstants.ESTIMATED_VELOCITY_TOLERANCE) + ) { + lastEstimatedVelocity.setValue( + new ChassisSpeeds( + (rawVelocity.vxMetersPerSecond + lastEstimatedVelocity.getValue().vxMetersPerSecond) / 2.0, + (rawVelocity.vyMetersPerSecond + lastEstimatedVelocity.getValue().vyMetersPerSecond) / 2.0, + (rawVelocity.omegaRadiansPerSecond + lastEstimatedVelocity.getValue().omegaRadiansPerSecond) / 2.0 + ) + ); + } + // If neither condition is met, it is a massive vision spike. We do NOT update the value, preserving the safe velocity. + lastEstimatedVelocity.setTimestamp(timestamp); + } + lastEstimatedPose.setTimestamp(timestamp); + lastEstimatedPose.setValue(getEstimatedPose()); } @Override @@ -237,8 +261,8 @@ private void updateVision(RobotPoseObservation visionRobotPoseObservation) { }); } - public Pose2d getLastEstimatedPoseVelocity() { - return lastEstimatedPoseVelocity.getEstimatedPose(); + public ChassisSpeeds getLastEstimatedVelocity() { + return lastEstimatedVelocity.getValue(); } private void addVisionMeasurement(RobotPoseObservation visionObservation) { @@ -278,67 +302,15 @@ private Optional getEstimatedPoseToIMUYawDifference(Optional Date: Sat, 20 Jun 2026 00:10:47 +0300 Subject: [PATCH 11/12] remove unused func --- src/main/java/frc/utils/math/ToleranceMath.java | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/utils/math/ToleranceMath.java b/src/main/java/frc/utils/math/ToleranceMath.java index ebde7448fc..f8e31366b9 100644 --- a/src/main/java/frc/utils/math/ToleranceMath.java +++ b/src/main/java/frc/utils/math/ToleranceMath.java @@ -35,14 +35,6 @@ public static boolean isNear(double wanted, double actual, double tolerance) { return Math.abs(wanted - actual) <= tolerance; } - public static boolean isPoseNear(Pose2d pose1, Pose2d pose2, Pose2d poseTolerance) { - boolean isXNear = ToleranceMath.isNear(pose1.getX(), pose2.getX(), poseTolerance.getX()); - boolean isYNear = ToleranceMath.isNear(pose1.getY(), pose2.getY(), poseTolerance.getY()); - boolean isRotationNear = ToleranceMath - .isNear(pose1.getRotation().getRadians(), pose2.getRotation().getRadians(), poseTolerance.getRotation().getRadians()); - return isXNear && isYNear && isRotationNear; - } - public static boolean isInRange(double value, double min, double max, double tolerance) { return (min - tolerance) <= value && value <= (max + tolerance); } @@ -63,10 +55,10 @@ public static Rotation2d clamp(Rotation2d angle, Rotation2d minAngle, Rotation2d return Rotation2d.fromRadians(MathUtil.clamp(angle.getRadians(), minAngle.getRadians(), maxAngle.getRadians())); } - public static boolean isNear(ChassisSpeeds wanted, ChassisSpeeds actual, ChassisSpeeds tolerance) { - return isNear(wanted.vxMetersPerSecond, actual.vxMetersPerSecond, tolerance.vxMetersPerSecond) - && isNear(wanted.vyMetersPerSecond, actual.vyMetersPerSecond, tolerance.vyMetersPerSecond) - && isNear(wanted.omegaRadiansPerSecond, actual.omegaRadiansPerSecond, tolerance.omegaRadiansPerSecond); + public static boolean isNear(ChassisSpeeds wantedSpeed, ChassisSpeeds actualSpeed, ChassisSpeeds tolerance) { + return isNear(wantedSpeed.vxMetersPerSecond, actualSpeed.vxMetersPerSecond, tolerance.vxMetersPerSecond) + && isNear(wantedSpeed.vyMetersPerSecond, actualSpeed.vyMetersPerSecond, tolerance.vyMetersPerSecond) + && isNear(wantedSpeed.omegaRadiansPerSecond, actualSpeed.omegaRadiansPerSecond, tolerance.omegaRadiansPerSecond); } } From 4ea7c3012f99643ca134d32df54190b3bcdabbf3 Mon Sep 17 00:00:00 2001 From: daniel Date: Mon, 22 Jun 2026 18:46:00 +0300 Subject: [PATCH 12/12] mini change --- .../WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index eb7dfc099d..49e11ced94 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -33,10 +33,10 @@ public class WPILibPoseEstimatorWrapper implements IPoseEstimator { private final RingBuffer poseToIMUYawDifferenceBuffer; private final TimeInterpolatableBuffer imuYawBuffer; private final TimeInterpolatableBuffer imuXYAccelerationGBuffer; - private RobotPoseObservation lastVisionObservation; + private final TimedValue lastEstimatedPose; + private final TimedValue lastEstimatedVelocity; + private RobotPoseObservation lastVisionObservation; private OdometryData lastOdometryData; - private final TimedValue lastEstimatedPose; - private final TimedValue lastEstimatedVelocity; private boolean isIMUOffsetCalibrated; public WPILibPoseEstimatorWrapper(