From: iefomit <timofei.stem@gmail.com>
Date: Sat, 28 Mar 2026 21:42:30 +0000 (-0700)
Subject: added logging
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=79aba662e2a09dd3155fa56fef6702d9b5a7c596;p=FRC2026.git

added logging
---

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 5c51fdd..62e5e44 100644
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -310,12 +310,16 @@ public class Drivetrain extends SubsystemBase {
             if (vision != null && visionEnabled && visionEnableTimer.hasElapsed(5)) {
                 vision.updateOdometry(poseEstimator, time -> getPoseAt(time).getRotation().getRadians(), slipped);
 
+                double currentGyroYaw = gyroInputs.yawPosition.getRadians();
+                Logger.recordOutput("GyroBias/CurrentGyroYaw", currentGyroYaw);
+                Logger.recordOutput("GyroBias/SampleCount", gyroBiasEstimator.getSampleCount());
+                Logger.recordOutput("GyroBias/CanSeeTag", vision.canSeeTag());
+
+
                 if (vision.canSeeTag()) {
                     slipped = false;
                     modulePoses.reset();
 
-                    double currentGyroYaw = gyroInputs.yawPosition.getRadians();
-
                     // to compare bias
                     ArrayList<EstimatedRobotPose> visionPoses = vision.getEstimatedPoses(getPose());
 
@@ -328,6 +332,10 @@ public class Drivetrain extends SubsystemBase {
 
                             if (!Double.isNaN(gyroYawAtTimestamp)) {
 
+                                // Log gyro and vision yaw data for debugging
+                                Logger.recordOutput("GyroBias GyroYaw", gyroYawAtTimestamp);
+                                Logger.recordOutput("GyroBias VisionYaw", visionYaw);
+
                                 // use weighted observation
                                 gyroBiasEstimator.addObservation(visionYaw, gyroYawAtTimestamp, 1.0);
                             }
@@ -338,6 +346,9 @@ public class Drivetrain extends SubsystemBase {
                     if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) {
                         double fullBias = gyroBiasEstimator.getAndResetBias();
                         double bias = gyroBiasEstimator.applyPartialCorrection(fullBias);
+                        
+                        Logger.recordOutput("GyroBias CalculatedBias", bias);
+                        
                         if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) {
                             gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias));
                             resetOdometry(