From 79aba662e2a09dd3155fa56fef6702d9b5a7c596 Mon Sep 17 00:00:00 2001 From: iefomit Date: Sat, 28 Mar 2026 14:42:30 -0700 Subject: [PATCH] added logging --- .../robot/subsystems/drivetrain/Drivetrain.java | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) 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 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( -- 2.39.5