diff --git a/hal/src/main/native/include/hal/simulation/NotifierData.h b/hal/src/main/native/include/hal/simulation/NotifierData.h
index 445e9395f8..a5b68b6e2e 100644
--- a/hal/src/main/native/include/hal/simulation/NotifierData.h
+++ b/hal/src/main/native/include/hal/simulation/NotifierData.h
@@ -17,7 +17,7 @@ struct HALSIM_NotifierInfo {
   HAL_NotifierHandle handle;
   char name[64];
   uint64_t timeout;
-  HAL_Bool running;
+  HAL_Bool waitTimeValid;
 };
 
 uint64_t HALSIM_GetNextNotifierTimeout(void);
diff --git a/hal/src/main/native/sim/MockHooks.cpp b/hal/src/main/native/sim/MockHooks.cpp
index edfbb0dcd9..fe29fe6540 100644
--- a/hal/src/main/native/sim/MockHooks.cpp
+++ b/hal/src/main/native/sim/MockHooks.cpp
@@ -92,6 +92,7 @@ void HALSIM_ResumeTiming(void) {
 HAL_Bool HALSIM_IsTimingPaused(void) { return IsTimingPaused(); }
 
 void HALSIM_StepTiming(uint64_t delta) {
+  WaitNotifiers();
   StepTiming(delta);
   WakeupWaitNotifiers();
 }
diff --git a/hal/src/main/native/sim/Notifier.cpp b/hal/src/main/native/sim/Notifier.cpp
index 8138468b7c..a697275542 100644
--- a/hal/src/main/native/sim/Notifier.cpp
+++ b/hal/src/main/native/sim/Notifier.cpp
@@ -28,10 +28,11 @@
 namespace {
 struct Notifier {
   std::string name;
-  uint64_t waitTime;
+  uint64_t waitTime = UINT64_MAX;
   bool active = true;
-  bool running = false;
-  uint64_t count = 0;
+  bool waitTimeValid = false;    // True if waitTime is set and in the future
+  bool waitingForAlarm = false;  // True if in HAL_WaitForNotifierAlarm()
+  uint64_t waitCount = 0;        // Counts calls to HAL_WaitForNotifierAlarm()
   wpi::mutex mutex;
   wpi::condition_variable cond;
 };
@@ -51,7 +52,7 @@ class NotifierHandleContainer
       {
         std::scoped_lock lock(notifier->mutex);
         notifier->active = false;
-        notifier->running = false;
+        notifier->waitTimeValid = false;
       }
       notifier->cond.notify_all();  // wake up any waiting threads
     });
@@ -83,18 +84,52 @@ void WakeupNotifiers() {
   });
 }
 
+void WaitNotifiers() {
+  std::unique_lock ulock(notifiersWaiterMutex);
+  wpi::SmallVector<HAL_NotifierHandle, 8> waiters;
+
+  // Wait for all Notifiers to hit HAL_WaitForNotifierAlarm()
+  notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
+    std::scoped_lock lock(notifier->mutex);
+    if (notifier->active && !notifier->waitingForAlarm) {
+      waiters.emplace_back(handle);
+    }
+  });
+  for (;;) {
+    int count = 0;
+    int end = waiters.size();
+    while (count < end) {
+      auto& it = waiters[count];
+      if (auto notifier = notifierHandles->Get(it)) {
+        std::scoped_lock lock(notifier->mutex);
+        if (notifier->active && !notifier->waitingForAlarm) {
+          ++count;
+          continue;
+        }
+      }
+      // No longer need to wait for it, put at end so it can be erased
+      std::swap(it, waiters[--end]);
+    }
+    if (count == 0) break;
+    waiters.resize(count);
+    notifiersWaiterCond.wait_for(ulock, std::chrono::duration<double>(1));
+  }
+}
+
 void WakeupWaitNotifiers() {
   std::unique_lock ulock(notifiersWaiterMutex);
   int32_t status = 0;
   uint64_t curTime = HAL_GetFPGATime(&status);
   wpi::SmallVector<std::pair<HAL_NotifierHandle, uint64_t>, 8> waiters;
+
+  // Wake up Notifiers that have expired timeouts
   notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
     std::scoped_lock lock(notifier->mutex);
-    // only wait for it if it's going to wake up (either because
-    // the timeout has expired or the alarm hasn't been waited on yet)
-    if (notifier->running &&
-        (notifier->count == 0 || curTime >= notifier->waitTime)) {
-      waiters.emplace_back(handle, notifier->count);
+
+    // Only wait for the Notifier if it has a valid timeout that's expired
+    if (notifier->active && notifier->waitTimeValid &&
+        curTime >= notifier->waitTime) {
+      waiters.emplace_back(handle, notifier->waitCount);
       notifier->cond.notify_all();
     }
   });
@@ -105,12 +140,15 @@ void WakeupWaitNotifiers() {
       auto& it = waiters[count];
       if (auto notifier = notifierHandles->Get(it.first)) {
         std::scoped_lock lock(notifier->mutex);
-        if (notifier->active && notifier->count == it.second) {
+
+        // waitCount is used here instead of waitingForAlarm because we want to
+        // wait until HAL_WaitForNotifierAlarm() is exited, then reentered
+        if (notifier->active && notifier->waitCount == it.second) {
           ++count;
           continue;
         }
       }
-      // no longer need to wait for it, put at end so it can be erased
+      // No longer need to wait for it, put at end so it can be erased
       it.swap(waiters[--end]);
     }
     if (count == 0) break;
@@ -148,7 +186,7 @@ void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
   {
     std::scoped_lock lock(notifier->mutex);
     notifier->active = false;
-    notifier->running = false;
+    notifier->waitTimeValid = false;
   }
   notifier->cond.notify_all();
 }
@@ -161,7 +199,7 @@ void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
   {
     std::scoped_lock lock(notifier->mutex);
     notifier->active = false;
-    notifier->running = false;
+    notifier->waitTimeValid = false;
   }
   notifier->cond.notify_all();
 }
@@ -174,7 +212,7 @@ void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
   {
     std::scoped_lock lock(notifier->mutex);
     notifier->waitTime = triggerTime;
-    notifier->running = (triggerTime != UINT64_MAX);
+    notifier->waitTimeValid = (triggerTime != UINT64_MAX);
   }
 
   // We wake up any waiters to change how long they're sleeping for
@@ -188,7 +226,7 @@ void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
 
   {
     std::scoped_lock lock(notifier->mutex);
-    notifier->running = false;
+    notifier->waitTimeValid = false;
   }
 }
 
@@ -199,26 +237,29 @@ uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
 
   std::unique_lock ulock(notifiersWaiterMutex);
   std::unique_lock lock(notifier->mutex);
-  ++notifier->count;
+  notifier->waitingForAlarm = true;
+  ++notifier->waitCount;
   ulock.unlock();
   notifiersWaiterCond.notify_all();
   while (notifier->active) {
     uint64_t curTime = HAL_GetFPGATime(status);
-    if (notifier->running && curTime >= notifier->waitTime) {
-      notifier->running = false;
+    if (notifier->waitTimeValid && curTime >= notifier->waitTime) {
+      notifier->waitTimeValid = false;
+      notifier->waitingForAlarm = false;
       return curTime;
     }
 
-    double waitTime;
-    if (!notifier->running || notifiersPaused) {
+    double waitDuration;
+    if (!notifier->waitTimeValid || notifiersPaused) {
       // If not running, wait 1000 seconds
-      waitTime = 1000.0;
+      waitDuration = 1000.0;
     } else {
-      waitTime = (notifier->waitTime - curTime) * 1e-6;
+      waitDuration = (notifier->waitTime - curTime) * 1e-6;
     }
 
-    notifier->cond.wait_for(lock, std::chrono::duration<double>(waitTime));
+    notifier->cond.wait_for(lock, std::chrono::duration<double>(waitDuration));
   }
+  notifier->waitingForAlarm = false;
   return 0;
 }
 
@@ -226,7 +267,8 @@ uint64_t HALSIM_GetNextNotifierTimeout(void) {
   uint64_t timeout = UINT64_MAX;
   notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) {
     std::scoped_lock lock(notifier->mutex);
-    if (notifier->active && notifier->running && timeout > notifier->waitTime)
+    if (notifier->active && notifier->waitTimeValid &&
+        timeout > notifier->waitTime)
       timeout = notifier->waitTime;
   });
   return timeout;
@@ -257,7 +299,7 @@ int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) {
         arr[num].name[sizeof(arr[num].name) - 1] = '\0';
       }
       arr[num].timeout = notifier->waitTime;
-      arr[num].running = notifier->running;
+      arr[num].waitTimeValid = notifier->waitTimeValid;
     }
     ++num;
   });
diff --git a/hal/src/main/native/sim/NotifierInternal.h b/hal/src/main/native/sim/NotifierInternal.h
index ad27016dfb..e6692ca833 100644
--- a/hal/src/main/native/sim/NotifierInternal.h
+++ b/hal/src/main/native/sim/NotifierInternal.h
@@ -11,5 +11,6 @@ namespace hal {
 void PauseNotifiers();
 void ResumeNotifiers();
 void WakeupNotifiers();
+void WaitNotifiers();
 void WakeupWaitNotifiers();
 }  // namespace hal
diff --git a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp
new file mode 100644
index 0000000000..6fe11b4481
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp
@@ -0,0 +1,311 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/TimedRobot.h"  // NOLINT(build/include_order)
+
+#include <stdint.h>
+
+#include <atomic>
+#include <thread>
+
+#include "frc/simulation/DriverStationSim.h"
+#include "frc/simulation/SimHooks.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+namespace {
+class TimedRobotTest : public ::testing::Test {
+ protected:
+  void SetUp() override { frc::sim::PauseTiming(); }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+};
+
+class MockRobot : public TimedRobot {
+ public:
+  std::atomic<uint32_t> m_robotInitCount{0};
+  std::atomic<uint32_t> m_simulationInitCount{0};
+  std::atomic<uint32_t> m_disabledInitCount{0};
+  std::atomic<uint32_t> m_autonomousInitCount{0};
+  std::atomic<uint32_t> m_teleopInitCount{0};
+  std::atomic<uint32_t> m_testInitCount{0};
+
+  std::atomic<uint32_t> m_robotPeriodicCount{0};
+  std::atomic<uint32_t> m_simulationPeriodicCount{0};
+  std::atomic<uint32_t> m_disabledPeriodicCount{0};
+  std::atomic<uint32_t> m_autonomousPeriodicCount{0};
+  std::atomic<uint32_t> m_teleopPeriodicCount{0};
+  std::atomic<uint32_t> m_testPeriodicCount{0};
+
+  void RobotInit() override { m_robotInitCount++; }
+
+  void SimulationInit() override { m_simulationInitCount++; }
+
+  void DisabledInit() override { m_disabledInitCount++; }
+
+  void AutonomousInit() override { m_autonomousInitCount++; }
+
+  void TeleopInit() override { m_teleopInitCount++; }
+
+  void TestInit() override { m_testInitCount++; }
+
+  void RobotPeriodic() override { m_robotPeriodicCount++; }
+
+  void SimulationPeriodic() override { m_simulationPeriodicCount++; }
+
+  void DisabledPeriodic() override { m_disabledPeriodicCount++; }
+
+  void AutonomousPeriodic() override { m_autonomousPeriodicCount++; }
+
+  void TeleopPeriodic() override { m_teleopPeriodicCount++; }
+
+  void TestPeriodic() override { m_testPeriodicCount++; }
+};
+}  // namespace
+
+TEST_F(TimedRobotTest, Disabled) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(2u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(2u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, Autonomous) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::SetAutonomous(true);
+  frc::sim::DriverStationSim::SetTest(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(1u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(2u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(2u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, Teleop) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetTest(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(1u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(1u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(1u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(2u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(2u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, Test) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetTest(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(1u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(1u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(1u, robot.m_testInitCount);
+
+  EXPECT_EQ(2u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(2u, robot.m_testPeriodicCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java
new file mode 100644
index 0000000000..47ffaa09d4
--- /dev/null
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java
@@ -0,0 +1,395 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.util.concurrent.atomic.AtomicInteger;
+
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class TimedRobotTest {
+  @SuppressWarnings("PMD.TooManyMethods")
+  class MockRobot extends TimedRobot {
+    public final AtomicInteger m_robotInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_simulationInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_disabledInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_autonomousInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_teleopInitCount = new AtomicInteger(0);
+    public final AtomicInteger m_testInitCount = new AtomicInteger(0);
+
+    public final AtomicInteger m_robotPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_simulationPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_disabledPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_autonomousPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_teleopPeriodicCount = new AtomicInteger(0);
+    public final AtomicInteger m_testPeriodicCount = new AtomicInteger(0);
+
+    @Override
+    public void robotInit() {
+      m_robotInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void simulationInit() {
+      m_simulationInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void disabledInit() {
+      m_disabledInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void autonomousInit() {
+      m_autonomousInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void teleopInit() {
+      m_teleopInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void testInit() {
+      m_testInitCount.addAndGet(1);
+    }
+
+    @Override
+    public void robotPeriodic() {
+      m_robotPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void simulationPeriodic() {
+      m_simulationPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void disabledPeriodic() {
+      m_disabledPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void autonomousPeriodic() {
+      m_autonomousPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void teleopPeriodic() {
+      m_teleopPeriodicCount.addAndGet(1);
+    }
+
+    @Override
+    public void testPeriodic() {
+      m_testPeriodicCount.addAndGet(1);
+    }
+  }
+
+  @BeforeEach
+  void setup() {
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void disabledTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(() -> {
+      robot.startCompetition();
+    });
+    robotThread.start();
+
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_robotPeriodicCount.get());
+    assertEquals(1, robot.m_simulationPeriodicCount.get());
+    assertEquals(1, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(2, robot.m_robotPeriodicCount.get());
+    assertEquals(2, robot.m_simulationPeriodicCount.get());
+    assertEquals(2, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void autonomousTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(() -> {
+      robot.startCompetition();
+    });
+    robotThread.start();
+
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(true);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_robotPeriodicCount.get());
+    assertEquals(1, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(1, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(2, robot.m_robotPeriodicCount.get());
+    assertEquals(2, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(2, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void teleopTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(() -> {
+      robot.startCompetition();
+    });
+    robotThread.start();
+
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(1, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_robotPeriodicCount.get());
+    assertEquals(1, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(1, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(1, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(2, robot.m_robotPeriodicCount.get());
+    assertEquals(2, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(2, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void testTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread = new Thread(() -> {
+      robot.startCompetition();
+    });
+    robotThread.start();
+
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(true);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(0, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(1, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_robotPeriodicCount.get());
+    assertEquals(1, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(1, robot.m_testPeriodicCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_robotInitCount.get());
+    assertEquals(1, robot.m_simulationInitCount.get());
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(1, robot.m_testInitCount.get());
+
+    assertEquals(2, robot.m_robotPeriodicCount.get());
+    assertEquals(2, robot.m_simulationPeriodicCount.get());
+    assertEquals(0, robot.m_disabledPeriodicCount.get());
+    assertEquals(0, robot.m_autonomousPeriodicCount.get());
+    assertEquals(0, robot.m_teleopPeriodicCount.get());
+    assertEquals(2, robot.m_testPeriodicCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+}