From 6e6f28d1ac5461c38a66818366aa55dbcbcc9bef Mon Sep 17 00:00:00 2001
From: Daniel Z <5725958+dzil123@users.noreply.github.com>
Date: Sat, 28 Mar 2020 09:29:15 -0700
Subject: [PATCH] Add watchdog and epoch reporting to CommandScheduler (#2319)

---
 .../wpilibj2/command/CommandScheduler.java    | 27 +++++++++++++++++++
 1 file changed, 27 insertions(+)

diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
index c12d78bf48..715074d5bb 100644
--- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
+++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
@@ -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)");
     }
   }