Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

C++: Add Watchdog to CommandScheduler (#2437)

Browse Source 
C++ counterpart to #2319
This commit is contained in:
Prateek Machiraju
2020-03-28 12:29:51 -04:00
committed by GitHub
parent 6e6f28d1ac
commit b7a79c70cc
2 changed files with 33 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
View File

@@ -8,6 +8,7 @@
#include "frc2/command/CommandScheduler.h"
#include <frc/RobotState.h>
#include <frc/TimedRobot.h>
#include <frc/WPIErrors.h>
#include <frc/livewindow/LiveWindow.h>
#include <frc/smartdashboard/SendableBuilder.h>
@@ -17,6 +18,7 @@
#include <networktables/NetworkTableEntry.h>
#include <wpi/DenseMap.h>
#include <wpi/SmallVector.h>
#include <wpi/raw_ostream.h>
#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<void()> 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) {