better timestamp, other fixes

diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
index b813aed58735e809c133b362428c1b759f2c7941..7dc6bdb2461912d7c243f222c6b1fe874d24f705 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
+++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java
@@ -34,6 +34,11 @@ public class RunSpindexer extends Command {
         // addRequirements(spindexer);
     // }
 
+    @Override
+    public void initialize() {
+        wasHoodForcedDown = hood.getHoodForcedDown();
+    }
+
     @Override
     public void execute() {
         boolean hoodForcedDown = hood.getHoodForcedDown();

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 895d739f08b8aa2bb1cabed965961cb016838f66..2c4f6074d4f301c2cfa16f31a8cadc8332b0bbe6 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -2,7 +2,6 @@ package frc.robot.subsystems.drivetrain;
 
 import java.util.ArrayList;
 import java.util.Arrays;
-import java.util.ArrayList;
 import java.util.Optional;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
@@ -23,12 +22,12 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj.motorcontrol.Talon;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.commands.Music;
@@ -640,7 +639,6 @@ public class Drivetrain extends SubsystemBase {
         // find two closest timestamps and interpolate
         int oldestIndex = (gyroHistoryHead - gyroHistoryCount + GYRO_HISTORY_SIZE) % GYRO_HISTORY_SIZE;
         int newestIndex = (gyroHistoryHead - 1 + GYRO_HISTORY_SIZE) % GYRO_HISTORY_SIZE;
-        
         double oldestTs = gyroTimestampsHistory[oldestIndex];
         double newestTs = gyroTimestampsHistory[newestIndex];
         
@@ -652,9 +650,41 @@ public class Drivetrain extends SubsystemBase {
             return gyroYawsHistory[newestIndex];
         }
         
-        // linear interpolation between closest readings
-        double alpha = (timestampSeconds - oldestTs) / (newestTs - oldestTs);
-        return gyroYawsHistory[oldestIndex] + alpha * (gyroYawsHistory[newestIndex] - gyroYawsHistory[oldestIndex]);
+        int prevIndex = -1;
+        int nextIndex = -1;
+        int idx = oldestIndex;
+        for (int i = 0; i < gyroHistoryCount; i++) {
+            double ts = gyroTimestampsHistory[idx];
+            if (ts <= timestampSeconds) {
+                prevIndex = idx;
+            }
+            if (ts >= timestampSeconds) {
+                nextIndex = idx;
+                break;
+            }
+            idx = (idx + 1) % GYRO_HISTORY_SIZE;
+        }
+        
+        if (prevIndex == -1) {
+            return gyroYawsHistory[oldestIndex];
+        }
+        if (nextIndex == -1) {
+            return gyroYawsHistory[newestIndex];
+        }
+        
+        double t0 = gyroTimestampsHistory[prevIndex];
+        double t1 = gyroTimestampsHistory[nextIndex];
+        if (prevIndex == nextIndex || t1 == t0) {
+            return gyroYawsHistory[prevIndex];
+        }
+        
+        double y0 = gyroYawsHistory[prevIndex];
+        double y1 = gyroYawsHistory[nextIndex];
+        
+        double alpha = (timestampSeconds - t0) / (t1 - t0);
+
+        double delta = MathUtil.angleModulus(y1 - y0);
+        return y0 + alpha * delta;
     }
 
     /**

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java
index 259e6a3a75509cfb87d92b7997970969a5618eb7..6c913e54ee277d73524d3ee512cb81c80b478855 100644 (file)
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist.java
@@ -1,9 +1,6 @@
 package frc.robot.util.TrenchAssist;
 
-import org.littletonrobotics.junction.Logger;
-
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;

diff --git a/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java b/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java
index 7e8d3f23219f252c4192d2d8d164afe9a26c3d41..4c871bba844705868eaa68377697aa7547e53beb 100644 (file)
--- a/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java
+++ b/src/main/java/frc/robot/util/Vision/GyroBiasEstimator.java
@@ -99,7 +99,7 @@ public class GyroBiasEstimator {
      * @return average bias in radians to apply, or 0 if not enough samples
      */
     public double getAndResetBias() {
-        if (sampleCount < GyroBiasConstants.MIN_SAMPLES || totalWeight < GyroBiasConstants.MIN_TOTAL_WEIGHT) {
+        if (sampleCount < GyroBiasConstants.MIN_SAMPLES) {
             return 0.0;
         }