Allow Canandgyro to be used for swerve pose estimation

lasarobotics · Oct 30, 2024 · 91e533d · 91e533d
1 parent 524e032
commit 91e533d
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 2 deletions.
diff --git a/src/main/java/org/lasarobotics/drive/SwervePoseEstimatorService.java b/src/main/java/org/lasarobotics/drive/SwervePoseEstimatorService.java
@@ -17,6 +17,7 @@
 import org.lasarobotics.hardware.PurpleManager;
 import org.lasarobotics.hardware.ctre.Pigeon2;
 import org.lasarobotics.hardware.kauailabs.NavX2;
+import org.lasarobotics.hardware.reduxrobotics.Canandgyro;
 import org.lasarobotics.utils.GlobalConstants;
 import org.lasarobotics.vision.AprilTagCamera;
 import org.littletonrobotics.junction.AutoLog;
@@ -114,6 +115,24 @@ public SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev, Pigeon2 imu, MAX
     );
   }
 
+  /**
+   * Create Swerve Pose Estimator Service
+   * <p>
+   * This service runs in the background and keeps track of where the robot is on the field
+   * @param odometryStdDev Standard deviation of wheel odometry measurements
+   * @param imu Redux Robotics Canandgyro
+   * @param modules MAXSwerve modules
+   */
+  public SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev, Canandgyro imu, MAXSwerveModule... modules) {
+    this(
+      odometryStdDev,
+      () -> imu.getInputs().rotation2d,
+      () -> imu.getInputs().yawRate,
+      () -> true,
+      modules
+    );
+  }
+
   private SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev,
                                      Supplier<Rotation2d> rotation2dSupplier,
                                      Supplier<Measure<Velocity<Angle>>> yawRateSupplier,

diff --git a/src/main/java/org/lasarobotics/hardware/reduxrobotics/Canandgyro.java b/src/main/java/org/lasarobotics/hardware/reduxrobotics/Canandgyro.java
@@ -8,7 +8,6 @@
 import org.lasarobotics.hardware.PurpleManager;
 import org.littletonrobotics.junction.AutoLog;
 import org.littletonrobotics.junction.Logger;
-import org.littletonrobotics.junction.inputs.LoggableInputs;
 
 import com.reduxrobotics.frames.Frame;
 
@@ -132,7 +131,7 @@ protected void periodic() {
   }
 
   @Override
-  public LoggableInputs getInputs() {
+  public CanandgyroInputsAutoLogged getInputs() {
     synchronized (m_inputs) { return m_inputs; }
   }