diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index 8c92894128..0898c9bdcd 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -8,6 +8,7 @@ #include "frc2/command/CommandScheduler.h" #include +#include #include #include #include @@ -17,6 +18,7 @@ #include #include #include +#include #include "frc2/command/CommandGroupBase.h" #include "frc2/command/CommandState.h" @@ -64,7 +66,10 @@ static bool ContainsKey(const TMap& map, TKey keyToCheck) { return map.find(keyToCheck) != map.end(); } -CommandScheduler::CommandScheduler() : m_impl(new Impl) { +CommandScheduler::CommandScheduler() + : m_impl(new Impl), m_watchdog(frc::TimedRobot::kDefaultPeriod, [] { + wpi::outs() << "CommandScheduler loop time overrun.\n"; + }) { HAL_Report(HALUsageReporting::kResourceType_Command, HALUsageReporting::kCommand2_Scheduler); frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler"); @@ -88,6 +93,10 @@ CommandScheduler& CommandScheduler::GetInstance() { return scheduler; } +void CommandScheduler::SetPeriod(units::second_t period) { + m_watchdog.SetTimeout(period); +} + void CommandScheduler::AddButton(wpi::unique_function button) { m_impl->buttons.emplace_back(std::move(button)); } @@ -141,6 +150,7 @@ void CommandScheduler::Schedule(bool interruptible, Command* command) { for (auto&& requirement : requirements) { m_impl->requirements[requirement] = command; } + m_watchdog.AddEpoch(command->GetName() + ".Initialize()"); } } @@ -177,15 +187,19 @@ void CommandScheduler::Run() { return; } + m_watchdog.Reset(); + // Run the periodic method of all registered subsystems. for (auto&& subsystem : m_impl->subsystems) { subsystem.getFirst()->Periodic(); + m_watchdog.AddEpoch("Subsystem Periodic()"); } // Poll buttons for new commands to add. for (auto&& button : m_impl->buttons) { button(); } + m_watchdog.AddEpoch("buttons.Run()"); m_impl->inRunLoop = true; // Run scheduled commands, remove finished commands. @@ -202,6 +216,7 @@ void CommandScheduler::Run() { for (auto&& action : m_impl->executeActions) { action(*command); } + m_watchdog.AddEpoch(command->GetName() + ".Execute()"); if (command->IsFinished()) { command->End(false); @@ -214,6 +229,7 @@ void CommandScheduler::Run() { } m_impl->scheduledCommands.erase(iterator); + m_watchdog.AddEpoch(command->GetName() + ".End(false)"); } } m_impl->inRunLoop = false; @@ -236,6 +252,11 @@ void CommandScheduler::Run() { Schedule({subsystem.getSecond().get()}); } } + + m_watchdog.Disable(); + if (m_watchdog.IsExpired()) { + m_watchdog.PrintEpochs(); + } } void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) { @@ -297,6 +318,7 @@ void CommandScheduler::Cancel(Command* command) { for (auto&& action : m_impl->interruptActions) { action(*command); } + m_watchdog.AddEpoch(command->GetName() + ".End(true)"); m_impl->scheduledCommands.erase(find); for (auto&& requirement : m_impl->requirements) { if (requirement.second == command) { diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h index f1d0917003..c3ee1eb6df 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h @@ -13,8 +13,10 @@ #include #include +#include #include #include +#include #include #include @@ -46,6 +48,12 @@ class CommandScheduler final : public frc::Sendable, using Action = std::function; + /** + * Changes the period of the loop overrun watchdog. This should be kept in + * sync with the TimedRobot period. + */ + void SetPeriod(units::second_t period); + /** * Adds a button binding to the scheduler, which will be polled to schedule * commands. @@ -337,6 +345,8 @@ class CommandScheduler final : public frc::Sendable, class Impl; std::unique_ptr m_impl; + frc::Watchdog m_watchdog; + friend class CommandTestBase; }; } // namespace frc2