From: iefomit Date: Sat, 28 Mar 2026 19:42:42 +0000 (-0700) Subject: add vision gyro estimator X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=7596f680895e5852f31072a6f3318ddccfceedeb;p=FRC2026.git add vision gyro estimator --- diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index d8dd198..603a239 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -68,7 +68,7 @@ public class VisionConstants { public static final double MAX_DISTANCE = 6; /** If vision should use manual calculations */ - public static final boolean USE_MANUAL_CALCULATIONS = true; + public static final boolean USE_MANUAL_CALCULATIONS = false; //
    did not work /** diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index cb77ac2..b9d7419 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.drivetrain; import java.util.Arrays; +import java.util.ArrayList; import java.util.Optional; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -29,6 +30,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.FieldConstants; +import frc.robot.constants.GyroBiasConstants; import frc.robot.constants.VisionConstants; import frc.robot.constants.swerve.DriveConstants; import frc.robot.constants.swerve.ModuleConstants; @@ -38,6 +40,8 @@ import frc.robot.util.SwerveModulePose; import frc.robot.util.SwerveStuff.SwerveSetpoint; import frc.robot.util.SwerveStuff.SwerveSetpointGenerator; import frc.robot.util.Vision.Vision; +import frc.robot.util.Vision.GyroBiasEstimator; +import org.photonvision.EstimatedRobotPose; /** * Represents a swerve drive style drivetrain. @@ -113,6 +117,9 @@ public class Drivetrain extends SubsystemBase { private Rotation2d rawGyroRotation = new Rotation2d(); + // for vision yaw correction + private GyroBiasEstimator gyroBiasEstimator = new GyroBiasEstimator(); + private final Field2d field = new Field2d(); /** @@ -306,6 +313,37 @@ public class Drivetrain extends SubsystemBase { if (vision.canSeeTag()) { slipped = false; modulePoses.reset(); + + double currentGyroYaw = gyroInputs.yawPosition.getRadians(); + + // to compare bias + ArrayList visionPoses = vision.getEstimatedPoses(getPose()); + + for (EstimatedRobotPose visionPose : visionPoses) { + if (visionPose.estimatedPose != null && visionPose.timestampSeconds > 0) { + double visionYaw = visionPose.estimatedPose.getRotation().getZ(); + + // gets at vision timestamp, not current gyro yaw + double gyroYawAtTimestamp = getGyroYawAtTimestamp(visionPose.timestampSeconds); + + if (!Double.isNaN(gyroYawAtTimestamp)) { + + // use weighted observation + gyroBiasEstimator.addObservation(visionYaw, gyroYawAtTimestamp, 1.0); + } + } + } + + // check if we have enough samples + if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) { + double fullBias = gyroBiasEstimator.getAndResetBias(); + double bias = gyroBiasEstimator.applyPartialCorrection(fullBias); + if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) { + gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias)); + resetOdometry( + new Pose2d(getPose().getTranslation(), new Rotation2d(currentGyroYaw + bias))); + } + } } } } @@ -536,6 +574,71 @@ public class Drivetrain extends SubsystemBase { return modules; } + /** + * gets gyro yaw at a specific timestamp with interpolation + * this is used for timestamp-synchronized gyro/vision comparison. + * + * @param timestampSeconds the timestamp to get the gyro yaw at + * @return the gyro yaw in radians, or Double.NaN if no valid data + */ + private double getGyroYawAtTimestamp(double timestampSeconds) { + double[] timestamps = gyroInputs.odometryYawTimestamps; + Rotation2d[] positions = gyroInputs.odometryYawPositions; + + if (timestamps == null || positions == null || timestamps.length == 0) { + return Double.NaN; + } + + // find closest timestamp + int closestIndex = 0; + double closestDiff = Double.MAX_VALUE; + + for (int i = 0; i < timestamps.length; i++) { + double diff = Math.abs(timestamps[i] - timestampSeconds); + if (diff < closestDiff) { + closestDiff = diff; + closestIndex = i; + } + } + + // only use if within + if (closestDiff > 0.1) { + return Double.NaN; + } + + // if we have adjacent samples, interpolate + if (timestamps.length > 1 && closestDiff < 0.05) { + // find sample before/after target timestamp + int beforeIndex = -1; + int afterIndex = -1; + + for (int i = 0; i < timestamps.length; i++) { + if (timestamps[i] <= timestampSeconds) { + beforeIndex = i; + } else { + afterIndex = i; + break; + } + } + + // interpolate if we have both before/after + if (beforeIndex >= 0 && afterIndex >= 0 && beforeIndex != afterIndex) { + double beforeTime = timestamps[beforeIndex]; + double afterTime = timestamps[afterIndex]; + double beforeYaw = positions[beforeIndex].getRadians(); + double afterYaw = positions[afterIndex].getRadians(); + + // linear interpolation + double t = (timestampSeconds - beforeTime) / (afterTime - beforeTime); + double interpolatedYaw = beforeYaw + t * (afterYaw - beforeYaw); + + return interpolatedYaw; + } + } + + return positions[closestIndex].getRadians(); + } + /** * Resets the yaw of the robot. * diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java index ca66aa9..dd63dcd 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIO.java @@ -29,4 +29,11 @@ public interface GyroIO { } public default void updateInputs(GyroIOInputs inputs) {} + + /** + * set the yaw angle of the gyro. + * + * @param rotation the new yaw angle + */ + public default void setYaw(Rotation2d rotation) {} } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java index 320c3b8..c6211f5 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/GyroIOPigeon2.java @@ -71,4 +71,9 @@ public class GyroIOPigeon2 implements GyroIO { yawTimestampQueue.clear(); yawPositionQueue.clear(); } + + @Override + public void setYaw(Rotation2d rotation) { + pigeon.getConfigurator().setYaw(rotation.getDegrees()); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Vision/Vision.java b/src/main/java/frc/robot/util/Vision/Vision.java index d92b3de..eb70f25 100644 --- a/src/main/java/frc/robot/util/Vision/Vision.java +++ b/src/main/java/frc/robot/util/Vision/Vision.java @@ -382,6 +382,24 @@ public class Vision { return sawTag; } + /** + * returns visible tag IDs from all cameras. + */ + public int[] getVisibleTagIds() { + ArrayList tagIds = new ArrayList<>(); + for(VisionCamera c : cameras) { + for(PhotonPipelineResult result : c.getResults()) { + for(PhotonTrackedTarget target : result.getTargets()) { + int id = target.getFiducialId(); + if(id > 0 && !tagIds.contains(id)) { + tagIds.add(id); + } + } + } + } + return tagIds.stream().mapToInt(Integer::intValue).toArray(); + } + /** * Enable or disable a single camera * @param index The camera index @@ -698,5 +716,9 @@ public class Vision { public void enable(boolean enable){ enabled = enable; } + + public List getResults() { + return inputs.results; + } } }