diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 111a2c3f61..e95d8ee1fd 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.interpolation.Interpolator; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.system.plant.DCMotor; @@ -363,9 +364,13 @@ public void periodic() { limelightRight.getIndependentRobotPose().ifPresent(poseEstimator::updateVision); limelightLeft.getIndependentRobotPose().ifPresent(poseEstimator::updateVision); + poseEstimator.updateLastEstimatedPose(); poseEstimator.log(); + + ChassisSpeeds currentEstimatedVelocity = poseEstimator.getFieldRelativeEstimatedVelocity(swerve.getFieldRelativeVelocity()); + ShootingCalculations - .updateShootingParams(poseEstimator.getEstimatedPose(), swerve.getFieldRelativeVelocity(), swerve.getIMUAngularVelocityRPS()[2]); + .updateShootingParams(poseEstimator.getEstimatedPose(), currentEstimatedVelocity, swerve.getIMUAngularVelocityRPS()[2]); Logger.recordOutput("lastBallThrownTimestamp", lastBallThrownTimestamp.get()); Logger.recordOutput( diff --git a/src/main/java/frc/robot/poseestimator/IPoseEstimator.java b/src/main/java/frc/robot/poseestimator/IPoseEstimator.java index de5d0854ee..df3859bb55 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; @@ -17,4 +18,10 @@ public interface IPoseEstimator extends IVisionEstimator, IOdometryEstimator { void log(); + ChassisSpeeds getLastEstimatedVelocity(); + + void updateLastEstimatedPose(); + + ChassisSpeeds getFieldRelativeEstimatedVelocity(ChassisSpeeds swerveVelocity); + } diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index 70c4fde1f3..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,4 +36,10 @@ public class WPILibPoseEstimatorConstants { public static double IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS = 2; + /** + * 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 3b2ca34d75..49e11ced94 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -8,18 +8,18 @@ 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.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; import frc.utils.pose.PoseUtil; +import frc.utils.time.TimeUtil; import org.littletonrobotics.junction.Logger; import java.util.Optional; @@ -33,7 +33,9 @@ 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 boolean isIMUOffsetCalibrated; @@ -72,9 +74,10 @@ 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 TimedValue<>(new Pose2d(), initialTimestampSeconds); + this.lastEstimatedVelocity = new TimedValue<>(new ChassisSpeeds(), initialTimestampSeconds); } - @Override public Pose2d getEstimatedPose() { return poseEstimator.getEstimatedPosition(); @@ -131,6 +134,38 @@ public void updateVision(RobotPoseObservation... visionRobotPoseObservations) { } } + @Override + public void updateLastEstimatedPose() { + 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 public void resetPose(OdometryData odometryData, Pose2d poseMeters) { Logger.recordOutput(logPath + "/lastPoseResetTo", poseMeters); @@ -226,6 +261,10 @@ private void updateVision(RobotPoseObservation visionRobotPoseObservation) { }); } + public ChassisSpeeds getLastEstimatedVelocity() { + return lastEstimatedVelocity.getValue(); + } + private void addVisionMeasurement(RobotPoseObservation visionObservation) { poseEstimator.addVisionMeasurement( visionObservation.robotPose(), @@ -262,4 +301,16 @@ private Optional getEstimatedPoseToIMUYawDifference(Optional gyroYaw.map(yaw -> estimatedPose.getRotation().minus(yaw))); } + @Override + public ChassisSpeeds getFieldRelativeEstimatedVelocity(ChassisSpeeds swerveVelocity) { + if (useEstimatedVelocity(swerveVelocity, lastEstimatedVelocity.getValue())) { + return lastEstimatedVelocity.getValue(); + } + return swerveVelocity; + } + + private boolean useEstimatedVelocity(ChassisSpeeds swerveVelocity, ChassisSpeeds estimatedVelocity) { + return !ToleranceMath.isNear(swerveVelocity, estimatedVelocity, WPILibPoseEstimatorConstants.ESTIMATED_VELOCITY_TOLERANCE); + } + } diff --git a/src/main/java/frc/utils/math/ToleranceMath.java b/src/main/java/frc/utils/math/ToleranceMath.java index bffa40a24b..f8e31366b9 100644 --- a/src/main/java/frc/utils/math/ToleranceMath.java +++ b/src/main/java/frc/utils/math/ToleranceMath.java @@ -55,4 +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 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); + } + }