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 6eafae5046..392471fb02 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 @@ -23,6 +23,8 @@ public class CommandGenericHID { new HashMap<>(); private final Map, Trigger>> m_axisGreaterThanCache = new HashMap<>(); + private final Map, Trigger>> + m_axisMagnitudeGreaterThanCache = new HashMap<>(); private final Map> m_povCache = new HashMap<>(); /** @@ -260,6 +262,37 @@ public class CommandGenericHID { Pair.of(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) > threshold)); } + /** + * Constructs a Trigger instance that is true when the axis magnitude 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 magnitude value is greater than the + * provided threshold. + */ + public Trigger axisMagnitudeGreaterThan(int axis, double threshold, EventLoop loop) { + var cache = m_axisMagnitudeGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); + return cache.computeIfAbsent( + Pair.of(axis, threshold), + k -> new Trigger(loop, () -> Math.abs(getRawAxis(axis)) > threshold)); + } + + /** + * Constructs a Trigger instance that is true when the axis magnitude 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 above which this trigger should return true. + * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). + */ + public Trigger axisMagnitudeGreaterThan(int axis, double threshold) { + return axisMagnitudeGreaterThan( + axis, threshold, CommandScheduler.getInstance().getDefaultButtonLoop()); + } + /** * Get the value of the axis. * 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 b39d3d4838..b7a8cbe661 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp @@ -75,6 +75,13 @@ Trigger CommandGenericHID::AxisGreaterThan(int axis, double threshold, }); } +Trigger CommandGenericHID::AxisMagnitudeGreaterThan( + int axis, double threshold, frc::EventLoop* loop) const { + return Trigger(loop, [this, axis, threshold]() { + return std::abs(m_hid.GetRawAxis(axis)) > threshold; + }); +} + void CommandGenericHID::SetRumble(frc::GenericHID::RumbleType type, double value) { m_hid.SetRumble(type, value); 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 563a4b2b5c..334fdc9eea 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandGenericHID.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandGenericHID.h @@ -230,6 +230,21 @@ class CommandGenericHID { frc::EventLoop* loop = CommandScheduler::GetInstance().GetDefaultButtonLoop()) const; + /** + * Constructs a Trigger instance that is true when the axis magnitude 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 magnitude value is + * greater than the provided threshold. + */ + Trigger AxisMagnitudeGreaterThan( + int axis, double threshold, + frc::EventLoop* loop = + CommandScheduler::GetInstance().GetDefaultButtonLoop()) const; + /** * Set the rumble output for the HID. *