From c5e0c08c5538c8f82e8fc800b062713c818aea6c Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 16 Mar 2025 13:20:25 -0700 Subject: [PATCH 1/9] Initial skid detection code --- .../java/frc/robot/utils/SkidDetection.java | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 src/main/java/frc/robot/utils/SkidDetection.java 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..7941966d --- /dev/null +++ b/src/main/java/frc/robot/utils/SkidDetection.java @@ -0,0 +1,64 @@ +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Rotation2d; +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 { + + private SwerveDriveKinematics kinematics; + + // TODO: TUNE + private static final double MAX_DEVIATION = 0.0; + + public SkidDetection(SwerveDriveKinematics kinematics) { + this.kinematics = kinematics; + } + + /** + * Detects modules that are skidding + * + * @param moduleStates [Front left, front right, back left, back right] + * @param heading + * @return the skidding modules [Front left, front right, back left, back right] true if skidding + */ + public boolean[] detectSkiddingModules(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(); + + double yTransComponentAverage = Arrays.stream(yTransComponents).average().getAsDouble(); + + boolean[] result = new boolean[4]; + for (int i = 0; i <= moduleStates.length; i++) { + // If the measurement deviates too much from the mean, assume it's skidding + if (Math.abs(xTransComponents[i] - xTransComponentAverage) > MAX_DEVIATION + || Math.abs(yTransComponents[i] - yTransComponentAverage) > MAX_DEVIATION) { + result[i] = true; + } else { + result[i] = false; + } + } + + return result; + } +} From 013ee45ea7ee0ddea795ce23de25cddcd37b6048 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 16 Mar 2025 13:28:45 -0700 Subject: [PATCH 2/9] Use standard deviation instead of a constant --- src/main/java/frc/robot/utils/SkidDetection.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/utils/SkidDetection.java b/src/main/java/frc/robot/utils/SkidDetection.java index 7941966d..68f950dc 100644 --- a/src/main/java/frc/robot/utils/SkidDetection.java +++ b/src/main/java/frc/robot/utils/SkidDetection.java @@ -10,9 +10,6 @@ public class SkidDetection { private SwerveDriveKinematics kinematics; - // TODO: TUNE - private static final double MAX_DEVIATION = 0.0; - public SkidDetection(SwerveDriveKinematics kinematics) { this.kinematics = kinematics; } @@ -45,14 +42,17 @@ public boolean[] detectSkiddingModules(SwerveModuleState[] moduleStates, double * 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 too much from the mean, assume it's skidding - if (Math.abs(xTransComponents[i] - xTransComponentAverage) > MAX_DEVIATION - || Math.abs(yTransComponents[i] - yTransComponentAverage) > MAX_DEVIATION) { + // If the measurement deviates by more than 2 standard deviations assume it's skidding (TODO: TUNE) + if (Math.abs(xTransComponents[i] - xTransComponentAverage) >= xTransComponentStdDev * 2 + || Math.abs(yTransComponents[i] - yTransComponentAverage) >= yTransComponentStdDev * 2) { result[i] = true; } else { result[i] = false; From 8b76cc632d0c2e4bc25d90ba523b9da87ae698ba Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 16 Mar 2025 14:10:46 -0700 Subject: [PATCH 3/9] Use the skidding modules to affect odometry update --- .../subsystems/swerve/SwerveSubsystem.java | 18 ++++++++++++++++++ .../java/frc/robot/utils/SkidDetection.java | 3 +-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 809fd5da..cc5452af 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -42,6 +42,7 @@ import frc.robot.subsystems.vision.VisionHelper; import frc.robot.subsystems.vision.VisionIO; import frc.robot.utils.Tracer; +import frc.robot.utils.SkidDetection; import java.util.Arrays; import java.util.List; import java.util.Map; @@ -64,6 +65,8 @@ public class SwerveSubsystem extends SubsystemBase { private SwerveDriveKinematics kinematics; + private SkidDetection skidDetection; + private final Vision[] cameras; /** For delta tracking with PhoenixOdometryThread* */ @@ -111,6 +114,9 @@ public SwerveSubsystem( this.gyroIO = gyroIO; this.odoThread = odoThread; this.simulation = simulation; + + this.skidDetection = new SkidDetection(this.kinematics); + cameras = new Vision[visionIOs.length]; modules = new Module[moduleIOs.length]; @@ -267,6 +273,18 @@ private void updateOdometry() { } missingModuleData.set(false); + boolean[] skiddingModules = skidDetection.detectSkiddingModules(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]) { + 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 diff --git a/src/main/java/frc/robot/utils/SkidDetection.java b/src/main/java/frc/robot/utils/SkidDetection.java index 68f950dc..90ad499f 100644 --- a/src/main/java/frc/robot/utils/SkidDetection.java +++ b/src/main/java/frc/robot/utils/SkidDetection.java @@ -1,6 +1,5 @@ package frc.robot.utils; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -18,7 +17,7 @@ public SkidDetection(SwerveDriveKinematics kinematics) { * Detects modules that are skidding * * @param moduleStates [Front left, front right, back left, back right] - * @param heading + * @param yawVelocityRadPerSec From gyro * @return the skidding modules [Front left, front right, back left, back right] true if skidding */ public boolean[] detectSkiddingModules(SwerveModuleState[] moduleStates, double yawVelocityRadPerSec) { From 5f44e54bf008d299cc37ac0bb80f4c91a521bd58 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 16 Mar 2025 14:11:20 -0700 Subject: [PATCH 4/9] Add semicolon --- src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index cc5452af..ba24d921 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -281,7 +281,7 @@ private void updateOdometry() { if (skiddingModules[i]) { 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)) + moduleDeltas[i] = new SwerveModulePosition(averageDeltaPosition, Rotation2d.fromRadians(averageDeltaRotationRads)); } } From 4cff41b682bfac0fb64952a7d8f0cdc13317e4cb Mon Sep 17 00:00:00 2001 From: SCool62 Date: Wed, 26 Mar 2025 13:48:04 -0700 Subject: [PATCH 5/9] Make detectSkiddingModules static --- .../subsystems/swerve/SwerveSubsystem.java | 26 +++++++++++----- .../java/frc/robot/utils/SkidDetection.java | 30 ++++++++++++------- 2 files changed, 38 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index ba24d921..fbafb5c9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -41,8 +41,8 @@ import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionHelper; import frc.robot.subsystems.vision.VisionIO; -import frc.robot.utils.Tracer; import frc.robot.utils.SkidDetection; +import frc.robot.utils.Tracer; import java.util.Arrays; import java.util.List; import java.util.Map; @@ -114,8 +114,6 @@ public SwerveSubsystem( this.gyroIO = gyroIO; this.odoThread = odoThread; this.simulation = simulation; - - this.skidDetection = new SkidDetection(this.kinematics); cameras = new Vision[visionIOs.length]; modules = new Module[moduleIOs.length]; @@ -273,15 +271,29 @@ private void updateOdometry() { } missingModuleData.set(false); - boolean[] skiddingModules = skidDetection.detectSkiddingModules(getModuleStates(), gyroInputs.yawVelocityRadPerSec); + boolean[] skiddingModules = + 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]) { - 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)); + 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)); } } diff --git a/src/main/java/frc/robot/utils/SkidDetection.java b/src/main/java/frc/robot/utils/SkidDetection.java index 90ad499f..613b8131 100644 --- a/src/main/java/frc/robot/utils/SkidDetection.java +++ b/src/main/java/frc/robot/utils/SkidDetection.java @@ -7,12 +7,6 @@ public class SkidDetection { - private SwerveDriveKinematics kinematics; - - public SkidDetection(SwerveDriveKinematics kinematics) { - this.kinematics = kinematics; - } - /** * Detects modules that are skidding * @@ -20,7 +14,10 @@ public SkidDetection(SwerveDriveKinematics kinematics) { * @param yawVelocityRadPerSec From gyro * @return the skidding modules [Front left, front right, back left, back right] true if skidding */ - public boolean[] detectSkiddingModules(SwerveModuleState[] moduleStates, double yawVelocityRadPerSec) { + public boolean[] detectSkiddingModules( + SwerveDriveKinematics kinematics, + SwerveModuleState[] moduleStates, + double yawVelocityRadPerSec) { // Get the pure rotational component based on the gyro yaw velocity SwerveModuleState[] rotationalComponents = @@ -42,14 +39,25 @@ public boolean[] detectSkiddingModules(SwerveModuleState[] moduleStates, double } 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 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); + 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) + // If the measurement deviates by more than 2 standard deviations assume it's skidding (TODO: + // TUNE) if (Math.abs(xTransComponents[i] - xTransComponentAverage) >= xTransComponentStdDev * 2 || Math.abs(yTransComponents[i] - yTransComponentAverage) >= yTransComponentStdDev * 2) { result[i] = true; From b6a2fbbf71ab1d51c88f8d4d574e792884698f8a Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 29 Mar 2025 11:21:47 -0700 Subject: [PATCH 6/9] More requested changes --- .../frc/robot/subsystems/swerve/SwerveSubsystem.java | 8 ++++++-- src/main/java/frc/robot/utils/SkidDetection.java | 10 ++++------ 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index fbafb5c9..b43d66aa 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -272,8 +272,12 @@ private void updateOdometry() { missingModuleData.set(false); boolean[] skiddingModules = - skidDetection.detectSkiddingModules( - kinematics, getModuleStates(), gyroInputs.yawVelocityRadPerSec); + 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++) { diff --git a/src/main/java/frc/robot/utils/SkidDetection.java b/src/main/java/frc/robot/utils/SkidDetection.java index 613b8131..e10b4e0a 100644 --- a/src/main/java/frc/robot/utils/SkidDetection.java +++ b/src/main/java/frc/robot/utils/SkidDetection.java @@ -58,12 +58,10 @@ public boolean[] detectSkiddingModules( for (int i = 0; i <= moduleStates.length; i++) { // If the measurement deviates by more than 2 standard deviations assume it's skidding (TODO: // TUNE) - if (Math.abs(xTransComponents[i] - xTransComponentAverage) >= xTransComponentStdDev * 2 - || Math.abs(yTransComponents[i] - yTransComponentAverage) >= yTransComponentStdDev * 2) { - result[i] = true; - } else { - result[i] = false; - } + result[i] = + Math.abs(xTransComponents[i] - xTransComponentAverage) >= xTransComponentStdDev * 2 + || Math.abs(yTransComponents[i] - yTransComponentAverage) + >= yTransComponentStdDev * 2; } return result; From c501a4c893b8b3aa210cd7830d4bcc6989edd0a1 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 29 Mar 2025 11:36:40 -0700 Subject: [PATCH 7/9] Weight vision higher if a skid just happened (i think this is how i do that --- .../subsystems/swerve/SwerveSubsystem.java | 37 +++++++++++-------- 1 file changed, 21 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index b43d66aa..49743b7b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -69,6 +69,8 @@ public class SwerveSubsystem extends SubsystemBase { private final Vision[] cameras; + private Timer lastSkidTimer = new Timer(); + /** For delta tracking with PhoenixOdometryThread* */ private SwerveModulePosition[] lastModulePositions = new SwerveModulePosition[] { @@ -283,21 +285,22 @@ private void updateOdometry() { for (int i = 0; i <= skiddingModules.length; i++) { // If a module is skidding, average the other modules if (skiddingModules[i]) { - 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)); + 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)); } } @@ -363,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 : lastSkidTimer.get() / 3.0)); }); lastEstTimestamp = camera.inputs.captureTimestampMicros / 1e6; // if (Robot.ROBOT_TYPE != RobotType.REAL) From bbd7320c47d8c761227cad667c1737a7b288b268 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 29 Mar 2025 11:39:53 -0700 Subject: [PATCH 8/9] CHange how it weights vision on a skid --- src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 49743b7b..7f9b0936 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -368,7 +368,7 @@ private void updateVision() { .times(DriverStation.isAutonomous() ? 2.0 : 1.0) .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 : lastSkidTimer.get() / 3.0)); + .times(lastSkidTimer.isRunning() && lastSkidTimer.get() > 3 ? 1.0 : 3.0)); }); lastEstTimestamp = camera.inputs.captureTimestampMicros / 1e6; // if (Robot.ROBOT_TYPE != RobotType.REAL) From 6a93f66bc0748fb17d84d27a6b1cc42b6083098f Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 29 Mar 2025 16:25:00 -0700 Subject: [PATCH 9/9] Make adjusted weight correct --- src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 7f9b0936..25b6fa69 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -368,7 +368,7 @@ private void updateVision() { .times(DriverStation.isAutonomous() ? 2.0 : 1.0) .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 : 3.0)); + .times(lastSkidTimer.isRunning() && lastSkidTimer.get() > 3 ? 1.0 : 1/3.0)); }); lastEstTimestamp = camera.inputs.captureTimestampMicros / 1e6; // if (Robot.ROBOT_TYPE != RobotType.REAL)