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

Add watchdog and epoch reporting to CommandScheduler (#2319)

Browse Source
This commit is contained in:
Daniel Z
2020-03-28 09:29:15 -07:00
committed by GitHub
parent d14978e549
commit 6e6f28d1ac
1 changed files with 27 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
View File

@@ -24,6 +24,8 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.RobotState;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
@@ -83,6 +85,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
private final Map<Command, Boolean> m_toSchedule = new LinkedHashMap<>();
private final List<Command> m_toCancel = new ArrayList<>();
private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> { });
CommandScheduler() {
HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand2_Scheduler);
@@ -96,6 +99,16 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
});
}
/**
* Changes the period of the loop overrun watchdog. This should be be kept in sync with the
* TimedRobot period.
*
* @param period Period in seconds.
*/
public void setPeriod(double period) {
m_watchdog.setTimeout(period);
}
@Override
public void close() {
SendableRegistry.remove(this);
@@ -136,6 +149,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
for (Subsystem requirement : requirements) {
m_requirements.put(requirement, command);
}
m_watchdog.addEpoch(command.getName() + ".initialize()");
}
/**
@@ -233,16 +247,19 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
if (m_disabled) {
return;
}
m_watchdog.reset();
//Run the periodic method of all registered subsystems.
for (Subsystem subsystem : m_subsystems.keySet()) {
subsystem.periodic();
m_watchdog.addEpoch(subsystem.getClass().getSimpleName() + ".periodic()");
}
//Poll buttons for new commands to add.
for (Runnable button : m_buttons) {
button.run();
}
m_watchdog.addEpoch("buttons.run()");
m_inRunLoop = true;
//Run scheduled commands, remove finished commands.
@@ -257,6 +274,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
m_requirements.keySet().removeAll(command.getRequirements());
iterator.remove();
m_watchdog.addEpoch(command.getName() + ".end(true)");
continue;
}
@@ -264,6 +282,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
for (Consumer<Command> action : m_executeActions) {
action.accept(command);
}
m_watchdog.addEpoch(command.getName() + ".execute()");
if (command.isFinished()) {
command.end(false);
for (Consumer<Command> action : m_finishActions) {
@@ -272,6 +291,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
iterator.remove();
m_requirements.keySet().removeAll(command.getRequirements());
m_watchdog.addEpoch(command.getName() + ".end(false)");
}
}
m_inRunLoop = false;
@@ -295,6 +315,12 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
schedule(subsystemCommand.getValue());
}
}
m_watchdog.disable();
if (m_watchdog.isExpired()) {
System.out.println("CommandScheduler loop overrun");
m_watchdog.printEpochs();
}
}
/**
@@ -378,6 +404,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
}
m_scheduledCommands.remove(command);
m_requirements.keySet().removeAll(command.getRequirements());
m_watchdog.addEpoch(command.getName() + ".end(true)");
}
}