[wpilib, commands] Cache controller BooleanEvents/Triggers and directly construct Triggers (#6738)

This has been a common footgun for teams, due to calling the factory functions in periodic loops.

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandGenericHID.java

@@ -4,9 +4,12 @@
 
 package edu.wpi.first.wpilibj2.command.button;
 
+import edu.wpi.first.math.Pair;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import java.util.HashMap;
+import java.util.Map;
 
 /**
  * A version of {@link GenericHID} with {@link Trigger} factories for command-based.
@@ -15,6 +18,12 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler;
  */
 public class CommandGenericHID {
   private final GenericHID m_hid;
+  private final Map<EventLoop, Map<Integer, Trigger>> m_buttonCache = new HashMap<>();
+  private final Map<EventLoop, Map<Pair<Integer, Double>, Trigger>> m_axisLessThanCache =
+      new HashMap<>();
+  private final Map<EventLoop, Map<Pair<Integer, Double>, Trigger>> m_axisGreaterThanCache =
+      new HashMap<>();
+  private final Map<EventLoop, Map<Integer, Trigger>> m_povCache = new HashMap<>();
 
   /**
    * Construct an instance of a device.
@@ -43,7 +52,7 @@ public class CommandGenericHID {
    * @see #button(int, EventLoop)
    */
   public Trigger button(int button) {
-    return this.button(button, CommandScheduler.getInstance().getDefaultButtonLoop());
+    return button(button, CommandScheduler.getInstance().getDefaultButtonLoop());
   }
 
   /**
@@ -54,7 +63,8 @@ public class CommandGenericHID {
    * @return an event instance representing the button's digital signal attached to the given loop.
    */
   public Trigger button(int button, EventLoop loop) {
-    return new Trigger(loop, () -> m_hid.getRawButton(button));
+    var cache = m_buttonCache.computeIfAbsent(loop, k -> new HashMap<>());
+    return cache.computeIfAbsent(button, k -> new Trigger(loop, () -> m_hid.getRawButton(k)));
   }
 
   /**
@@ -85,7 +95,10 @@ public class CommandGenericHID {
    * @return a Trigger instance based around this angle of a POV on the HID.
    */
   public Trigger pov(int pov, int angle, EventLoop loop) {
-    return new Trigger(loop, () -> m_hid.getPOV(pov) == angle);
+    var cache = m_povCache.computeIfAbsent(loop, k -> new HashMap<>());
+    // angle can be -1, so use 3600 instead of 360
+    return cache.computeIfAbsent(
+        pov * 3600 + angle, k -> new Trigger(loop, () -> m_hid.getPOV(pov) == angle));
   }
 
   /**
@@ -212,7 +225,9 @@ public class CommandGenericHID {
    *     threshold.
    */
   public Trigger axisLessThan(int axis, double threshold, EventLoop loop) {
-    return m_hid.axisLessThan(axis, threshold, loop).castTo(Trigger::new);
+    var cache = m_axisLessThanCache.computeIfAbsent(loop, k -> new HashMap<>());
+    return cache.computeIfAbsent(
+        Pair.of(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) < threshold));
   }
 
   /**
@@ -240,7 +255,9 @@ public class CommandGenericHID {
    *     threshold.
    */
   public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) {
-    return m_hid.axisGreaterThan(axis, threshold, loop).castTo(Trigger::new);
+    var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>());
+    return cache.computeIfAbsent(
+        Pair.of(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) > threshold));
   }
 
   /**

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandJoystick.java

@@ -55,7 +55,7 @@ public class CommandJoystick extends CommandGenericHID {
    *     given loop.
    */
   public Trigger trigger(EventLoop loop) {
-    return m_hid.trigger(loop).castTo(Trigger::new);
+    return button(Joystick.ButtonType.kTrigger.value, loop);
   }
 
   /**
@@ -77,7 +77,7 @@ public class CommandJoystick extends CommandGenericHID {
    *     loop.
    */
   public Trigger top(EventLoop loop) {
-    return m_hid.top(loop).castTo(Trigger::new);
+    return button(Joystick.ButtonType.kTop.value, loop);
   }
 
   /**

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

@@ -13,7 +13,7 @@ frc::GenericHID& CommandGenericHID::GetHID() {
 }
 
 Trigger CommandGenericHID::Button(int button, frc::EventLoop* loop) const {
-  return m_hid.Button(button, loop).CastTo<Trigger>();
+  return Trigger(loop, [this, button] { return m_hid.GetRawButton(button); });
 }
 
 Trigger CommandGenericHID::POV(int angle, frc::EventLoop* loop) const {

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

@@ -14,11 +14,11 @@ frc::Joystick& CommandJoystick::GetHID() {
 }
 
 Trigger CommandJoystick::Trigger(frc::EventLoop* loop) const {
-  return m_hid.Trigger(loop).CastTo<class Trigger>();
+  return Button(frc::Joystick::ButtonType::kTriggerButton, loop);
 }
 
 Trigger CommandJoystick::Top(frc::EventLoop* loop) const {
-  return m_hid.Top(loop).CastTo<class Trigger>();
+  return Button(frc::Joystick::ButtonType::kTopButton, loop);
 }
 
 double CommandJoystick::GetMagnitude() const {