Add error checking for high frequency odometry Spark Max values

Mechanical-Advantage · Feb 21, 2024 · 501a6c9 · 501a6c9
1 parent 17fd193
commit 501a6c9
Show file tree

Hide file tree

Showing 3 changed files with 50 additions and 9 deletions.
diff --git a/...rojects/advanced_swerve_drive/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/...rojects/advanced_swerve_drive/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
@@ -20,6 +20,7 @@
 import com.ctre.phoenix6.hardware.Pigeon2;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
+import java.util.OptionalDouble;
 import java.util.Queue;
 
 /** IO implementation for Pigeon2 */
@@ -44,7 +45,15 @@ public GyroIOPigeon2(boolean phoenixDrive) {
       yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue();
       yawPositionQueue =
           SparkMaxOdometryThread.getInstance()
-              .registerSignal(() -> pigeon.getYaw().getValueAsDouble());
+              .registerSignal(
+                  () -> {
+                    boolean valid = yaw.refresh().getStatus().isOK();
+                    if (valid) {
+                      return OptionalDouble.of(yaw.getValueAsDouble());
+                    } else {
+                      return OptionalDouble.empty();
+                    }
+                  });
     }
   }
 

diff --git a/...ects/advanced_swerve_drive/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/...ects/advanced_swerve_drive/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java
@@ -17,11 +17,13 @@
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.REVLibError;
 import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.RobotController;
+import java.util.OptionalDouble;
 import java.util.Queue;
 
 /**
@@ -116,9 +118,27 @@ public ModuleIOSparkMax(int index) {
         PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY));
     timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue();
     drivePositionQueue =
-        SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition);
+        SparkMaxOdometryThread.getInstance()
+            .registerSignal(
+                () -> {
+                  double value = driveEncoder.getPosition();
+                  if (driveSparkMax.getLastError() == REVLibError.kOk) {
+                    return OptionalDouble.of(value);
+                  } else {
+                    return OptionalDouble.empty();
+                  }
+                });
     turnPositionQueue =
-        SparkMaxOdometryThread.getInstance().registerSignal(turnRelativeEncoder::getPosition);
+        SparkMaxOdometryThread.getInstance()
+            .registerSignal(
+                () -> {
+                  double value = turnRelativeEncoder.getPosition();
+                  if (driveSparkMax.getLastError() == REVLibError.kOk) {
+                    return OptionalDouble.of(value);
+                  } else {
+                    return OptionalDouble.empty();
+                  }
+                });
 
     driveSparkMax.burnFlash();
     turnSparkMax.burnFlash();

diff --git a/...dvanced_swerve_drive/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/...dvanced_swerve_drive/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java
@@ -16,9 +16,10 @@
 import edu.wpi.first.wpilibj.Notifier;
 import java.util.ArrayList;
 import java.util.List;
+import java.util.OptionalDouble;
 import java.util.Queue;
 import java.util.concurrent.ArrayBlockingQueue;
-import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 
 /**
@@ -28,7 +29,7 @@
  * blocking thread. A Notifier thread is used to gather samples with consistent timing.
  */
 public class SparkMaxOdometryThread {
-  private List<DoubleSupplier> signals = new ArrayList<>();
+  private List<Supplier<OptionalDouble>> signals = new ArrayList<>();
   private List<Queue<Double>> queues = new ArrayList<>();
   private List<Queue<Double>> timestampQueues = new ArrayList<>();
 
@@ -53,7 +54,7 @@ public void start() {
     }
   }
 
-  public Queue<Double> registerSignal(DoubleSupplier signal) {
+  public Queue<Double> registerSignal(Supplier<OptionalDouble> signal) {
     Queue<Double> queue = new ArrayBlockingQueue<>(20);
     Drive.odometryLock.lock();
     try {
@@ -80,11 +81,22 @@ private void periodic() {
     Drive.odometryLock.lock();
     double timestamp = Logger.getRealTimestamp() / 1e6;
     try {
+      double[] values = new double[signals.size()];
+      boolean isValid = true;
       for (int i = 0; i < signals.size(); i++) {
-        queues.get(i).offer(signals.get(i).getAsDouble());
+        OptionalDouble value = signals.get(i).get();
+        if (value.isPresent()) {
+          values[i] = value.getAsDouble();
+        } else {
+          isValid = false;
+          break;
+        }
       }
-      for (int i = 0; i < timestampQueues.size(); i++) {
-        timestampQueues.get(i).offer(timestamp);
+      if (isValid) {
+        for (int i = 0; i < signals.size(); i++) {
+          queues.get(i).offer(values[i]);
+          timestampQueues.get(i).offer(timestamp);
+        }
       }
     } finally {
       Drive.odometryLock.unlock();