From: iefomit Date: Sat, 28 Mar 2026 19:28:51 +0000 (-0700) Subject: making estimator to correct for gyro X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=445c27d9474a79c76634f01234e8a16a7f72883c;p=FRC2026.git making estimator to correct for gyro --- diff --git a/src/main/java/frc/robot/constants/GyroBiasConstants.java b/src/main/java/frc/robot/constants/GyroBiasConstants.java new file mode 100644 index 0000000..698713b --- /dev/null +++ b/src/main/java/frc/robot/constants/GyroBiasConstants.java @@ -0,0 +1,27 @@ +package frc.robot.constants; + +/** + * constants for gyro bias estimation and correction via vision. + */ +public class GyroBiasConstants { + /** minimum samples before applying correction */ + public static final int MIN_SAMPLES = 10; + + /** maximum angle difference to accept in radians */ + public static final double MAX_ANGLE_DIFF_RAD = 0.1; // 5.7 deg + + /** minimum correction to apply in radians */ + public static final double MIN_CORRECTION_RAD = 0.02; // 1 deg + + /** fraction of the correction to apply (0.0 to 1.0) */ + public static final double CORRECTION_FRACTION = 0.2; + + /** maximum correction per cycle in radians */ + public static final double MAX_CORRECTION_PER_CYCLE_RAD = 0.05; // 2.9 deg + + /** alpha for exponential moving average 0.0 to 1.0, higher is more responsive */ + public static final double EMA_ALPHA = 0.3; + + /** min total weight required for weighted average */ + public static final double MIN_TOTAL_WEIGHT = 3.0; +} diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 69247a1..c2e770c 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -5,14 +5,12 @@ import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.filter.LinearFilter; diff --git a/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java b/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java new file mode 100644 index 0000000..99849a5 --- /dev/null +++ b/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java @@ -0,0 +1,203 @@ +package frc.robot.util.Vision; + +import edu.wpi.first.math.geometry.Pose3d; +import frc.robot.constants.GyroBiasConstants; + +/** + * estimates gyro bias by comparing vision-derived yaw to gyro yaw. + * + * when the robot observes April tags, PhotonVision calculates what the robot heading + * SHOULD be based on known tag positions vs observed angles. This can be compared to + * the gyro reading to detect and correct drift. + * + */ +public class GyroBiasEstimator { + + private double weightedBiasSum = 0.0; + private double totalWeight = 0.0; + private int sampleCount = 0; + + // exponential moving average + private double emaBias = 0.0; + private boolean emaInitialized = false; + + /** + * process a new observation comparing vision pose to gyro reading. + * + * @param visionPose the pose estimated by vision (from PhotonVision) + * @param gyroYaw current gyro reading in radians + * @param visionWeight weight for observation (0.0 to 1.0, higher is more trusted) + * @return true if bias should be applied (has enough samples) + */ + public boolean addObservation(Pose3d visionPose, double gyroYaw, double visionWeight) { + if (visionPose == null) { + return false; + } + + // get yaw from vision + double visionYaw = visionPose.getRotation().getZ(); + + return addObservation(visionYaw, gyroYaw, visionWeight); + } + + /** + * process a new observation with just yaw values. + * + * @param visionYaw yaw from vision pose in radians + * @param gyroYaw current gyro reading in radians + * @param visionWeight weight for this observation (0.0 to 1.0, higher is more trusted) + * @return true if bias should be applied + */ + public boolean addObservation(double visionYaw, double gyroYaw, double visionWeight) { + // normalize to [-PI, PI] + double diff = normalizeAngle(visionYaw - gyroYaw); + + // reject outliers + if (Math.abs(diff) > GyroBiasConstants.MAX_ANGLE_DIFF_RAD) { + return false; + } + + // clamp weight + double weight = Math.max(0.0, Math.min(1.0, visionWeight)); + + // accumulate weighted bias + weightedBiasSum += diff * weight; + totalWeight += weight; + sampleCount++; + + // update exponential moving average + if (!emaInitialized) { + emaBias = diff; + emaInitialized = true; + } else { + emaBias = emaBias * (1.0 - GyroBiasConstants.EMA_ALPHA) + diff * GyroBiasConstants.EMA_ALPHA; + } + + return sampleCount >= GyroBiasConstants.MIN_SAMPLES; + } + + /** + * process new observation with default weight of 1.0. + * maintains backward compatibility. + */ + public boolean addObservation(Pose3d visionPose, double gyroYaw) { + return addObservation(visionPose, gyroYaw, 1.0); + } + + /** + * process new observation with default weight of 1.0. + * maintains backward compatibility + */ + public boolean addObservation(double visionYaw, double gyroYaw) { + return addObservation(visionYaw, gyroYaw, 1.0); + } + + /** + * get average bias and reset + * + * @return average bias in radians to apply, or 0 if not enough samples + */ + public double getAndResetBias() { + if (sampleCount < GyroBiasConstants.MIN_SAMPLES || totalWeight < GyroBiasConstants.MIN_TOTAL_WEIGHT) { + return 0.0; + } + + // use weighted average if we have enough weight, otherwise use EMA + double avgBias; + if (totalWeight >= GyroBiasConstants.MIN_TOTAL_WEIGHT) { + avgBias = weightedBiasSum / totalWeight; + } else { + avgBias = emaBias; + } + + // reset + weightedBiasSum = 0.0; + totalWeight = 0.0; + sampleCount = 0; + emaInitialized = false; + + return avgBias; + } + + /** + * apply partial correction to avoid sudden jumps. + * + * @param fullBias the full calculated bias + * @return partial correction to apply + */ + public double applyPartialCorrection(double fullBias) { + double clampedBias = fullBias; + if (clampedBias > GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD) { + clampedBias = GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD; + } else if (clampedBias < -GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD) { + clampedBias = -GyroBiasConstants.MAX_CORRECTION_PER_CYCLE_RAD; + } + + return clampedBias * GyroBiasConstants.CORRECTION_FRACTION; + } + + /** + * check if correction should be applied. + * + * @param gyroYaw current gyro reading + * @param visionYaw vision-derived yaw + * @return true if we should call setYaw(), false otherwise + */ + public boolean shouldCorrect(double gyroYaw, double visionYaw) { + if (addObservation(visionYaw, gyroYaw)) { + double bias = getAndResetBias(); + return Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD; + } + return false; + } + + /** + * normalize angle to [-PI, PI] + */ + private double normalizeAngle(double angle) { + while (angle > Math.PI) { + angle -= 2 * Math.PI; + } + while (angle < -Math.PI) { + angle += 2 * Math.PI; + } + return angle; + } + + /** + * get sample count for debugging + */ + public int getSampleCount() { + return sampleCount; + } + + /** + * get current accumulated bias without resetting + */ + public double getCurrentBias() { + if (sampleCount == 0) { + return 0.0; + } + if (totalWeight > 0) { + return weightedBiasSum / totalWeight; + } + return emaBias; + } + + /** + * get current total weight for debugging + */ + public double getTotalWeight() { + return totalWeight; + } + + /** + * reset accumulated state + */ + public void reset() { + weightedBiasSum = 0.0; + totalWeight = 0.0; + sampleCount = 0; + emaInitialized = false; + } +}