[wpilib] Add EventLoop (#4104)

This is a generic expansion of the command-based Trigger framework.

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp

@@ -38,9 +38,10 @@ class CommandScheduler::Impl {
   // commands.  Also used as a list of currently-registered subsystems.
   wpi::DenseMap<Subsystem*, std::unique_ptr<Command>> subsystems;
 
+  frc::EventLoop defaultButtonLoop;
   // The set of currently-registered buttons that will be polled every
   // iteration.
-  wpi::SmallVector<wpi::unique_function<void()>, 4> buttons;
+  frc::EventLoop* activeButtonLoop{&defaultButtonLoop};
 
   bool disabled{false};
 
@@ -95,12 +96,20 @@ 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));
+frc::EventLoop* CommandScheduler::GetActiveButtonLoop() const {
+  return m_impl->activeButtonLoop;
+}
+
+void CommandScheduler::SetActiveButtonLoop(frc::EventLoop* loop) {
+  m_impl->activeButtonLoop = loop;
+}
+
+frc::EventLoop* CommandScheduler::GetDefaultButtonLoop() const {
+  return &(m_impl->defaultButtonLoop);
 }
 
 void CommandScheduler::ClearButtons() {
-  m_impl->buttons.clear();
+  m_impl->activeButtonLoop->Clear();
 }
 
 void CommandScheduler::Schedule(bool interruptible, Command* command) {
@@ -200,10 +209,11 @@ void CommandScheduler::Run() {
     m_watchdog.AddEpoch("Subsystem Periodic()");
   }
 
+  // Cache the active instance to avoid concurrency problems if SetActiveLoop()
+  // is called from inside the button bindings.
+  frc::EventLoop* loopCache = m_impl->activeButtonLoop;
   // Poll buttons for new commands to add.
-  for (auto&& button : m_impl->buttons) {
-    button();
-  }
+  loopCache->Poll();
   m_watchdog.AddEpoch("buttons.Run()");
 
   m_impl->inRunLoop = true;

wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp

@@ -8,22 +8,14 @@
 
 #include "frc2/command/InstantCommand.h"
 
+using namespace frc;
 using namespace frc2;
 
 Trigger::Trigger(const Trigger& other) = default;
 
 Trigger Trigger::WhenActive(Command* command, bool interruptible) {
-  CommandScheduler::GetInstance().AddButton(
-      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
-        bool pressed = m_isActive();
-
-        if (!pressedLast && pressed) {
-          command->Schedule(interruptible);
-        }
-
-        pressedLast = pressed;
-      });
-
+  this->Rising().IfHigh(
+      [command, interruptible] { command->Schedule(interruptible); });
   return *this;
 }
 
@@ -39,18 +31,8 @@ Trigger Trigger::WhenActive(std::function<void()> toRun,
 }
 
 Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
-  CommandScheduler::GetInstance().AddButton(
-      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
-        bool pressed = m_isActive();
-
-        if (pressed) {
-          command->Schedule(interruptible);
-        } else if (pressedLast && !pressed) {
-          command->Cancel();
-        }
-
-        pressedLast = pressed;
-      });
+  this->IfHigh([command, interruptible] { command->Schedule(interruptible); });
+  this->Falling().IfHigh([command] { command->Cancel(); });
   return *this;
 }
 
@@ -67,32 +49,15 @@ Trigger Trigger::WhileActiveContinous(
 }
 
 Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
-  CommandScheduler::GetInstance().AddButton(
-      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
-        bool pressed = m_isActive();
-
-        if (!pressedLast && pressed) {
-          command->Schedule(interruptible);
-        } else if (pressedLast && !pressed) {
-          command->Cancel();
-        }
-
-        pressedLast = pressed;
-      });
+  this->Rising().IfHigh(
+      [command, interruptible] { command->Schedule(interruptible); });
+  this->Falling().IfHigh([command] { command->Cancel(); });
   return *this;
 }
 
 Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
-  CommandScheduler::GetInstance().AddButton(
-      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
-        bool pressed = m_isActive();
-
-        if (pressedLast && !pressed) {
-          command->Schedule(interruptible);
-        }
-
-        pressedLast = pressed;
-      });
+  this->Falling().IfHigh(
+      [command, interruptible] { command->Schedule(interruptible); });
   return *this;
 }
 
@@ -108,41 +73,17 @@ Trigger Trigger::WhenInactive(std::function<void()> toRun,
 }
 
 Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
-  CommandScheduler::GetInstance().AddButton(
-      [pressedLast = m_isActive(), *this, command, interruptible]() mutable {
-        bool pressed = m_isActive();
-
-        if (!pressedLast && pressed) {
-          if (command->IsScheduled()) {
-            command->Cancel();
-          } else {
-            command->Schedule(interruptible);
-          }
-        }
-
-        pressedLast = pressed;
-      });
+  this->Rising().IfHigh([command, interruptible] {
+    if (command->IsScheduled()) {
+      command->Cancel();
+    } else {
+      command->Schedule(interruptible);
+    }
+  });
   return *this;
 }
 
 Trigger Trigger::CancelWhenActive(Command* command) {
-  CommandScheduler::GetInstance().AddButton(
-      [pressedLast = m_isActive(), *this, command]() mutable {
-        bool pressed = m_isActive();
-
-        if (!pressedLast && pressed) {
-          command->Cancel();
-        }
-
-        pressedLast = pressed;
-      });
+  this->Rising().IfHigh([command] { command->Cancel(); });
   return *this;
 }
-
-Trigger Trigger::Debounce(units::second_t debounceTime,
-                          frc::Debouncer::DebounceType type) {
-  return Trigger(
-      [debouncer = frc::Debouncer(debounceTime, type), *this]() mutable {
-        return debouncer.Calculate(m_isActive());
-      });
-}