From 30df55372b32bd7a8f065b3b4ee4a5527ffc8229 Mon Sep 17 00:00:00 2001 From: iefomit Date: Sat, 28 Mar 2026 16:33:01 -0700 Subject: [PATCH] logging, trying to debug --- .../robot/commands/gpm/Superstructure.java | 2 +- .../robot/constants/GyroBiasConstants.java | 4 +- .../robot/constants/ShotInterpolation.java | 2 +- .../subsystems/drivetrain/Drivetrain.java | 62 +++---------------- 4 files changed, 13 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index d3cce24..1c88853 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -229,7 +229,7 @@ public class Superstructure extends Command { hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); double x = drivepose.getX(); // compared as meters double y = drivepose.getY(); - System.out.println("X: " + Units.metersToInches(x) + "Y: " + Units.metersToInches(y)); + // System.out.println("X: " + Units.metersToInches(x) + "Y: " + Units.metersToInches(y)); // if (FieldConstants.underTrench(x, y)) { // hood.forceHoodDown(true); // System.out.println("Hood forced down"); diff --git a/src/main/java/frc/robot/constants/GyroBiasConstants.java b/src/main/java/frc/robot/constants/GyroBiasConstants.java index 698713b..e7d5f6d 100644 --- a/src/main/java/frc/robot/constants/GyroBiasConstants.java +++ b/src/main/java/frc/robot/constants/GyroBiasConstants.java @@ -8,10 +8,10 @@ public class GyroBiasConstants { 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 + public static final double MAX_ANGLE_DIFF_RAD = Math.toRadians(45); /** minimum correction to apply in radians */ - public static final double MIN_CORRECTION_RAD = 0.02; // 1 deg + public static final double MIN_CORRECTION_RAD = Math.toRadians(1.0); /** fraction of the correction to apply (0.0 to 1.0) */ public static final double CORRECTION_FRACTION = 0.2; diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java index 0c0da26..7b72da9 100644 --- a/src/main/java/frc/robot/constants/ShotInterpolation.java +++ b/src/main/java/frc/robot/constants/ShotInterpolation.java @@ -52,7 +52,7 @@ public class ShotInterpolation { shooterVelocityMap.put(4.07, 15.5); shooterVelocityMap.put(0.0, 9.55); - shooterVelocityMap.put(25.0, 43.44); + shooterVelocityMap.put(25.0, 20.00); newHoodMap.put(1.00, 78.0); newHoodMap.put(1.49, 72.0); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 5c51fdd..ab6de75 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -328,6 +328,8 @@ public class Drivetrain extends SubsystemBase { if (!Double.isNaN(gyroYawAtTimestamp)) { + Logger.recordOutput("GyroYaw", Math.toDegrees(gyroYawAtTimestamp)); + Logger.recordOutput("VisionYaw", Math.toDegrees(visionYaw)); // use weighted observation gyroBiasEstimator.addObservation(visionYaw, gyroYawAtTimestamp, 1.0); } @@ -338,10 +340,14 @@ public class Drivetrain extends SubsystemBase { if (gyroBiasEstimator.getSampleCount() >= GyroBiasConstants.MIN_SAMPLES) { double fullBias = gyroBiasEstimator.getAndResetBias(); double bias = gyroBiasEstimator.applyPartialCorrection(fullBias); + System.out.println("bias: " + bias); + System.out.println("FullBias"+ fullBias); + if (Math.abs(bias) > GyroBiasConstants.MIN_CORRECTION_RAD) { gyroIO.setYaw(new Rotation2d(currentGyroYaw + bias)); resetOdometry( new Pose2d(getPose().getTranslation(), new Rotation2d(currentGyroYaw + bias))); + System.out.print("reset"); } } } @@ -581,59 +587,9 @@ public class Drivetrain extends SubsystemBase { * @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 100ms - if (closestDiff > 0.1) { - return Double.NaN; - } - - // try to interpolate if we have before/after samples - if (timestamps.length > 1) { - 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 and after samples - if (beforeIndex >= 0 && afterIndex >= 0) { - 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); - return beforeYaw + t * (afterYaw - beforeYaw); - } - } - - return positions[closestIndex].getRadians(); + private double + getGyroYawAtTimestamp(double timestampSeconds) { + return getPose().getRotation().getRadians(); } /** -- 2.39.5