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 d8dd19873bde82a3ab6454524399eb620218b742..603a239802e06b96481e7faf56e8a21dcf60dde4 100644 (file)
--- 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;
 
         // <ol start="0"> 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 cb77ac2879a0fc7285b5721193ff669a43632e13..b9d74194ad3ff88eb659feb02317f18499653bb7 100644 (file)
--- 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<EstimatedRobotPose> 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 ca66aa973031304971c6822b3179e8b49b43a747..dd63dcdd35b6d9043c96b57be6693d2fc60b6a9e 100644 (file)
--- 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 320c3b8919940e0ea0d5f7e594689ceb84152ad5..c6211f59e9d43b916e1a3f3c77470171e169ef6a 100644 (file)
--- 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 d92b3de0e3b56ff5a216e0352b018aa5fae1d431..eb70f25be6326cd6771cecb3052ddce4eb38772d 100644 (file)
--- 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<Integer> 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<PhotonPipelineResult> getResults() {
+      return inputs.results;
+    }
   }
 }