Skid detection

src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java

@@ -41,6 +41,7 @@
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionHelper;
 import frc.robot.subsystems.vision.VisionIO;
+import frc.robot.utils.SkidDetection;
 import frc.robot.utils.Tracer;
 import java.util.Arrays;
 import java.util.List;
@@ -64,8 +65,12 @@ public class SwerveSubsystem extends SubsystemBase {
 
   private SwerveDriveKinematics kinematics;
 
+  private SkidDetection skidDetection;
+
   private final Vision[] cameras;
 
+  private Timer lastSkidTimer = new Timer();
+
   /** For delta tracking with PhoenixOdometryThread* */
   private SwerveModulePosition[] lastModulePositions =
       new SwerveModulePosition[] {
@@ -111,6 +116,7 @@ public SwerveSubsystem(
     this.gyroIO = gyroIO;
     this.odoThread = odoThread;
     this.simulation = simulation;
+
     cameras = new Vision[visionIOs.length];
     modules = new Module[moduleIOs.length];
 
@@ -267,6 +273,37 @@ private void updateOdometry() {
       }
       missingModuleData.set(false);
 
+      boolean[] skiddingModules =
+          Tracer.trace(
+              "detect skidding modules",
+              () ->
+                  skidDetection.detectSkiddingModules(
+                      kinematics, getModuleStates(), gyroInputs.yawVelocityRadPerSec));
+
+      Logger.recordOutput("Odometry/Skidding Modules", skiddingModules);
+
+      for (int i = 0; i <= skiddingModules.length; i++) {
+        // If a module is skidding, average the other modules
+        if (skiddingModules[i]) {
+          lastSkidTimer.restart();
+          // double averageDeltaPosition =
+          //     (Arrays.stream(moduleDeltas)
+          //                 .map(delta -> delta.distanceMeters)
+          //                 .reduce(0.0, (Double a, Double b) -> a + b)
+          //             - moduleDeltas[i].distanceMeters)
+          //         / 3;
+          // double averageDeltaRotationRads =
+          //     (Arrays.stream(moduleDeltas)
+          //                 .map(delta -> delta.angle.getRadians())
+          //                 .reduce(0.0, (Double a, Double b) -> a + b)
+          //             - moduleDeltas[i].angle.getRadians())
+          //         / 3;
+          // moduleDeltas[i] =
+          //     new SwerveModulePosition(
+          //         averageDeltaPosition, Rotation2d.fromRadians(averageDeltaRotationRads));
+        }
+      }
+
       // If we have all our module data . . .
       // The twist represents the motion of the robot since the last
       // sample in x, y, and theta based only on the modules, without
@@ -329,7 +366,9 @@ private void updateVision() {
                     camera.inputs.captureTimestampMicros / 1.0e6,
                     deviations
                         .times(DriverStation.isAutonomous() ? 2.0 : 1.0)
-                        .times(camera.getName().equals("Front_Camera") ? 1.0 : 1.5));
+                        .times(camera.getName().equals("Front_Camera") ? 1.0 : 1.5)
+                        // Weight vision higher if a skid recently happened TODO: TUNE WEIGHTS
+                        .times(lastSkidTimer.isRunning() && lastSkidTimer.get() > 3 ? 1.0 : 1/3.0));
               });
           lastEstTimestamp = camera.inputs.captureTimestampMicros / 1e6;
           // if (Robot.ROBOT_TYPE != RobotType.REAL)

src/main/java/frc/robot/utils/SkidDetection.java

@@ -0,0 +1,69 @@
+package frc.robot.utils;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import java.util.Arrays;
+
+public class SkidDetection {
+
+  /**
+   * Detects modules that are skidding
+   *
+   * @param moduleStates [Front left, front right, back left, back right]
+   * @param yawVelocityRadPerSec From gyro
+   * @return the skidding modules [Front left, front right, back left, back right] true if skidding
+   */
+  public boolean[] detectSkiddingModules(
+      SwerveDriveKinematics kinematics,
+      SwerveModuleState[] moduleStates,
+      double yawVelocityRadPerSec) {
+
+    // Get the pure rotational component based on the gyro yaw velocity
+    SwerveModuleState[] rotationalComponents =
+        kinematics.toSwerveModuleStates(new ChassisSpeeds(0.0, 0.0, yawVelocityRadPerSec));
+    double[] xTransComponents = new double[4];
+    double[] yTransComponents = new double[4];
+    for (int i = 0; i <= moduleStates.length; i++) {
+      SwerveModuleState moduleState = moduleStates[i];
+      SwerveModuleState rotationalComponentState = rotationalComponents[i];
+      // Subtract rotational component vectors from the total measured vectors
+      xTransComponents[i] =
+          (moduleState.speedMetersPerSecond * moduleState.angle.getCos())
+              - (rotationalComponentState.speedMetersPerSecond
+                  * rotationalComponentState.angle.getCos());
+      yTransComponents[i] =
+          (moduleState.speedMetersPerSecond * moduleState.angle.getSin())
+              - (rotationalComponentState.speedMetersPerSecond
+                  * rotationalComponentState.angle.getSin());
+    }
+    double xTransComponentAverage = Arrays.stream(xTransComponents).average().getAsDouble();
+    // Check that this is the correct calc
+    double xTransComponentStdDev =
+        Math.sqrt(
+            Arrays.stream(xTransComponents)
+                    .map(value -> Math.pow(value - xTransComponentAverage, 2))
+                    .sum()
+                / 4);
+
+    double yTransComponentAverage = Arrays.stream(yTransComponents).average().getAsDouble();
+    double yTransComponentStdDev =
+        Math.sqrt(
+            Arrays.stream(yTransComponents)
+                    .map(value -> Math.pow(value - yTransComponentAverage, 2))
+                    .sum()
+                / 4);
+
+    boolean[] result = new boolean[4];
+    for (int i = 0; i <= moduleStates.length; i++) {
+      // If the measurement deviates by more than 2 standard deviations assume it's skidding (TODO:
+      // TUNE)
+      result[i] =
+          Math.abs(xTransComponents[i] - xTransComponentAverage) >= xTransComponentStdDev * 2
+              || Math.abs(yTransComponents[i] - yTransComponentAverage)
+                  >= yTransComponentStdDev * 2;
+    }
+
+    return result;
+  }
+}