logging, trying to debug

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index d3cce246a26f57b0792a8bf0c242ed97e73705c5..1c88853cb5d3172d02d8a87d5a161bb9f0dfedc5 100644 (file)
--- 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 698713bf7324a908299caecea880ea78990774d2..e7d5f6d46767ea04ea64cad7bfff76b6b367d18d 100644 (file)
--- 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 0c0da265a3a2a3500cc4f22c4caf229367d04487..7b72da93b66abb2975fc433b45df4eff53b3413f 100644 (file)
--- 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 5c51fdde2292b110410eb91cbf6e450d95f57e23..ab6de75bcbd77d244a4f9c9987abe26f6919c78c 100644 (file)
--- 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();
     }
 
     /**