diff --git a/wpilibc/src/main/native/cpp/opmode/LinearOpMode.cpp b/wpilibc/src/main/native/cpp/opmode/LinearOpMode.cpp index 17f2a083c0..b0a818f6f9 100644 --- a/wpilibc/src/main/native/cpp/opmode/LinearOpMode.cpp +++ b/wpilibc/src/main/native/cpp/opmode/LinearOpMode.cpp @@ -4,6 +4,7 @@ #include "wpi/opmode/LinearOpMode.hpp" +#include "wpi/driverstation/DriverStation.hpp" #include "wpi/hal/DriverStation.h" #include "wpi/internal/DriverStationModeThread.hpp" @@ -14,6 +15,14 @@ void LinearOpMode::OpModeRun(int64_t opModeId) { word.SetOpModeId(opModeId); internal::DriverStationModeThread bgThread{word}; Run(); + + // Wait for opmode to be stopped or disabled, otherwise OpModeRobot will + // recreate and re-run the opmode immediately. + while (IsRunning() && DriverStation::IsEnabled() && + DriverStation::GetOpModeId() == opModeId) { + using namespace std::chrono_literals; + std::this_thread::sleep_for(20ms); + } } void LinearOpMode::OpModeStop() { diff --git a/wpilibj/src/main/java/org/wpilib/opmode/LinearOpMode.java b/wpilibj/src/main/java/org/wpilib/opmode/LinearOpMode.java index d72cef4956..a70cf2e581 100644 --- a/wpilibj/src/main/java/org/wpilib/opmode/LinearOpMode.java +++ b/wpilibj/src/main/java/org/wpilib/opmode/LinearOpMode.java @@ -67,6 +67,12 @@ public abstract class LinearOpMode implements OpMode { try (DriverStationModeThread bgThread = new DriverStationModeThread(word)) { run(); + + // Wait for opmode to be stopped or disabled, otherwise OpModeRobot will recreate and re-run + // the opmode immediately. + while (isRunning() && DriverStation.isEnabled() && DriverStation.getOpModeId() == opModeId) { + Thread.sleep(20); + } } }