Initialize swerve module positions

lasarobotics · Sep 29, 2024 · 7a3b1a1 · 7a3b1a1
1 parent df7093e
commit 7a3b1a1
Showing 1 changed file with 3 additions and 0 deletions.
diff --git a/src/main/java/org/lasarobotics/drive/SwervePoseEstimatorService.java b/src/main/java/org/lasarobotics/drive/SwervePoseEstimatorService.java
@@ -152,6 +152,9 @@ private SwervePoseEstimatorService(Matrix<N3,N1> odometryStdDev,
       rRearModule.get().getPosition(syncedTimestamp)
     };
 
+    // Get initial swerve positions
+    this.m_previousModulePositions = m_swerveModulePositionSupplier.apply(m_syncedTimestamp);
+
     // Initialise kinematics
     this.m_kinematics = new SwerveDriveKinematics(
       lFrontModule.get().getModuleCoordinate(),