add whileTrueLowPriority to trigger and condense some other logic

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

@@ -10,6 +10,8 @@
 import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.WrapperCommand;
+
 import java.util.function.BooleanSupplier;
 
 /**
@@ -139,23 +141,60 @@ public void run() {
    */
   public Trigger whileTrue(Command command) {
     requireNonNullParam(command, "command", "whileTrue");
-    m_loop.bind(
-        new Runnable() {
-          private boolean m_pressedLast = m_condition.getAsBoolean();
-
-          @Override
-          public void run() {
-            boolean pressed = m_condition.getAsBoolean();
+    return onTrue(new WrapperCommand(command) {
+      @Override
+      public boolean isFinished() {
+          return !m_condition.getAsBoolean() || super.isFinished();
+      }
+    });
+  }
 
-            if (!m_pressedLast && pressed) {
-              command.schedule();
-            } else if (m_pressedLast && !pressed) {
-              command.cancel();
-            }
+  /**
+   * Sets up a {@link Command} to mimic a default command while a condition is true.
+   *
+   * <p>The command will not interrupt any command other than the original default command of the
+   * subsystems the command requires.
+   *
+   * @param cmd the command to schedule
+   * @return this trigger, so calls can be chained
+   */
+  public Trigger whileTrueLowPriority(Command cmd) {
+    // you could implement this by overiding the subsystems default command
+    // but that has alot of foot guns and likely would leak into causing issues
+    m_loop.bind(
+            new Runnable() {
+              // caching the sceduler to save slightly on fetching instance
+              private final CommandScheduler scheduler = CommandScheduler.getInstance();
+
+              public boolean freeToScehdule(Command cmd) {
+                var requirements = cmd.getRequirements();
+                for (var requirement : requirements) {
+                  // scheduler.requiring(requirement) returns a command or null
+                  // requirement.getDefaultCommand() returns the default command or null
+                  //
+                  // logic assertions:
+                  // - if both are null the command is free to schedule
+                  // - if the default command is null and required command is not null the command
+                  // is blocked from scheduling, so return false
+                  // - required command should never be null if default command is not null
+                  if (scheduler.requiring(requirement) != requirement.getDefaultCommand()) {
+                    return false;
+                  }
+                }
+                return true;
+              }
 
-            m_pressedLast = pressed;
-          }
-        });
+              @Override
+              public void run() {
+                if (m_condition.getAsBoolean()) {
+                  if (!scheduler.isScheduled(cmd) && freeToScehdule(cmd)) {
+                    cmd.schedule();
+                  }
+                } else if (scheduler.isScheduled(cmd)) {
+                  cmd.cancel();
+                }
+              }
+            });
     return this;
   }
 
@@ -171,24 +210,12 @@ public void run() {
    */
   public Trigger whileFalse(Command command) {
     requireNonNullParam(command, "command", "whileFalse");
-    m_loop.bind(
-        new Runnable() {
-          private boolean m_pressedLast = m_condition.getAsBoolean();
-
-          @Override
-          public void run() {
-            boolean pressed = m_condition.getAsBoolean();
-
-            if (m_pressedLast && !pressed) {
-              command.schedule();
-            } else if (!m_pressedLast && pressed) {
-              command.cancel();
-            }
-
-            m_pressedLast = pressed;
-          }
-        });
-    return this;
+    return onFalse(new WrapperCommand(command) {
+      @Override
+      public boolean isFinished() {
+          return m_condition.getAsBoolean() || super.isFinished();
+      }
+    });
   }
 
   /**