diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 809fd5da..25b6fa69 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/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) diff --git a/src/main/java/frc/robot/utils/SkidDetection.java b/src/main/java/frc/robot/utils/SkidDetection.java new file mode 100644 index 00000000..e10b4e0a --- /dev/null +++ b/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; + } +}