switched to single-thread during simulation

BroncBotz3481 · Nov 20, 2024 · 7e3ca88 · 7e3ca88
1 parent 44dbc6c
commit 7e3ca88
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
@@ -138,9 +138,12 @@ public void setupPhotonVision()
   public void periodic()
   {
     // When vision is enabled we must manually update odometry in SwerveDrive
-    if (visionDriveTest)
+    if (SwerveDriveTelemetry.isSimulation || visionDriveTest)
     {
       swerveDrive.updateOdometry();
+    }
+    if (visionDriveTest)
+    {
       vision.updatePoseEstimation(swerveDrive);
     }
   }

diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java
@@ -288,7 +288,16 @@ public SwerveDrive(
       SwerveDriveTelemetry.measuredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount];
     }
 
-    setOdometryPeriod(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02);
+
+    // during simulation, call update odometry in main thread since maple-sim is not thread-safe
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      stopOdometryThread();
+    }
+    // otherwise, start the odometry thread
+    else {
+      setOdometryPeriod(0.02);
+    }
 
     checkIfTunerXCompatible();
   }