From 176fddeb4c44d7fc36d8a1358eb9ea5606660443 Mon Sep 17 00:00:00 2001 From: Ryan Blue Date: Mon, 26 Dec 2022 14:29:14 -0500 Subject: [PATCH] [commands] Add functions to HID classes to allow use of axes as BooleanEvents/Triggers (#4762) --- .../command/button/CommandGenericHID.java | 56 +++++++++++++ .../command/button/CommandXboxController.java | 78 +++++++++++++++++++ .../frc2/command/button/CommandGenericHID.cpp | 14 ++++ .../command/button/CommandXboxController.cpp | 10 +++ .../frc2/command/button/CommandGenericHID.h | 36 +++++++++ .../command/button/CommandXboxController.h | 35 +++++++++ wpilibc/src/main/native/cpp/GenericHID.cpp | 14 ++++ .../src/main/native/cpp/XboxController.cpp | 22 ++++++ .../src/main/native/include/frc/GenericHID.h | 25 ++++++ .../main/native/include/frc/XboxController.h | 44 +++++++++++ .../edu/wpi/first/wpilibj/GenericHID.java | 27 +++++++ .../edu/wpi/first/wpilibj/XboxController.java | 52 +++++++++++++ 12 files changed, 413 insertions(+) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandGenericHID.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandGenericHID.java index a34e261a4c..c393862f18 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandGenericHID.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandGenericHID.java @@ -187,6 +187,62 @@ public class CommandGenericHID { return pov(-1); } + /** + * Constructs a Trigger instance that is true when the axis value is less than {@code threshold}, + * attached to {@link CommandScheduler#getDefaultButtonLoop() the default command scheduler button + * loop}. + * + * @param axis The axis to read, starting at 0 + * @param threshold The value below which this trigger should return true. + * @return a Trigger instance that is true when the axis value is less than the provided + * threshold. + */ + public Trigger axisLessThan(int axis, double threshold) { + return axisLessThan(axis, threshold, CommandScheduler.getInstance().getDefaultButtonLoop()); + } + + /** + * Constructs a Trigger instance that is true when the axis value is less than {@code threshold}, + * attached to the given loop. + * + * @param axis The axis to read, starting at 0 + * @param threshold The value below which this trigger should return true. + * @param loop the event loop instance to attach the trigger to + * @return a Trigger instance that is true when the axis value is less than the provided + * threshold. + */ + public Trigger axisLessThan(int axis, double threshold, EventLoop loop) { + return m_hid.axisLessThan(axis, threshold, loop).castTo(Trigger::new); + } + + /** + * Constructs a Trigger instance that is true when the axis value is less than {@code threshold}, + * attached to {@link CommandScheduler#getDefaultButtonLoop() the default command scheduler button + * loop}. + * + * @param axis The axis to read, starting at 0 + * @param threshold The value above which this trigger should return true. + * @return a Trigger instance that is true when the axis value is greater than the provided + * threshold. + */ + public Trigger axisGreaterThan(int axis, double threshold) { + return axisGreaterThan(axis, threshold, CommandScheduler.getInstance().getDefaultButtonLoop()); + } + + /** + * Constructs a Trigger instance that is true when the axis value is greater than {@code + * threshold}, attached to the given loop. + * + * @param axis The axis to read, starting at 0 + * @param threshold The value above which this trigger should return true. + * @param loop the event loop instance to attach the trigger to. + * @return a Trigger instance that is true when the axis value is greater than the provided + * threshold. + */ + public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { + return m_hid.axisGreaterThan(axis, threshold, loop).castTo(Trigger::new); + } + /** * Get the value of the axis. * diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandXboxController.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandXboxController.java index 38b35da558..1e83e83183 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandXboxController.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandXboxController.java @@ -257,6 +257,84 @@ public class CommandXboxController extends CommandGenericHID { return m_hid.back(loop).castTo(Trigger::new); } + /** + * Constructs a Trigger instance around the axis value of the left trigger. The returned trigger + * will be true when the axis value is greater than {@code threshold}. + * + * @param loop the event loop instance to attach the Trigger to. + * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value + * should be in the range [0, 1] where 0 is the unpressed state of the axis. + * @return a Trigger instance that is true when the left trigger's axis exceeds the provided + * threshold, attached to the given event loop + */ + public Trigger leftTrigger(EventLoop loop, double threshold) { + return m_hid.leftTrigger(threshold, loop).castTo(Trigger::new); + } + + /** + * Constructs a Trigger instance around the axis value of the left trigger. The returned trigger + * will be true when the axis value is greater than {@code threshold}. + * + * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value + * should be in the range [0, 1] where 0 is the unpressed state of the axis. + * @return a Trigger instance that is true when the left trigger's axis exceeds the provided + * threshold, attached to the {@link CommandScheduler#getDefaultButtonLoop() default scheduler + * button loop}. + */ + public Trigger leftTrigger(double threshold) { + return leftTrigger(CommandScheduler.getInstance().getDefaultButtonLoop(), threshold); + } + + /** + * Constructs a Trigger instance around the axis value of the left trigger. The returned trigger + * will be true when the axis value is greater than 0.5. + * + * @return a Trigger instance that is true when the left trigger's axis exceeds 0.5, attached to + * the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}. + */ + public Trigger leftTrigger() { + return leftTrigger(0.5); + } + + /** + * Constructs a Trigger instance around the axis value of the right trigger. The returned trigger + * will be true when the axis value is greater than {@code threshold}. + * + * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value + * should be in the range [0, 1] where 0 is the unpressed state of the axis. + * @param loop the event loop instance to attach the Trigger to. + * @return a Trigger instance that is true when the right trigger's axis exceeds the provided + * threshold, attached to the given event loop + */ + public Trigger rightTrigger(double threshold, EventLoop loop) { + return m_hid.rightTrigger(threshold, loop).castTo(Trigger::new); + } + + /** + * Constructs a Trigger instance around the axis value of the right trigger. The returned trigger + * will be true when the axis value is greater than {@code threshold}. + * + * @param threshold the minimum axis value for the returned {@link Trigger} to be true. This value + * should be in the range [0, 1] where 0 is the unpressed state of the axis. + * @return a Trigger instance that is true when the right trigger's axis exceeds the provided + * threshold, attached to the {@link CommandScheduler#getDefaultButtonLoop() default scheduler + * button loop}. + */ + public Trigger rightTrigger(double threshold) { + return rightTrigger(threshold, CommandScheduler.getInstance().getDefaultButtonLoop()); + } + + /** + * Constructs a Trigger instance around the axis value of the right trigger. The returned trigger + * will be true when the axis value is greater than 0.5. + * + * @return a Trigger instance that is true when the right trigger's axis exceeds 0.5, attached to + * the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}. + */ + public Trigger rightTrigger() { + return rightTrigger(0.5); + } + /** * Get the X axis value of left side of the controller. * diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp index 6050c99c0f..ea5696ed62 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp @@ -54,3 +54,17 @@ Trigger CommandGenericHID::POVUpLeft(frc::EventLoop* loop) const { Trigger CommandGenericHID::POVCenter(frc::EventLoop* loop) const { return POV(360, loop); } + +Trigger CommandGenericHID::AxisLessThan(int axis, double threshold, + frc::EventLoop* loop) const { + return Trigger(loop, [this, axis, threshold]() { + return this->GetRawAxis(axis) < threshold; + }); +} + +Trigger CommandGenericHID::AxisGreaterThan(int axis, double threshold, + frc::EventLoop* loop) const { + return Trigger(loop, [this, axis, threshold]() { + return this->GetRawAxis(axis) > threshold; + }); +} diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandXboxController.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandXboxController.cpp index 7c60ca9e8b..8c4eba0fd2 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandXboxController.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandXboxController.cpp @@ -49,3 +49,13 @@ Trigger CommandXboxController::Back(frc::EventLoop* loop) const { Trigger CommandXboxController::Start(frc::EventLoop* loop) const { return XboxController::Start(loop).CastTo(); } + +Trigger CommandXboxController::LeftTrigger(double threshold, + frc::EventLoop* loop) const { + return XboxController::LeftTrigger(threshold, loop).CastTo(); +} + +Trigger CommandXboxController::RightTrigger(double threshold, + frc::EventLoop* loop) const { + return XboxController::RightTrigger(threshold, loop).CastTo(); +} diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandGenericHID.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandGenericHID.h index d27cd30f4b..d08af4e137 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandGenericHID.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandGenericHID.h @@ -179,5 +179,41 @@ class CommandGenericHID : public frc::GenericHID { */ Trigger POVCenter(frc::EventLoop* loop = CommandScheduler::GetInstance() .GetDefaultButtonLoop()) const; + + /** + * Constructs a Trigger instance that is true when the axis value is less than + * {@code threshold}, attached to {@link + * CommandScheduler::GetDefaultButtonLoop() the default command scheduler + * button loop}. + * @param axis The axis to read, starting at 0. + * @param threshold The value below which this trigger should return true. + * @param loop the event loop instance to attach the event to. Defaults to + * {@link CommandScheduler::GetDefaultButtonLoop() the default command + * scheduler button loop}. + * @return a Trigger instance that is true when the axis value is less than + * the provided threshold. + */ + Trigger AxisLessThan( + int axis, double threshold, + frc::EventLoop* loop = + CommandScheduler::GetInstance().GetDefaultButtonLoop()) const; + + /** + * Constructs a Trigger instance that is true when the axis value is greater + * than {@code threshold}, attached to {@link + * CommandScheduler::GetDefaultButtonLoop() the default command scheduler + * button loop}. + * @param axis The axis to read, starting at 0. + * @param threshold The value below which this trigger should return true. + * @param loop the event loop instance to attach the event to. Defaults to + * {@link CommandScheduler::GetDefaultButtonLoop() the default command + * scheduler button loop}. + * @return a Trigger instance that is true when the axis value is greater than + * the provided threshold. + */ + Trigger AxisGreaterThan( + int axis, double threshold, + frc::EventLoop* loop = + CommandScheduler::GetInstance().GetDefaultButtonLoop()) const; }; } // namespace frc2 diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandXboxController.h b/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandXboxController.h index fb1357a295..337ec2fba8 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandXboxController.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandXboxController.h @@ -141,5 +141,40 @@ class CommandXboxController : public frc::XboxController { */ Trigger Start(frc::EventLoop* loop = CommandScheduler::GetInstance() .GetDefaultButtonLoop()) const; + + /** + * Constructs a Trigger instance around the axis value of the left trigger. + * The returned Trigger will be true when the axis value is greater than + * {@code threshold}. + * + * @param threshold the minimum axis value for the returned Trigger to be + * true. This value should be in the range [0, 1] where 0 is the unpressed + * state of the axis. Defaults to 0.5. + * @param loop the event loop instance to attach the Trigger to. Defaults to + * the CommandScheduler's default loop. + * @return a Trigger instance that is true when the left trigger's axis + * exceeds the provided threshold, attached to the given loop + */ + Trigger LeftTrigger(double threshold = 0.5, + frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + + /** + * Constructs a Trigger instance around the axis value of the right trigger. + * The returned Trigger will be true when the axis value is greater than + * {@code threshold}. + * + * @param threshold the minimum axis value for the returned Trigger to be + * true. This value should be in the range [0, 1] where 0 is the unpressed + * state of the axis. Defaults to 0.5. + * @param loop the event loop instance to attach the Trigger to. Defaults to + * the CommandScheduler's default loop. + * @return a Trigger instance that is true when the right trigger's axis + * exceeds the provided threshold, attached to the given loop + */ + Trigger RightTrigger( + double threshold = 0.5, + frc::EventLoop* loop = + CommandScheduler::GetInstance().GetDefaultButtonLoop()) const; }; } // namespace frc2 diff --git a/wpilibc/src/main/native/cpp/GenericHID.cpp b/wpilibc/src/main/native/cpp/GenericHID.cpp index 39db699cc4..f4d26b64de 100644 --- a/wpilibc/src/main/native/cpp/GenericHID.cpp +++ b/wpilibc/src/main/native/cpp/GenericHID.cpp @@ -89,6 +89,20 @@ BooleanEvent GenericHID::POVCenter(EventLoop* loop) const { return POV(360, loop); } +BooleanEvent GenericHID::AxisLessThan(int axis, double threshold, + EventLoop* loop) const { + return BooleanEvent(loop, [this, axis, threshold]() { + return this->GetRawAxis(axis) < threshold; + }); +} + +BooleanEvent GenericHID::AxisGreaterThan(int axis, double threshold, + EventLoop* loop) const { + return BooleanEvent(loop, [this, axis, threshold]() { + return this->GetRawAxis(axis) > threshold; + }); +} + int GenericHID::GetAxisCount() const { return DriverStation::GetStickAxisCount(m_port); } diff --git a/wpilibc/src/main/native/cpp/XboxController.cpp b/wpilibc/src/main/native/cpp/XboxController.cpp index b47808b1cc..f10419f400 100644 --- a/wpilibc/src/main/native/cpp/XboxController.cpp +++ b/wpilibc/src/main/native/cpp/XboxController.cpp @@ -197,3 +197,25 @@ bool XboxController::GetStartButtonReleased() { BooleanEvent XboxController::Start(EventLoop* loop) const { return BooleanEvent(loop, [this]() { return this->GetStartButton(); }); } + +BooleanEvent XboxController::LeftTrigger(double threshold, + EventLoop* loop) const { + return BooleanEvent(loop, [this, threshold]() { + return this->GetLeftTriggerAxis() > threshold; + }); +} + +BooleanEvent XboxController::LeftTrigger(EventLoop* loop) const { + return this->LeftTrigger(0.5, loop); +} + +BooleanEvent XboxController::RightTrigger(double threshold, + EventLoop* loop) const { + return BooleanEvent(loop, [this, threshold]() { + return this->GetRightTriggerAxis() > threshold; + }); +} + +BooleanEvent XboxController::RightTrigger(EventLoop* loop) const { + return this->RightTrigger(0.5, loop); +} diff --git a/wpilibc/src/main/native/include/frc/GenericHID.h b/wpilibc/src/main/native/include/frc/GenericHID.h index a2a96cdc6e..fc5dd3642e 100644 --- a/wpilibc/src/main/native/include/frc/GenericHID.h +++ b/wpilibc/src/main/native/include/frc/GenericHID.h @@ -231,6 +231,31 @@ class GenericHID { */ BooleanEvent POVCenter(EventLoop* loop) const; + /** + * Constructs an event instance that is true when the axis value is less than + * threshold + * + * @param axis The axis to read, starting at 0. + * @param threshold The value below which this trigger should return true. + * @param loop the event loop instance to attach the event to. + * @return an event instance that is true when the axis value is less than the + * provided threshold. + */ + BooleanEvent AxisLessThan(int axis, double threshold, EventLoop* loop) const; + + /** + * Constructs an event instance that is true when the axis value is greater + * than threshold + * + * @param axis The axis to read, starting at 0. + * @param threshold The value above which this trigger should return true. + * @param loop the event loop instance to attach the event to. + * @return an event instance that is true when the axis value is greater than + * the provided threshold. + */ + BooleanEvent AxisGreaterThan(int axis, double threshold, + EventLoop* loop) const; + /** * Get the number of axes for the HID. * diff --git a/wpilibc/src/main/native/include/frc/XboxController.h b/wpilibc/src/main/native/include/frc/XboxController.h index c9c499fb42..370e46e3f0 100644 --- a/wpilibc/src/main/native/include/frc/XboxController.h +++ b/wpilibc/src/main/native/include/frc/XboxController.h @@ -342,6 +342,50 @@ class XboxController : public GenericHID { */ BooleanEvent Start(EventLoop* loop) const; + /** + * Constructs an event instance around the axis value of the left trigger. The + * returned trigger will be true when the axis value is greater than {@code + * threshold}. + * @param threshold the minimum axis value for the returned event to be true. + * This value should be in the range [0, 1] where 0 is the unpressed state of + * the axis. + * @param loop the event loop instance to attach the event to. + * @return an event instance that is true when the left trigger's axis exceeds + * the provided threshold, attached to the given event loop + */ + BooleanEvent LeftTrigger(double threshold, EventLoop* loop) const; + + /** + * Constructs an event instance around the axis value of the left trigger. + * The returned trigger will be true when the axis value is greater than 0.5. + * @param loop the event loop instance to attach the event to. + * @return an event instance that is true when the right trigger's axis + * exceeds 0.5, attached to the given event loop + */ + BooleanEvent LeftTrigger(EventLoop* loop) const; + + /** + * Constructs an event instance around the axis value of the right trigger. + * The returned trigger will be true when the axis value is greater than + * {@code threshold}. + * @param threshold the minimum axis value for the returned event to be true. + * This value should be in the range [0, 1] where 0 is the unpressed state of + * the axis. + * @param loop the event loop instance to attach the event to. + * @return an event instance that is true when the right trigger's axis + * exceeds the provided threshold, attached to the given event loop + */ + BooleanEvent RightTrigger(double threshold, EventLoop* loop) const; + + /** + * Constructs an event instance around the axis value of the right trigger. + * The returned trigger will be true when the axis value is greater than 0.5. + * @param loop the event loop instance to attach the event to. + * @return an event instance that is true when the right trigger's axis + * exceeds 0.5, attached to the given event loop + */ + BooleanEvent RightTrigger(EventLoop* loop) const; + struct Button { static constexpr int kLeftBumper = 5; static constexpr int kRightBumper = 6; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java index 686dd88ad6..a89966efc9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java @@ -297,6 +297,33 @@ public class GenericHID { return pov(-1, loop); } + /** + * Constructs an event instance that is true when the axis value is less than {@code threshold}, + * attached to the given loop. + * + * @param axis The axis to read, starting at 0 + * @param threshold The value below which this event should return true. + * @param loop the event loop instance to attach the event to. + * @return an event instance that is true when the axis value is less than the provided threshold. + */ + public BooleanEvent axisLessThan(int axis, double threshold, EventLoop loop) { + return new BooleanEvent(loop, () -> getRawAxis(axis) < threshold); + } + + /** + * Constructs an event instance that is true when the axis value is greater than {@code + * threshold}, attached to the given loop. + * + * @param axis The axis to read, starting at 0 + * @param threshold The value above which this event should return true. + * @param loop the event loop instance to attach the event to. + * @return an event instance that is true when the axis value is greater than the provided + * threshold. + */ + public BooleanEvent axisGreaterThan(int axis, double threshold, EventLoop loop) { + return new BooleanEvent(loop, () -> getRawAxis(axis) > threshold); + } + /** * Get the number of axes for the HID. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java index d5f0fcc4e7..47195eb1df 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java @@ -537,4 +537,56 @@ public class XboxController extends GenericHID { public BooleanEvent start(EventLoop loop) { return new BooleanEvent(loop, this::getStartButton); } + + /** + * Constructs an event instance around the axis value of the right trigger. The returned trigger + * will be true when the axis value is greater than {@code threshold}. + * + * @param threshold the minimum axis value for the returned {@link BooleanEvent} to be true. This + * value should be in the range [0, 1] where 0 is the unpressed state of the axis. + * @param loop the event loop instance to attach the event to. + * @return an event instance that is true when the right trigger's axis exceeds the provided + * threshold, attached to the given event loop + */ + public BooleanEvent leftTrigger(double threshold, EventLoop loop) { + return new BooleanEvent(loop, () -> getLeftTriggerAxis() > threshold); + } + + /** + * Constructs an event instance around the axis value of the right trigger. The returned trigger + * will be true when the axis value is greater than 0.5. + * + * @param loop the event loop instance to attach the event to. + * @return an event instance that is true when the right trigger's axis exceeds the provided + * threshold, attached to the given event loop + */ + public BooleanEvent leftTrigger(EventLoop loop) { + return leftTrigger(0.5, loop); + } + + /** + * Constructs an event instance around the axis value of the right trigger. The returned trigger + * will be true when the axis value is greater than {@code threshold}. + * + * @param threshold the minimum axis value for the returned {@link BooleanEvent} to be true. This + * value should be in the range [0, 1] where 0 is the unpressed state of the axis. + * @param loop the event loop instance to attach the event to. + * @return an event instance that is true when the right trigger's axis exceeds the provided + * threshold, attached to the given event loop + */ + public BooleanEvent rightTrigger(double threshold, EventLoop loop) { + return new BooleanEvent(loop, () -> getRightTriggerAxis() > threshold); + } + + /** + * Constructs an event instance around the axis value of the right trigger. The returned trigger + * will be true when the axis value is greater than 0.5. + * + * @param loop the event loop instance to attach the event to. + * @return an event instance that is true when the right trigger's axis exceeds the provided + * threshold, attached to the given event loop + */ + public BooleanEvent rightTrigger(EventLoop loop) { + return rightTrigger(0.5, loop); + } }