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 b5c2d3cfd0..4bd927f7f6 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 @@ -4,6 +4,8 @@ package edu.wpi.first.wpilibj2.command; +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; @@ -15,13 +17,13 @@ import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Watchdog; +import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj2.command.button.Trigger; import java.util.ArrayList; -import java.util.Collection; import java.util.Collections; import java.util.Iterator; import java.util.LinkedHashMap; -import java.util.LinkedHashSet; import java.util.List; import java.util.Map; import java.util.Set; @@ -64,8 +66,9 @@ public final class CommandScheduler implements NTSendable, AutoCloseable { // as a list of currently-registered subsystems. private final Map m_subsystems = new LinkedHashMap<>(); + private final EventLoop m_defaultButtonLoop = new EventLoop(); // The set of currently-registered buttons that will be polled every iteration. - private final Collection m_buttons = new LinkedHashSet<>(); + private EventLoop m_activeButtonLoop = m_defaultButtonLoop; private boolean m_disabled; @@ -114,18 +117,53 @@ public final class CommandScheduler implements NTSendable, AutoCloseable { LiveWindow.setDisabledListener(null); } + /** + * Get the default button poll. + * + * @return a reference to the default {@link EventLoop} object polling buttons. + */ + public EventLoop getDefaultButtonLoop() { + return m_defaultButtonLoop; + } + + /** + * Get the active button poll. + * + * @return a reference to the current {@link EventLoop} object polling buttons. + */ + public EventLoop getActiveButtonLoop() { + return m_activeButtonLoop; + } + + /** + * Replace the button poll with another one. + * + * @param loop the new button polling loop object. + */ + public void setActiveButtonLoop(EventLoop loop) { + m_activeButtonLoop = + requireNonNullParam(loop, "loop", "CommandScheduler" + ".replaceButtonEventLoop"); + } + /** * Adds a button binding to the scheduler, which will be polled to schedule commands. * * @param button The button to add + * @deprecated Use {@link Trigger} */ + @Deprecated(since = "2023") public void addButton(Runnable button) { - m_buttons.add(button); + m_activeButtonLoop.bind(() -> true, button); } - /** Removes all button bindings from the scheduler. */ + /** + * Removes all button bindings from the scheduler. + * + * @deprecated call {@link EventLoop#clear()} on {@link #getActiveButtonLoop()} directly instead. + */ + @Deprecated(since = "2023") public void clearButtons() { - m_buttons.clear(); + m_activeButtonLoop.clear(); } /** @@ -254,10 +292,11 @@ public final class CommandScheduler implements NTSendable, AutoCloseable { m_watchdog.addEpoch(subsystem.getClass().getSimpleName() + ".periodic()"); } + // Cache the active instance to avoid concurrency problems if setActiveLoop() is called from + // inside the button bindings. + EventLoop loopCache = m_activeButtonLoop; // Poll buttons for new commands to add. - for (Runnable button : m_buttons) { - button.run(); - } + loopCache.poll(); m_watchdog.addEpoch("buttons.run()"); m_inRunLoop = true; diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java index 0f8594d9f4..0ac8df15e8 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java @@ -20,9 +20,11 @@ import java.util.function.BooleanSupplier; */ public class Button extends Trigger { /** - * Default constructor; creates a button that is never pressed (unless {@link Button#get()} is - * overridden). + * Default constructor; creates a button that is never pressed. + * + * @deprecated Replace with {@code new Button(() -> false) }. */ + @Deprecated(since = "2023") public Button() {} /** diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java index d856089b52..58cf804c16 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java @@ -4,6 +4,8 @@ package edu.wpi.first.wpilibj2.command.button; +import java.util.concurrent.atomic.AtomicBoolean; + /** * This class is intended to be used within a program. The programmer can manually set its value. * Also includes a setting for whether or not it should invert its value. @@ -11,8 +13,9 @@ package edu.wpi.first.wpilibj2.command.button; *

This class is provided by the NewCommands VendorDep */ public class InternalButton extends Button { - private boolean m_pressed; - private boolean m_inverted; + // need to be references, so they can be mutated after being captured in the constructor. + private final AtomicBoolean m_pressed; + private final AtomicBoolean m_inverted; /** Creates an InternalButton that is not inverted. */ public InternalButton() { @@ -26,19 +29,24 @@ public class InternalButton extends Button { * when set to false. */ public InternalButton(boolean inverted) { - m_pressed = m_inverted = inverted; + this(new AtomicBoolean(), new AtomicBoolean(inverted)); + } + + /* + * Mock constructor so the AtomicBoolean objects can be constructed before the super + * constructor invocation. + */ + private InternalButton(AtomicBoolean state, AtomicBoolean inverted) { + super(() -> state.get() != inverted.get()); + this.m_pressed = state; + this.m_inverted = inverted; } public void setInverted(boolean inverted) { - m_inverted = inverted; + m_inverted.set(inverted); } public void setPressed(boolean pressed) { - m_pressed = pressed; - } - - @Override - public boolean get() { - return m_pressed ^ m_inverted; + m_pressed.set(pressed); } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java index ae4aa1c8bc..d3bf8412f2 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java @@ -14,9 +14,6 @@ import edu.wpi.first.wpilibj.GenericHID; *

This class is provided by the NewCommands VendorDep */ public class JoystickButton extends Button { - private final GenericHID m_joystick; - private final int m_buttonNumber; - /** * Creates a joystick button for triggering commands. * @@ -24,19 +21,7 @@ public class JoystickButton extends Button { * @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) } */ public JoystickButton(GenericHID joystick, int buttonNumber) { + super(() -> joystick.getRawButton(buttonNumber)); requireNonNullParam(joystick, "joystick", "JoystickButton"); - - m_joystick = joystick; - m_buttonNumber = buttonNumber; - } - - /** - * Gets the value of the joystick button. - * - * @return The value of the joystick button - */ - @Override - public boolean get() { - return m_joystick.getRawButton(m_buttonNumber); } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/NetworkButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/NetworkButton.java index 5c66af64f6..8eb7d445ad 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/NetworkButton.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/NetworkButton.java @@ -4,6 +4,8 @@ package edu.wpi.first.wpilibj2.command.button; +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; @@ -14,15 +16,14 @@ import edu.wpi.first.networktables.NetworkTableInstance; *

This class is provided by the NewCommands VendorDep */ public class NetworkButton extends Button { - private final NetworkTableEntry m_entry; - /** * Creates a NetworkButton that commands can be bound to. * * @param entry The entry that is the value. */ public NetworkButton(NetworkTableEntry entry) { - m_entry = entry; + super(() -> entry.getInstance().isConnected() && entry.getBoolean(false)); + requireNonNullParam(entry, "entry", "NetworkButton"); } /** @@ -44,9 +45,4 @@ public class NetworkButton extends Button { public NetworkButton(String table, String field) { this(NetworkTableInstance.getDefault().getTable(table), field); } - - @Override - public boolean get() { - return m_entry.getInstance().isConnected() && m_entry.getBoolean(false); - } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java index 626ac9b5e3..8b39f47f0d 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java @@ -14,10 +14,6 @@ import edu.wpi.first.wpilibj.GenericHID; *

This class is provided by the NewCommands VendorDep */ public class POVButton extends Button { - private final GenericHID m_joystick; - private final int m_angle; - private final int m_povNumber; - /** * Creates a POV button for triggering commands. * @@ -26,11 +22,8 @@ public class POVButton extends Button { * @param povNumber The POV number (see {@link GenericHID#getPOV(int)}) */ public POVButton(GenericHID joystick, int angle, int povNumber) { + super(() -> joystick.getPOV(povNumber) == angle); requireNonNullParam(joystick, "joystick", "POVButton"); - - m_joystick = joystick; - m_angle = angle; - m_povNumber = povNumber; } /** @@ -42,14 +35,4 @@ public class POVButton extends Button { public POVButton(GenericHID joystick, int angle) { this(joystick, angle, 0); } - - /** - * Checks whether the current value of the POV is the target angle. - * - * @return Whether the value of the POV matches the target angle - */ - @Override - public boolean get() { - return m_joystick.getPOV(m_povNumber) == m_angle; - } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java index 50abf962f0..e1136f96f5 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java @@ -7,43 +7,58 @@ package edu.wpi.first.wpilibj2.command.button; import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.wpilibj.event.BooleanEvent; +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.InstantCommand; import edu.wpi.first.wpilibj2.command.Subsystem; +import java.util.function.BiFunction; import java.util.function.BooleanSupplier; /** - * This class provides an easy way to link commands to inputs. - * - *

It is very easy to link a button to a command. For instance, you could link the trigger button - * of a joystick to a "score" command. - * - *

It is encouraged that teams write a subclass of Trigger if they want to have something unusual - * (for instance, if they want to react to the user holding a button while the robot is reading a - * certain sensor input). For this, they only have to write the {@link Trigger#get()} method to get - * the full functionality of the Trigger class. + * This class is a wrapper around {@link BooleanEvent}, providing an easy way to link commands to + * digital inputs. * *

This class is provided by the NewCommands VendorDep */ -public class Trigger implements BooleanSupplier { - private final BooleanSupplier m_isActive; - +public class Trigger extends BooleanEvent { /** - * Creates a new trigger with the given condition determining whether it is active. + * Creates a new trigger with the given condition/digital signal. * - * @param isActive returns whether or not the trigger should be active + * @param loop the loop that polls this trigger + * @param signal the digital signal represented. */ - public Trigger(BooleanSupplier isActive) { - m_isActive = isActive; + public Trigger(EventLoop loop, BooleanSupplier signal) { + super(loop, signal); } /** - * Creates a new trigger that is always inactive. Useful only as a no-arg constructor for - * subclasses that will be overriding {@link Trigger#get()} anyway. + * Copies the BooleanEvent into a Trigger object. + * + * @param toCast the BooleanEvent + * @return a Trigger wrapping the given BooleanEvent + * @see BooleanEvent#castTo(BiFunction) */ + public static Trigger cast(BooleanEvent toCast) { + return toCast.castTo(Trigger::new); + } + + /** + * Creates a new trigger with the given condition/digital signal. + * + *

Polled by the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}. + * + * @param signal the digital signal represented. + */ + public Trigger(BooleanSupplier signal) { + this(CommandScheduler.getInstance().getDefaultButtonLoop(), signal); + } + + /** Creates a new trigger that is always inactive. */ + @Deprecated public Trigger() { - m_isActive = () -> false; + this(() -> false); } /** @@ -54,23 +69,11 @@ public class Trigger implements BooleanSupplier { *

Functionally identical to {@link Trigger#getAsBoolean()}. * * @return whether or not the trigger condition is active. + * @deprecated use {@link #getAsBoolean()} */ - public boolean get() { - return m_isActive.getAsBoolean(); - } - - /** - * Returns whether or not the trigger is active. - * - *

This method will be called repeatedly a command is linked to the Trigger. - * - *

Functionally identical to {@link Trigger#get()}. - * - * @return whether or not the trigger condition is active. - */ - @Override - public final boolean getAsBoolean() { - return this.get(); + @Deprecated + public final boolean get() { + return getAsBoolean(); } /** @@ -83,23 +86,7 @@ public class Trigger implements BooleanSupplier { public Trigger whenActive(final Command command, boolean interruptible) { requireNonNullParam(command, "command", "whenActive"); - CommandScheduler.getInstance() - .addButton( - new Runnable() { - private boolean m_pressedLast = get(); - - @Override - public void run() { - boolean pressed = get(); - - if (!m_pressedLast && pressed) { - command.schedule(interruptible); - } - - m_pressedLast = pressed; - } - }); - + this.rising().ifHigh(() -> command.schedule(interruptible)); return this; } @@ -138,24 +125,9 @@ public class Trigger implements BooleanSupplier { public Trigger whileActiveContinuous(final Command command, boolean interruptible) { requireNonNullParam(command, "command", "whileActiveContinuous"); - CommandScheduler.getInstance() - .addButton( - new Runnable() { - private boolean m_pressedLast = get(); + this.ifHigh(() -> command.schedule(interruptible)); + this.falling().ifHigh(command::cancel); - @Override - public void run() { - boolean pressed = get(); - - if (pressed) { - command.schedule(interruptible); - } else if (m_pressedLast) { - command.cancel(); - } - - m_pressedLast = pressed; - } - }); return this; } @@ -163,7 +135,7 @@ public class Trigger implements BooleanSupplier { * Constantly starts the given command while the button is held. * *

{@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and - * will be canceled when the trigger becomes inactive. The command is set to be interruptible. + * will be canceled when the trigger becomes inactive. * * @param command the command to start * @return this trigger, so calls can be chained @@ -194,24 +166,9 @@ public class Trigger implements BooleanSupplier { public Trigger whileActiveOnce(final Command command, boolean interruptible) { requireNonNullParam(command, "command", "whileActiveOnce"); - CommandScheduler.getInstance() - .addButton( - new Runnable() { - private boolean m_pressedLast = get(); + this.rising().ifHigh(() -> command.schedule(interruptible)); + this.falling().ifHigh(command::cancel); - @Override - public void run() { - boolean pressed = get(); - - if (!m_pressedLast && pressed) { - command.schedule(interruptible); - } else if (m_pressedLast && !pressed) { - command.cancel(); - } - - m_pressedLast = pressed; - } - }); return this; } @@ -236,22 +193,8 @@ public class Trigger implements BooleanSupplier { public Trigger whenInactive(final Command command, boolean interruptible) { requireNonNullParam(command, "command", "whenInactive"); - CommandScheduler.getInstance() - .addButton( - new Runnable() { - private boolean m_pressedLast = get(); + this.falling().ifHigh(() -> command.schedule(interruptible)); - @Override - public void run() { - boolean pressed = get(); - - if (m_pressedLast && !pressed) { - command.schedule(interruptible); - } - - m_pressedLast = pressed; - } - }); return this; } @@ -286,26 +229,16 @@ public class Trigger implements BooleanSupplier { public Trigger toggleWhenActive(final Command command, boolean interruptible) { requireNonNullParam(command, "command", "toggleWhenActive"); - CommandScheduler.getInstance() - .addButton( - new Runnable() { - private boolean m_pressedLast = get(); - - @Override - public void run() { - boolean pressed = get(); - - if (!m_pressedLast && pressed) { - if (command.isScheduled()) { - command.cancel(); - } else { - command.schedule(interruptible); - } - } - - m_pressedLast = pressed; + this.rising() + .ifHigh( + () -> { + if (command.isScheduled()) { + command.cancel(); + } else { + command.schedule(interruptible); } }); + return this; } @@ -328,85 +261,45 @@ public class Trigger implements BooleanSupplier { public Trigger cancelWhenActive(final Command command) { requireNonNullParam(command, "command", "cancelWhenActive"); - CommandScheduler.getInstance() - .addButton( - new Runnable() { - private boolean m_pressedLast = get(); + this.rising().ifHigh(command::cancel); - @Override - public void run() { - boolean pressed = get(); - - if (!m_pressedLast && pressed) { - command.cancel(); - } - - m_pressedLast = pressed; - } - }); return this; } - /** - * Composes this trigger with a boolean supplier, returning a new trigger that is active when both - * triggers are active. - * - * @param booleanSupplier the boolean supplier to compose with - * @return the trigger that is active when both triggers are active - */ - public Trigger and(BooleanSupplier booleanSupplier) { - return new Trigger(() -> get() && booleanSupplier.getAsBoolean()); + /* ----------- Super method type redeclarations ----------------- */ + + @Override + public Trigger and(BooleanSupplier trigger) { + return cast(super.and(trigger)); } - /** - * Composes this trigger with a boolean supplier, returning a new trigger that is active when - * either trigger is active. - * - * @param booleanSupplier the boolean supplier to compose with - * @return the trigger that is active when either trigger is active - */ - public Trigger or(BooleanSupplier booleanSupplier) { - return new Trigger(() -> get() || booleanSupplier.getAsBoolean()); + @Override + public Trigger or(BooleanSupplier trigger) { + return cast(super.or(trigger)); } - /** - * Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the - * negation of this trigger. - * - * @return the negated trigger - */ + @Override public Trigger negate() { - return new Trigger(() -> !get()); + return cast(super.negate()); } - /** - * Creates a new debounced trigger from this trigger - it will become active when this trigger has - * been active for longer than the specified period. - * - * @param seconds The debounce period. - * @return The debounced trigger (rising edges debounced only) - */ + @Override public Trigger debounce(double seconds) { return debounce(seconds, Debouncer.DebounceType.kRising); } - /** - * Creates a new debounced trigger from this trigger - it will become active when this trigger has - * been active for longer than the specified period. - * - * @param seconds The debounce period. - * @param type The debounce type. - * @return The debounced trigger. - */ + @Override public Trigger debounce(double seconds, Debouncer.DebounceType type) { - return new Trigger( - new BooleanSupplier() { - Debouncer m_debouncer = new Debouncer(seconds, type); + return cast(super.debounce(seconds, type)); + } - @Override - public boolean getAsBoolean() { - return m_debouncer.calculate(get()); - } - }); + @Override + public Trigger rising() { + return cast(super.rising()); + } + + @Override + public Trigger falling() { + return cast(super.falling()); } } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index f79fa59b73..1a1bd699fd 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/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> subsystems; + frc::EventLoop defaultButtonLoop; // The set of currently-registered buttons that will be polled every // iteration. - wpi::SmallVector, 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 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; diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp index e370097d72..8db84fe5bf 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp +++ b/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 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 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()); - }); -} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h index e0abd0f4a4..d6458bb3c4 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h @@ -10,9 +10,11 @@ #include #include +#include #include #include #include +#include #include #include @@ -52,16 +54,32 @@ class CommandScheduler final : public nt::NTSendable, void SetPeriod(units::second_t period); /** - * Adds a button binding to the scheduler, which will be polled to schedule - * commands. + * Get the active button poll. * - * @param button The button to add + * @return a reference to the current {@link frc::EventLoop} object polling + * buttons. */ - void AddButton(wpi::unique_function button); + frc::EventLoop* GetActiveButtonLoop() const; + + /** + * Replace the button poll with another one. + * + * @param loop the new button polling loop object. + */ + void SetActiveButtonLoop(frc::EventLoop* loop); + + /** + * Get the default button poll. + * + * @return a reference to the default {@link frc::EventLoop} object polling + * buttons. + */ + frc::EventLoop* GetDefaultButtonLoop() const; /** * Removes all button bindings from the scheduler. */ + WPI_DEPRECATED("Call Clear on the EventLoop instance directly!") void ClearButtons(); /** diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h index cfaeefe0c1..409e34eb72 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h @@ -9,6 +9,8 @@ #include #include +#include +#include #include #include #include @@ -19,32 +21,41 @@ namespace frc2 { class Command; /** - * A class used to bind command scheduling to events. The - * Trigger class is a base for all command-event-binding classes, and so the - * methods are named fairly abstractly; for purpose-specific wrappers, see - * Button. + * This class is a command-based wrapper around {@link BooleanEvent}, providing + * an easy way to link commands to inputs. * * This class is provided by the NewCommands VendorDep * * @see Button */ -class Trigger { +class Trigger : public frc::BooleanEvent { public: + /** + * Creates a new trigger with the given condition determining whether it is + * active. + * + *

Polled by the default scheduler button loop. + * + * @param isActive returns whether or not the trigger should be active + */ + explicit Trigger(std::function isActive) + : BooleanEvent{CommandScheduler::GetInstance().GetDefaultButtonLoop(), + std::move(isActive)} {} + /** * Create a new trigger that is active when the given condition is true. * + * @param loop The loop instance that polls this trigger. * @param isActive Whether the trigger is active. */ - Trigger(std::function isActive) // NOLINT - : m_isActive{std::move(isActive)} {} + Trigger(frc::EventLoop* loop, std::function isActive) + : BooleanEvent{loop, std::move(isActive)} {} /** * Create a new trigger that is never active (default constructor) - activity * can be further determined by subclass code. */ - Trigger() { - m_isActive = [] { return false; }; - } + Trigger() : Trigger([] { return false; }) {} Trigger(const Trigger& other); @@ -72,19 +83,10 @@ class Trigger { template >>> Trigger WhenActive(T&& command, bool interruptible = true) { - CommandScheduler::GetInstance().AddButton( - [pressedLast = m_isActive(), *this, - command = std::make_unique>( + this->Rising().IfHigh( + [command = std::make_unique>( std::forward(command)), - interruptible]() mutable { - bool pressed = m_isActive(); - - if (!pressedLast && pressed) { - command->Schedule(interruptible); - } - - pressedLast = pressed; - }); + interruptible] { command->Schedule(interruptible); }); return *this; } @@ -131,21 +133,11 @@ class Trigger { template >>> Trigger WhileActiveContinous(T&& command, bool interruptible = true) { - CommandScheduler::GetInstance().AddButton( - [pressedLast = m_isActive(), *this, - command = std::make_unique>( - std::forward(command)), - interruptible]() mutable { - bool pressed = m_isActive(); + std::shared_ptr ptr = + std::make_shared>(std::forward(command)); + this->IfHigh([ptr, interruptible] { ptr->Schedule(interruptible); }); + this->Falling().IfHigh([ptr] { ptr->Cancel(); }); - if (pressed) { - command->Schedule(interruptible); - } else if (pressedLast && !pressed) { - command->Cancel(); - } - - pressedLast = pressed; - }); return *this; } @@ -191,21 +183,13 @@ class Trigger { template >>> Trigger WhileActiveOnce(T&& command, bool interruptible = true) { - CommandScheduler::GetInstance().AddButton( - [pressedLast = m_isActive(), *this, - command = std::make_unique>( - std::forward(command)), - interruptible]() mutable { - bool pressed = m_isActive(); + std::shared_ptr ptr = + std::make_shared>(std::forward(command)); - if (!pressedLast && pressed) { - command->Schedule(interruptible); - } else if (pressedLast && !pressed) { - command->Cancel(); - } + this->Rising().IfHigh( + [ptr, interruptible] { ptr->Schedule(interruptible); }); + this->Falling().IfHigh([ptr] { ptr->Cancel(); }); - pressedLast = pressed; - }); return *this; } @@ -233,19 +217,11 @@ class Trigger { template >>> Trigger WhenInactive(T&& command, bool interruptible = true) { - CommandScheduler::GetInstance().AddButton( - [pressedLast = m_isActive(), *this, - command = std::make_unique>( + this->Falling().IfHigh( + [command = std::make_unique>( std::forward(command)), - interruptible]() mutable { - bool pressed = m_isActive(); + interruptible] { command->Schedule(interruptible); }); - if (pressedLast && !pressed) { - command->Schedule(interruptible); - } - - pressedLast = pressed; - }); return *this; } @@ -291,23 +267,17 @@ class Trigger { template >>> Trigger ToggleWhenActive(T&& command, bool interruptible = true) { - CommandScheduler::GetInstance().AddButton( - [pressedLast = m_isActive(), *this, - command = std::make_unique>( + this->Rising().IfHigh( + [command = std::make_unique>( std::forward(command)), - interruptible]() mutable { - bool pressed = m_isActive(); - - if (!pressedLast && pressed) { - if (command->IsScheduled()) { - command->Cancel(); - } else { - command->Schedule(interruptible); - } + interruptible] { + if (!command->IsScheduled()) { + command->Schedule(interruptible); + } else { + command->Cancel(); } - - pressedLast = pressed; }); + return *this; } @@ -321,13 +291,27 @@ class Trigger { */ Trigger CancelWhenActive(Command* command); + /** + * Get a new event that events only when this one newly changes to true. + * + * @return a new event representing when this one newly changes to true. + */ + Trigger Rising() { return BooleanEvent::Rising().CastTo(); } + + /** + * Get a new event that triggers only when this one newly changes to false. + * + * @return a new event representing when this one newly changes to false. + */ + Trigger Falling() { return BooleanEvent::Falling().CastTo(); } + /** * Composes two triggers with logical AND. * * @return A trigger which is active when both component triggers are active. */ - Trigger operator&&(Trigger rhs) { - return Trigger([*this, rhs] { return m_isActive() && rhs.m_isActive(); }); + Trigger operator&&(std::function rhs) { + return BooleanEvent::operator&&(rhs).CastTo(); } /** @@ -335,8 +319,8 @@ class Trigger { * * @return A trigger which is active when either component trigger is active. */ - Trigger operator||(Trigger rhs) { - return Trigger([*this, rhs] { return m_isActive() || rhs.m_isActive(); }); + Trigger operator||(std::function rhs) { + return BooleanEvent::operator||(rhs).CastTo(); } /** @@ -345,9 +329,7 @@ class Trigger { * @return A trigger which is active when the component trigger is inactive, * and vice-versa. */ - Trigger operator!() { - return Trigger([*this] { return !m_isActive(); }); - } + Trigger operator!() { return BooleanEvent::operator!().CastTo(); } /** * Creates a new debounced trigger from this trigger - it will become active @@ -359,9 +341,8 @@ class Trigger { */ Trigger Debounce(units::second_t debounceTime, frc::Debouncer::DebounceType type = - frc::Debouncer::DebounceType::kRising); - - private: - std::function m_isActive; + frc::Debouncer::DebounceType::kRising) { + return BooleanEvent::Debounce(debounceTime, type).CastTo(); + } }; } // namespace frc2 diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java index 5dbcaf348f..1f308c2ed1 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java @@ -20,7 +20,7 @@ public class CommandTestBase { void commandSetup() { CommandScheduler.getInstance().cancelAll(); CommandScheduler.getInstance().enable(); - CommandScheduler.getInstance().clearButtons(); + CommandScheduler.getInstance().getActiveButtonLoop().clear(); CommandGroupBase.clearGroupedCommands(); setDSEnabled(true); diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java index b778de6013..2b13910fed 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java @@ -170,10 +170,10 @@ class ButtonTest extends CommandTestBase { button1.setPressed(true); button2.setPressed(false); - assertFalse(button1.and(button2).get()); - assertTrue(button1.or(button2).get()); - assertFalse(button1.negate().get()); - assertTrue(button1.and(button2.negate()).get()); + assertFalse(button1.and(button2).getAsBoolean()); + assertTrue(button1.or(button2).getAsBoolean()); + assertFalse(button1.negate().getAsBoolean()); + assertTrue(button1.and(button2.negate()).getAsBoolean()); } @Test @@ -183,8 +183,8 @@ class ButtonTest extends CommandTestBase { button1.setPressed(true); - assertFalse(button1.and(booleanSupplier).get()); - assertTrue(button1.or(booleanSupplier).get()); + assertFalse(button1.and(booleanSupplier).getAsBoolean()); + assertTrue(button1.or(booleanSupplier).getAsBoolean()); } @Test diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp index 7ebbb70005..4dccc0ca6d 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp @@ -10,7 +10,7 @@ CommandTestBase::CommandTestBase() { auto& scheduler = CommandScheduler::GetInstance(); scheduler.CancelAll(); scheduler.Enable(); - scheduler.ClearButtons(); + scheduler.GetActiveButtonLoop()->Clear(); } CommandScheduler CommandTestBase::GetScheduler() { @@ -22,7 +22,7 @@ void CommandTestBase::SetUp() { } void CommandTestBase::TearDown() { - CommandScheduler::GetInstance().ClearButtons(); + CommandScheduler::GetInstance().GetActiveButtonLoop()->Clear(); } void CommandTestBase::SetDSEnabled(bool enabled) { diff --git a/wpilibc/src/main/native/cpp/event/BooleanEvent.cpp b/wpilibc/src/main/native/cpp/event/BooleanEvent.cpp new file mode 100644 index 0000000000..28ab0fca14 --- /dev/null +++ b/wpilibc/src/main/native/cpp/event/BooleanEvent.cpp @@ -0,0 +1,64 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/event/BooleanEvent.h" + +using namespace frc; + +BooleanEvent::BooleanEvent(EventLoop* loop, std::function condition) + : m_loop(loop), m_condition(std::move(condition)) {} + +BooleanEvent::operator std::function() { + return m_condition; +} + +bool BooleanEvent::GetAsBoolean() const { + return m_condition(); +} + +void BooleanEvent::IfHigh(wpi::unique_function action) { + m_loop->Bind(m_condition, std::move(action)); +} + +BooleanEvent BooleanEvent::operator!() { + return BooleanEvent(this->m_loop, [lhs = m_condition] { return !lhs(); }); +} + +BooleanEvent BooleanEvent::operator&&(std::function rhs) { + return BooleanEvent(this->m_loop, + [lhs = m_condition, rhs] { return lhs() && rhs(); }); +} + +BooleanEvent BooleanEvent::operator||(std::function rhs) { + return BooleanEvent(this->m_loop, + [lhs = m_condition, rhs] { return lhs() || rhs(); }); +} + +BooleanEvent BooleanEvent::Rising() { + return BooleanEvent( + this->m_loop, [lhs = m_condition, m_previous = m_condition()]() mutable { + bool present = lhs(); + bool past = m_previous; + m_previous = present; + return !past && present; + }); +} + +BooleanEvent BooleanEvent::Falling() { + return BooleanEvent( + this->m_loop, [lhs = m_condition, m_previous = m_condition()]() mutable { + bool present = lhs(); + bool past = m_previous; + m_previous = present; + return past && !present; + }); +} + +BooleanEvent BooleanEvent::Debounce(units::second_t debounceTime, + frc::Debouncer::DebounceType type) { + return BooleanEvent( + this->m_loop, + [debouncer = frc::Debouncer(debounceTime, type), + lhs = m_condition]() mutable { return debouncer.Calculate(lhs()); }); +} diff --git a/wpilibc/src/main/native/cpp/event/EventLoop.cpp b/wpilibc/src/main/native/cpp/event/EventLoop.cpp new file mode 100644 index 0000000000..71cff7066c --- /dev/null +++ b/wpilibc/src/main/native/cpp/event/EventLoop.cpp @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/event/EventLoop.h" + +using namespace frc; + +EventLoop::EventLoop() {} + +void EventLoop::Binding::Poll() { + if (condition()) { + action(); + } +} + +void EventLoop::Bind(std::function condition, + wpi::unique_function action) { + m_bindings.emplace_back(Binding{condition, std::move(action)}); +} + +void EventLoop::Poll() { + for (Binding& binding : m_bindings) { + binding.Poll(); + } +} + +void EventLoop::Clear() { + m_bindings.clear(); +} diff --git a/wpilibc/src/main/native/include/frc/event/BooleanEvent.h b/wpilibc/src/main/native/include/frc/event/BooleanEvent.h new file mode 100644 index 0000000000..513d57842b --- /dev/null +++ b/wpilibc/src/main/native/include/frc/event/BooleanEvent.h @@ -0,0 +1,136 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include + +#include +#include + +#include "EventLoop.h" + +namespace frc { + +/** + * This class provides an easy way to link actions to inputs. Each object + * represents a boolean condition to which callback actions can be bound using + * {@link #IfHigh(wpi::unique_function)}. + * + *

These events can easily be composed using factories such as {@link + * #operator!}, + * {@link #operator||}, {@link #operator&&} etc. + * + *

To get an event that activates only when this one changes, see {@link + * #Falling()} and {@link #Rising()}. + */ +class BooleanEvent { + public: + /** + * Creates a new event with the given condition determining whether it is + * active. + * + * @param loop the loop that polls this event + * @param condition returns whether or not the event should be active + */ + BooleanEvent(EventLoop* loop, std::function condition); + + /** + * Check whether this event is active or not. + * + * @return true if active. + */ + bool GetAsBoolean() const; + + /** + * Bind an action to this event. + * + * @param action the action to run if this event is active. + */ + void IfHigh(wpi::unique_function action); + + operator std::function(); // NOLINT + + /** + * A method to "downcast" a BooleanEvent instance to a subclass (for example, + * to a command-based version of this class). + * + * @param ctor a method reference to the constructor of the subclass that + * accepts the loop as the first parameter and the condition/signal as the + * second. + * @return an instance of the subclass. + */ + template >> + T CastTo(std::function)> ctor = + [](EventLoop* loop, std::function condition) { + return T(loop, condition); + }) { + return ctor(m_loop, m_condition); + } + + /** + * Creates a new event that is active when this event is inactive, i.e. that + * acts as the negation of this event. + * + * @return the negated event + */ + BooleanEvent operator!(); + + /** + * Composes this event with another event, returning a new event that is + * active when both events are active. + * + *

The new event will use this event's polling loop. + * + * @param rhs the event to compose with + * @return the event that is active when both events are active + */ + BooleanEvent operator&&(std::function rhs); + + /** + * Composes this event with another event, returning a new event that is + * active when either event is active. + * + *

The new event will use this event's polling loop. + * + * @param rhs the event to compose with + * @return the event that is active when either event is active + */ + BooleanEvent operator||(std::function rhs); + + /** + * Get a new event that events only when this one newly changes to true. + * + * @return a new event representing when this one newly changes to true. + */ + BooleanEvent Rising(); + + /** + * Get a new event that triggers only when this one newly changes to false. + * + * @return a new event representing when this one newly changes to false. + */ + BooleanEvent Falling(); + + /** + * Creates a new debounced event from this event - it will become active when + * this event has been active for longer than the specified period. + * + * @param debounceTime The debounce period. + * @param type The debounce type. + * @return The debounced event. + */ + BooleanEvent Debounce(units::second_t debounceTime, + frc::Debouncer::DebounceType type = + frc::Debouncer::DebounceType::kRising); + + private: + EventLoop* m_loop; + std::function m_condition; +}; +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/event/EventLoop.h b/wpilibc/src/main/native/include/frc/event/EventLoop.h new file mode 100644 index 0000000000..f2b82cb3e2 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/event/EventLoop.h @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include +#include + +namespace frc { +/** The loop polling BooleanEvent objects and executing the actions bound to + * them. */ +class EventLoop { + public: + EventLoop(); + + /** + * Bind a new action to run whenever the condition is true. + * + * @param condition the condition to listen to. + * @param action the action to run. + */ + void Bind(std::function condition, + wpi::unique_function action); + + /** + * Poll all bindings. + */ + void Poll(); + + /** + * Clear all bindings. + */ + void Clear(); + + private: + struct Binding { + std::function condition; + wpi::unique_function action; + + void Poll(); + }; + std::list m_bindings; +}; +} // namespace frc diff --git a/wpilibcExamples/src/main/cpp/examples/EventLoop/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/EventLoop/cpp/Robot.cpp new file mode 100644 index 0000000000..15a1be02f3 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/EventLoop/cpp/Robot.cpp @@ -0,0 +1,102 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static const auto SHOT_VELOCITY = 200_rpm; +static const auto TOLERANCE = 8_rpm; +static const auto KICKER_THRESHOLD = 15_mm; + +class Robot : public frc::TimedRobot { + public: + void RobotInit() override { + m_controller.SetTolerance(TOLERANCE.value()); + + frc::BooleanEvent isBallAtKicker{&m_loop, [&kickerSensor = m_kickerSensor] { + return kickerSensor.GetRange() < + KICKER_THRESHOLD; + }}; + frc::BooleanEvent intakeButton{ + &m_loop, [&joystick = m_joystick] { return joystick.GetRawButton(2); }}; + + // if the thumb button is held + (intakeButton + // and there is not a ball at the kicker + && !isBallAtKicker) + // activate the intake + .IfHigh([&intake = m_intake] { intake.Set(0.5); }); + + // if the thumb button is not held + (intakeButton + // or there is a ball in the kicker + || isBallAtKicker) + // stop the intake + .IfHigh([&intake = m_intake] { intake.Set(0.0); }); + + frc::BooleanEvent shootTrigger{ + &m_loop, [&joystick = m_joystick] { return joystick.GetTrigger(); }}; + + // if the trigger is held + shootTrigger + // accelerate the shooter wheel + .IfHigh([&shooter = m_shooter, &controller = m_controller, &ff = m_ff, + &encoder = m_shooterEncoder] { + shooter.SetVoltage(units::volt_t(controller.Calculate( + encoder.GetRate(), SHOT_VELOCITY.value())) + + ff.Calculate(SHOT_VELOCITY)); + }); + // if not, stop + shootTrigger.IfHigh([&shooter = m_shooter] { shooter.Set(0.0); }); + + frc::BooleanEvent atTargetVelocity = + frc::BooleanEvent( + &m_loop, + [&controller = m_controller] { return controller.AtSetpoint(); }) + // debounce for more stability + .Debounce(0.2_s); + + // if we're at the target velocity, kick the ball into the shooter wheel + atTargetVelocity.IfHigh([&kicker = m_kicker] { kicker.Set(0.7); }); + + // when we stop being at the target velocity, it means the ball was shot + atTargetVelocity + .Falling() + // so stop the kicker + .IfHigh([&kicker = m_kicker] { kicker.Set(0.0); }); + } + + void RobotPeriodic() override { m_loop.Poll(); } + + private: + frc::PWMSparkMax m_shooter{0}; + frc::Encoder m_shooterEncoder{0, 1}; + frc::PIDController m_controller{0.3, 0, 0}; + frc::SimpleMotorFeedforward m_ff{0.1_V, 0.065_V / 1_rpm}; + + frc::PWMSparkMax m_kicker{1}; + frc::Ultrasonic m_kickerSensor{2, 3}; + + frc::PWMSparkMax m_intake{3}; + + frc::EventLoop m_loop{}; + frc::Joystick m_joystick{0}; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 9eb5b6b1be..9fd88a178a 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -84,6 +84,16 @@ "gradlebase": "cpp", "commandversion": 2 }, + { + "name": "EventLoop", + "description": "Demonstrate managing a ball system using EventLoop and BooleanEvent.", + "tags": [ + "EventLoop" + ], + "foldername": "EventLoop", + "gradlebase": "cpp", + "commandversion": 2 + }, { "name": "Arcade Drive", "description": "An example program which demonstrates the use of Arcade Drive with the DifferentialDrive class", diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java new file mode 100644 index 0000000000..33d76aeaf3 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java @@ -0,0 +1,184 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.event; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import edu.wpi.first.math.filter.Debouncer; +import java.util.function.BiFunction; +import java.util.function.BooleanSupplier; + +/** + * This class provides an easy way to link actions to high-active logic signals. Each object + * represents a digital signal to which callback actions can be bound using {@link + * #ifHigh(Runnable)}. + * + *

These signals can easily be composed for advanced functionality using {@link + * #and(BooleanSupplier)}, {@link #or(BooleanSupplier)}, {@link #negate()} etc. + * + *

To get an event that activates only when this one changes, see {@link #falling()} and {@link + * #rising()}. + */ +public class BooleanEvent implements BooleanSupplier { + /** Poller loop. */ + protected final EventLoop m_loop; + /** Condition. */ + private final BooleanSupplier m_signal; + + /** + * Creates a new event with the given signal determining whether it is active. + * + * @param loop the loop that polls this event + * @param signal the digital signal represented by this object. + */ + public BooleanEvent(EventLoop loop, BooleanSupplier signal) { + m_loop = requireNonNullParam(loop, "loop", "BooleanEvent"); + m_signal = requireNonNullParam(signal, "signal", "BooleanEvent"); + } + + /** + * Check the state of this signal (high or low). + * + * @return true for the high state, false for the low state. + */ + @Override + public final boolean getAsBoolean() { + return m_signal.getAsBoolean(); + } + + /** + * Bind an action to this event. + * + * @param action the action to run if this event is active. + */ + public final void ifHigh(Runnable action) { + m_loop.bind(m_signal, action); + } + + /** + * Get a new event that events only when this one newly changes to true. + * + * @return a new event representing when this one newly changes to true. + */ + public BooleanEvent rising() { + return new BooleanEvent( + m_loop, + new BooleanSupplier() { + private boolean m_previous = m_signal.getAsBoolean(); + + @Override + public boolean getAsBoolean() { + boolean present = m_signal.getAsBoolean(); + boolean ret = !m_previous && present; + m_previous = present; + return ret; + } + }); + } + + /** + * Get a new event that triggers only when this one newly changes to false. + * + * @return a new event representing when this one newly changes to false. + */ + public BooleanEvent falling() { + return new BooleanEvent( + m_loop, + new BooleanSupplier() { + private boolean m_previous = m_signal.getAsBoolean(); + + @Override + public boolean getAsBoolean() { + boolean present = m_signal.getAsBoolean(); + boolean ret = m_previous && !present; + m_previous = present; + return ret; + } + }); + } + + /** + * Creates a new debounced event from this event - it will become active when this event has been + * active for longer than the specified period. + * + * @param seconds The debounce period. + * @return The debounced event (rising edges debounced only) + */ + public BooleanEvent debounce(double seconds) { + return debounce(seconds, Debouncer.DebounceType.kRising); + } + + /** + * Creates a new debounced event from this event - it will become active when this event has been + * active for longer than the specified period. + * + * @param seconds The debounce period. + * @param type The debounce type. + * @return The debounced event. + */ + public BooleanEvent debounce(double seconds, Debouncer.DebounceType type) { + return new BooleanEvent( + m_loop, + new BooleanSupplier() { + private final Debouncer m_debouncer = new Debouncer(seconds, type); + + @Override + public boolean getAsBoolean() { + return m_debouncer.calculate(m_signal.getAsBoolean()); + } + }); + } + + /** + * Creates a new event that is active when this event is inactive, i.e. that acts as the negation + * of this event. + * + * @return the negated event + */ + public BooleanEvent negate() { + return new BooleanEvent(m_loop, () -> !m_signal.getAsBoolean()); + } + + /** + * Composes this event with another event, returning a new signal that is in the high state when + * both signals are in the high state. + * + *

The new event will use this event's polling loop. + * + * @param other the event to compose with + * @return the event that is active when both events are active + */ + public BooleanEvent and(BooleanSupplier other) { + requireNonNullParam(other, "other", "and"); + return new BooleanEvent(m_loop, () -> m_signal.getAsBoolean() && other.getAsBoolean()); + } + + /** + * Composes this event with another event, returning a new signal that is high when either signal + * is high. + * + *

The new event will use this event's polling loop. + * + * @param other the event to compose with + * @return a signal that is high when either signal is high. + */ + public BooleanEvent or(BooleanSupplier other) { + requireNonNullParam(other, "other", "or"); + return new BooleanEvent(m_loop, () -> m_signal.getAsBoolean() || other.getAsBoolean()); + } + + /** + * A method to "downcast" a BooleanEvent instance to a subclass (for example, to a command-based + * version of this class). + * + * @param ctor a method reference to the constructor of the subclass that accepts the loop as the + * first parameter and the condition/signal as the second. + * @param the subclass type + * @return an instance of the subclass. + */ + public T castTo(BiFunction ctor) { + return ctor.apply(m_loop, m_signal); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/EventLoop.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/EventLoop.java new file mode 100644 index 0000000000..88eed86044 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/EventLoop.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.event; + +import java.util.Collection; +import java.util.LinkedHashSet; +import java.util.function.BooleanSupplier; + +/** The loop polling {@link BooleanEvent} objects and executing the actions bound to them. */ +public final class EventLoop { + private final Collection m_bindings = new LinkedHashSet<>(); + + /** + * Bind a new action to run whenever the condition is true. + * + * @param condition the condition to listen to. + * @param action the action to run. + */ + public void bind(BooleanSupplier condition, Runnable action) { + m_bindings.add(new Binding(condition, action)); + } + + /** Poll all bindings. */ + public void poll() { + m_bindings.forEach(Binding::poll); + } + + /** Clear all bindings. */ + public void clear() { + m_bindings.clear(); + } + + private static class Binding { + private final BooleanSupplier m_condition; + private final Runnable m_action; + + private Binding(BooleanSupplier condition, Runnable action) { + this.m_condition = condition; + this.m_action = action; + } + + void poll() { + if (m_condition.getAsBoolean()) { + m_action.run(); + } + } + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/BooleanEventTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/BooleanEventTest.java new file mode 100644 index 0000000000..59caf23e5b --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/BooleanEventTest.java @@ -0,0 +1,168 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.event; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicInteger; +import org.junit.jupiter.api.Test; + +class BooleanEventTest { + @Test + void testBinaryCompositions() { + var loop = new EventLoop(); + + var andCounter = new AtomicInteger(0); + var orCounter = new AtomicInteger(0); + + assertEquals(0, andCounter.get()); + assertEquals(0, orCounter.get()); + + new BooleanEvent(loop, () -> true).and(() -> false).ifHigh(andCounter::incrementAndGet); + new BooleanEvent(loop, () -> true).or(() -> false).ifHigh(orCounter::incrementAndGet); + + loop.poll(); + + assertEquals(0, andCounter.get()); + assertEquals(1, orCounter.get()); + } + + @Test + void testBinaryCompositionLoopSemantics() { + var loop1 = new EventLoop(); + var loop2 = new EventLoop(); + + var counter1 = new AtomicInteger(0); + var counter2 = new AtomicInteger(0); + + new BooleanEvent(loop1, () -> true) + .and(new BooleanEvent(loop2, () -> true)) + .ifHigh(counter1::incrementAndGet); + + new BooleanEvent(loop2, () -> true) + .and(new BooleanEvent(loop1, () -> true)) + .ifHigh(counter2::incrementAndGet); + + assertEquals(0, counter1.get()); + assertEquals(0, counter2.get()); + + loop1.poll(); + + assertEquals(1, counter1.get()); + assertEquals(0, counter2.get()); + + loop2.poll(); + + assertEquals(1, counter1.get()); + assertEquals(1, counter2.get()); + } + + @Test + void testEdgeDecorators() { + var bool = new AtomicBoolean(false); + var counter = new AtomicInteger(0); + + var loop = new EventLoop(); + + new BooleanEvent(loop, bool::get).falling().ifHigh(counter::decrementAndGet); + new BooleanEvent(loop, bool::get).rising().ifHigh(counter::incrementAndGet); + + assertEquals(0, counter.get()); + + bool.set(false); + loop.poll(); + + assertEquals(0, counter.get()); + + bool.set(true); + loop.poll(); + + assertEquals(1, counter.get()); + + bool.set(true); + loop.poll(); + + assertEquals(1, counter.get()); + + bool.set(false); + loop.poll(); + + assertEquals(0, counter.get()); + } + + @Test + void testEdgeReuse() { + var loop = new EventLoop(); + var bool = new AtomicBoolean(false); + var counter = new AtomicInteger(0); + + var event = new BooleanEvent(loop, bool::get).rising(); + event.ifHigh(counter::incrementAndGet); + event.ifHigh(counter::incrementAndGet); + + assertEquals(0, counter.get()); + + loop.poll(); + + assertEquals(0, counter.get()); + + bool.set(true); + loop.poll(); + + assertEquals(1, counter.get()); // FIXME?: natural sense dictates counter == 2!! + + loop.poll(); + + assertEquals(1, counter.get()); + + bool.set(false); + loop.poll(); + + assertEquals(1, counter.get()); + + bool.set(true); + loop.poll(); + + assertEquals(2, counter.get()); + } + + @Test + void testEdgeReconstruct() { + var loop = new EventLoop(); + var bool = new AtomicBoolean(false); + var counter = new AtomicInteger(0); + + var event = new BooleanEvent(loop, bool::get); + event.rising().ifHigh(counter::incrementAndGet); + event.rising().ifHigh(counter::incrementAndGet); + + assertEquals(0, counter.get()); + + loop.poll(); + + assertEquals(0, counter.get()); + + bool.set(true); + loop.poll(); + + // unlike the previous test ... + assertEquals(2, counter.get()); // as natural sense dictates, counter == 2 + + loop.poll(); + + assertEquals(2, counter.get()); + + bool.set(false); + loop.poll(); + + assertEquals(2, counter.get()); + + bool.set(true); + loop.poll(); + + assertEquals(4, counter.get()); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/EventLoopTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/EventLoopTest.java new file mode 100644 index 0000000000..7f0bb1e95e --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/event/EventLoopTest.java @@ -0,0 +1,61 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.event; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicInteger; +import org.junit.jupiter.api.Test; + +class EventLoopTest { + @Test + void testConditions() { + var counterTrue = new AtomicInteger(0); + var counterFalse = new AtomicInteger(0); + var loop = new EventLoop(); + loop.bind(() -> true, counterTrue::incrementAndGet); + loop.bind(() -> false, counterFalse::incrementAndGet); + + assertEquals(0, counterTrue.get()); + assertEquals(0, counterFalse.get()); + + loop.poll(); + + assertEquals(1, counterTrue.get()); + assertEquals(0, counterFalse.get()); + + loop.poll(); + + assertEquals(2, counterTrue.get()); + assertEquals(0, counterFalse.get()); + } + + @Test + void testClear() { + var condition = new AtomicBoolean(false); + var counter = new AtomicInteger(0); + var loop = new EventLoop(); + + // first ensure binding works + loop.bind(condition::get, counter::incrementAndGet); + + condition.set(false); + loop.poll(); + assertEquals(0, counter.get()); + + condition.set(true); + loop.poll(); + assertEquals(1, counter.get()); + + // clear bindings + loop.clear(); + + condition.set(true); + loop.poll(); + // shouldn't change + assertEquals(1, counter.get()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Main.java new file mode 100644 index 0000000000..d6f3150a6c --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Main.java @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.eventloop; + +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.examples.encoder.Robot; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java new file mode 100644 index 0000000000..b4619ae7bb --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java @@ -0,0 +1,91 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.examples.eventloop; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Ultrasonic; +import edu.wpi.first.wpilibj.event.BooleanEvent; +import edu.wpi.first.wpilibj.event.EventLoop; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; + +public class Robot extends TimedRobot { + public static final int SHOT_VELOCITY = 200; // rpm + public static final int TOLERANCE = 8; // rpm + public static final int KICKER_THRESHOLD = 15; // mm + + private final MotorController m_shooter = new PWMSparkMax(0); + private final Encoder m_shooterEncoder = new Encoder(0, 1); + private final PIDController m_controller = new PIDController(0.3, 0, 0); + private final SimpleMotorFeedforward m_ff = new SimpleMotorFeedforward(0.1, 0.065); + + private final MotorController m_kicker = new PWMSparkMax(1); + private final Ultrasonic m_kickerSensor = new Ultrasonic(2, 3); + + private final MotorController m_intake = new PWMSparkMax(2); + + private final EventLoop m_loop = new EventLoop(); + private final Joystick m_joystick = new Joystick(0); + + @Override + public void robotInit() { + m_controller.setTolerance(TOLERANCE); + + BooleanEvent isBallAtKicker = + new BooleanEvent(m_loop, () -> m_kickerSensor.getRangeMM() < KICKER_THRESHOLD); + BooleanEvent intakeButton = new BooleanEvent(m_loop, () -> m_joystick.getRawButton(2)); + + // if the thumb button is held + intakeButton + // and there is not a ball at the kicker + .and(isBallAtKicker.negate()) + // activate the intake + .ifHigh(() -> m_intake.set(0.5)); + + // if the thumb button is not held + intakeButton + // or there is a ball in the kicker + .or(isBallAtKicker) + // stop the intake + .ifHigh(m_intake::stopMotor); + + BooleanEvent shootTrigger = new BooleanEvent(m_loop, m_joystick::getTrigger); + + // if the trigger is held + shootTrigger + // accelerate the shooter wheel + .ifHigh( + () -> + m_shooter.setVoltage( + m_controller.calculate(m_shooterEncoder.getRate(), SHOT_VELOCITY) + + m_ff.calculate(SHOT_VELOCITY))); + // if not, stop + shootTrigger.ifHigh(m_shooter::stopMotor); + + BooleanEvent atTargetVelocity = + new BooleanEvent(m_loop, m_controller::atSetpoint) + // debounce for more stability + .debounce(0.2); + + // if we're at the target velocity, kick the ball into the shooter wheel + atTargetVelocity.ifHigh(() -> m_kicker.set(0.7)); + + // when we stop being at the target velocity, it means the ball was shot + atTargetVelocity + .falling() + // so stop the kicker + .ifHigh(m_kicker::stopMotor); + } + + @Override + public void robotPeriodic() { + // poll all the bindings + m_loop.poll(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index ddf5d5ebec..e0e8755da8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -89,6 +89,17 @@ "mainclass": "Main", "commandversion": 2 }, + { + "name": "EventLoop", + "description": "Demonstrate managing a ball system using EventLoop and BooleanEvent.", + "tags": [ + "EventLoop" + ], + "foldername": "eventloop", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, { "name": "Relay", "description": "Demonstrate controlling a Relay from Joystick buttons.",