From e0e186e9c1ada9bdc79eebf4deb8139bd14a4f08 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 15 Sep 2024 17:14:15 -0400 Subject: [PATCH 01/16] first draft java --- .../command/button/CommandGenericHID.java | 61 ++++++++++++++++++- 1 file changed, 60 insertions(+), 1 deletion(-) 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 6eafae5046d..ab4f383a194 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 @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj2.command.button; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.event.EventLoop; @@ -244,6 +245,64 @@ 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 active (non-zero) given a {@code + * deadband}, attached to the given loop. + * + * @param axis The axis to read, starting at 0 + * @param deadband The value used to deadband the axis value. The trigger returns true if the deadbanded value is non-zero. + * @param loop the event loop instance to attach the trigger to. + * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). + */ + public Trigger axisActive(int axis, double deadband, EventLoop loop) { + var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); + return cache.computeIfAbsent( + Pair.of(axis, deadband), k -> new Trigger(loop, () -> MathUtil.applyDeadband(getRawAxis(axis), deadband) != 0.0)); + } + + /** + * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a {@code + * deadband}, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command scheduler button + * loop}. + * + * @param axis The axis to read, starting at 0 + * @param deadband The value used to deadband the axis value. The trigger returns true if the deadbanded value is non-zero. + * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). + */ + public Trigger axisActive(int axis, double deadband) { + return axisGreaterThan(axis, deadband, CommandScheduler.getInstance().getDefaultButtonLoop()); + } + + /** + * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a {@code + * deadband}, attached to the given loop. + * + * @param axis The axis to read, starting at 0 + * @param deadband The value used to deadband the axis value. The trigger returns true if the deadbanded value is non-zero. + * @param maxMagnitude The maximum magnitude of the input. Can be infinite. + * @param loop the event loop instance to attach the trigger to. + * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). + */ + public Trigger axisActive(int axis, double deadband, double maxMagnitude, EventLoop loop) { + var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); + return cache.computeIfAbsent( + Pair.of(axis, deadband), k -> new Trigger(loop, () -> MathUtil.applyDeadband(getRawAxis(axis), deadband, maxMagnitude) != 0.0)); + } + + /** + * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a {@code + * deadband}, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command scheduler button + * loop}. + * + * @param axis The axis to read, starting at 0 + * @param deadband The value used to deadband the axis value. The trigger returns true if the deadbanded value is non-zero. + * @param maxMagnitude The maximum magnitude of the input. Can be infinite. + * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). + */ + public Trigger axisActive(int axis, double deadband, double maxMagnitude) { + return axisActive(axis, deadband, maxMagnitude, CommandScheduler.getInstance().getDefaultButtonLoop()); + } + /** * Constructs a Trigger instance that is true when the axis value is greater than {@code * threshold}, attached to the given loop. @@ -258,7 +317,7 @@ public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( Pair.of(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) > threshold)); - } + } /** * Get the value of the axis. From dee6b4182de252eb28df190c53936f64fdb14b28 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 15 Sep 2024 17:14:57 -0400 Subject: [PATCH 02/16] first draft java --- .../command/button/CommandGenericHID.java | 61 ++++++++++++++++++- 1 file changed, 60 insertions(+), 1 deletion(-) 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 6eafae5046d..ab4f383a194 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 @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj2.command.button; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.event.EventLoop; @@ -244,6 +245,64 @@ 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 active (non-zero) given a {@code + * deadband}, attached to the given loop. + * + * @param axis The axis to read, starting at 0 + * @param deadband The value used to deadband the axis value. The trigger returns true if the deadbanded value is non-zero. + * @param loop the event loop instance to attach the trigger to. + * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). + */ + public Trigger axisActive(int axis, double deadband, EventLoop loop) { + var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); + return cache.computeIfAbsent( + Pair.of(axis, deadband), k -> new Trigger(loop, () -> MathUtil.applyDeadband(getRawAxis(axis), deadband) != 0.0)); + } + + /** + * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a {@code + * deadband}, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command scheduler button + * loop}. + * + * @param axis The axis to read, starting at 0 + * @param deadband The value used to deadband the axis value. The trigger returns true if the deadbanded value is non-zero. + * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). + */ + public Trigger axisActive(int axis, double deadband) { + return axisGreaterThan(axis, deadband, CommandScheduler.getInstance().getDefaultButtonLoop()); + } + + /** + * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a {@code + * deadband}, attached to the given loop. + * + * @param axis The axis to read, starting at 0 + * @param deadband The value used to deadband the axis value. The trigger returns true if the deadbanded value is non-zero. + * @param maxMagnitude The maximum magnitude of the input. Can be infinite. + * @param loop the event loop instance to attach the trigger to. + * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). + */ + public Trigger axisActive(int axis, double deadband, double maxMagnitude, EventLoop loop) { + var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); + return cache.computeIfAbsent( + Pair.of(axis, deadband), k -> new Trigger(loop, () -> MathUtil.applyDeadband(getRawAxis(axis), deadband, maxMagnitude) != 0.0)); + } + + /** + * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a {@code + * deadband}, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command scheduler button + * loop}. + * + * @param axis The axis to read, starting at 0 + * @param deadband The value used to deadband the axis value. The trigger returns true if the deadbanded value is non-zero. + * @param maxMagnitude The maximum magnitude of the input. Can be infinite. + * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). + */ + public Trigger axisActive(int axis, double deadband, double maxMagnitude) { + return axisActive(axis, deadband, maxMagnitude, CommandScheduler.getInstance().getDefaultButtonLoop()); + } + /** * Constructs a Trigger instance that is true when the axis value is greater than {@code * threshold}, attached to the given loop. @@ -258,7 +317,7 @@ public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( Pair.of(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) > threshold)); - } + } /** * Get the value of the axis. From 13d5fdeaa9bed06bc5381e96079647ac974faaf1 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 15 Sep 2024 21:54:03 +0000 Subject: [PATCH 03/16] Formatting fixes --- .../command/button/CommandGenericHID.java | 56 +++++++++++-------- 1 file changed, 33 insertions(+), 23 deletions(-) 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 ab4f383a194..806582d4611 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 @@ -246,27 +246,30 @@ public Trigger axisGreaterThan(int axis, double threshold) { } /** - * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a {@code - * deadband}, attached to the given loop. + * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a + * {@code deadband}, attached to the given loop. * * @param axis The axis to read, starting at 0 - * @param deadband The value used to deadband the axis value. The trigger returns true if the deadbanded value is non-zero. + * @param deadband The value used to deadband the axis value. The trigger returns true if the + * deadbanded value is non-zero. * @param loop the event loop instance to attach the trigger to. * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). */ public Trigger axisActive(int axis, double deadband, EventLoop loop) { var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( - Pair.of(axis, deadband), k -> new Trigger(loop, () -> MathUtil.applyDeadband(getRawAxis(axis), deadband) != 0.0)); + Pair.of(axis, deadband), + k -> new Trigger(loop, () -> MathUtil.applyDeadband(getRawAxis(axis), deadband) != 0.0)); } /** - * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a {@code - * deadband}, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command scheduler button - * loop}. - * + * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a + * {@code deadband}, attached to {@link CommandScheduler#getDefaultButtonLoop() the default + * command scheduler button loop}. + * * @param axis The axis to read, starting at 0 - * @param deadband The value used to deadband the axis value. The trigger returns true if the deadbanded value is non-zero. + * @param deadband The value used to deadband the axis value. The trigger returns true if the + * deadbanded value is non-zero. * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). */ public Trigger axisActive(int axis, double deadband) { @@ -274,34 +277,41 @@ public Trigger axisActive(int axis, double deadband) { } /** - * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a {@code - * deadband}, attached to the given loop. + * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a + * {@code deadband}, attached to the given loop. * * @param axis The axis to read, starting at 0 - * @param deadband The value used to deadband the axis value. The trigger returns true if the deadbanded value is non-zero. - * @param maxMagnitude The maximum magnitude of the input. Can be infinite. + * @param deadband The value used to deadband the axis value. The trigger returns true if the + * deadbanded value is non-zero. + * @param maxMagnitude The maximum magnitude of the input. Can be infinite. * @param loop the event loop instance to attach the trigger to. * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). */ public Trigger axisActive(int axis, double deadband, double maxMagnitude, EventLoop loop) { var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( - Pair.of(axis, deadband), k -> new Trigger(loop, () -> MathUtil.applyDeadband(getRawAxis(axis), deadband, maxMagnitude) != 0.0)); + Pair.of(axis, deadband), + k -> + new Trigger( + loop, + () -> MathUtil.applyDeadband(getRawAxis(axis), deadband, maxMagnitude) != 0.0)); } /** - * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a {@code - * deadband}, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command scheduler button - * loop}. - * + * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a + * {@code deadband}, attached to {@link CommandScheduler#getDefaultButtonLoop() the default + * command scheduler button loop}. + * * @param axis The axis to read, starting at 0 - * @param deadband The value used to deadband the axis value. The trigger returns true if the deadbanded value is non-zero. - * @param maxMagnitude The maximum magnitude of the input. Can be infinite. + * @param deadband The value used to deadband the axis value. The trigger returns true if the + * deadbanded value is non-zero. + * @param maxMagnitude The maximum magnitude of the input. Can be infinite. * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). */ public Trigger axisActive(int axis, double deadband, double maxMagnitude) { - return axisActive(axis, deadband, maxMagnitude, CommandScheduler.getInstance().getDefaultButtonLoop()); - } + return axisActive( + axis, deadband, maxMagnitude, CommandScheduler.getInstance().getDefaultButtonLoop()); + } /** * Constructs a Trigger instance that is true when the axis value is greater than {@code @@ -317,7 +327,7 @@ public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( Pair.of(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) > threshold)); - } + } /** * Get the value of the axis. From 09be2adf51d27a1887c7acfc84dc2318c63a702d Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 15 Sep 2024 18:03:05 -0400 Subject: [PATCH 04/16] method rearrangement --- .../command/button/CommandGenericHID.java | 32 ++++++++++--------- 1 file changed, 17 insertions(+), 15 deletions(-) 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 806582d4611..6dd37ee4559 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 @@ -245,6 +245,22 @@ 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) { + var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); + return cache.computeIfAbsent( + Pair.of(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) > threshold)); + } + /** * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a * {@code deadband}, attached to the given loop. @@ -313,21 +329,7 @@ public Trigger axisActive(int axis, double deadband, double maxMagnitude) { axis, deadband, maxMagnitude, 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) { - var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); - return cache.computeIfAbsent( - Pair.of(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) > threshold)); - } + /** * Get the value of the axis. From 08f2ee721f3876b9ca24dc0b9b7698d39e31573a Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Sun, 15 Sep 2024 22:08:43 +0000 Subject: [PATCH 05/16] Formatting fixes --- .../wpi/first/wpilibj2/command/button/CommandGenericHID.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) 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 6dd37ee4559..beb2b5e27c5 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 @@ -259,7 +259,7 @@ public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( Pair.of(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) > threshold)); - } + } /** * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a @@ -329,8 +329,6 @@ public Trigger axisActive(int axis, double deadband, double maxMagnitude) { axis, deadband, maxMagnitude, CommandScheduler.getInstance().getDefaultButtonLoop()); } - - /** * Get the value of the axis. * From b7ac6c39b4cb49502a3ba791a263c47230554424 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 15 Sep 2024 18:10:08 -0400 Subject: [PATCH 06/16] formatting fixes --- .../wpi/first/wpilibj2/command/button/CommandGenericHID.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 6dd37ee4559..109527e4d96 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 @@ -259,7 +259,7 @@ public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( Pair.of(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) > threshold)); - } + } /** * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a From 82d31e29e6aecafdf37d986db51411dff9e3383e Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 15 Sep 2024 23:44:08 -0400 Subject: [PATCH 07/16] Update wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandGenericHID.java Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- .../wpi/first/wpilibj2/command/button/CommandGenericHID.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 beb2b5e27c5..e03d17737d9 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 @@ -275,7 +275,7 @@ public Trigger axisActive(int axis, double deadband, EventLoop loop) { var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( Pair.of(axis, deadband), - k -> new Trigger(loop, () -> MathUtil.applyDeadband(getRawAxis(axis), deadband) != 0.0)); + k -> new Trigger(loop, () -> Math.abs(getRawAxis(axis)) > deadband)); } /** From 63102a234f21bd564265c2bd03cb5aedc2ce8f7e Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 15 Sep 2024 23:44:27 -0400 Subject: [PATCH 08/16] Update wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandGenericHID.java Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- .../wpi/first/wpilibj2/command/button/CommandGenericHID.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e03d17737d9..5f064036774 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 @@ -289,7 +289,7 @@ public Trigger axisActive(int axis, double deadband, EventLoop loop) { * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). */ public Trigger axisActive(int axis, double deadband) { - return axisGreaterThan(axis, deadband, CommandScheduler.getInstance().getDefaultButtonLoop()); + return axisActive(axis, deadband, CommandScheduler.getInstance().getDefaultButtonLoop()); } /** From ab63fac57b2eabebd40a913c2f5fa750dc849696 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Sun, 15 Sep 2024 23:47:09 -0400 Subject: [PATCH 09/16] cache addition and method removal --- .../command/button/CommandGenericHID.java | 42 ++----------------- 1 file changed, 3 insertions(+), 39 deletions(-) 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 5f064036774..043cef1fc78 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 @@ -4,7 +4,6 @@ package edu.wpi.first.wpilibj2.command.button; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.event.EventLoop; @@ -24,6 +23,8 @@ public class CommandGenericHID { new HashMap<>(); private final Map, Trigger>> m_axisGreaterThanCache = new HashMap<>(); + private final Map, Trigger>> m_axisActive = + new HashMap<>(); private final Map> m_povCache = new HashMap<>(); /** @@ -272,7 +273,7 @@ public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). */ public Trigger axisActive(int axis, double deadband, EventLoop loop) { - var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); + var cache = m_axisActive.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( Pair.of(axis, deadband), k -> new Trigger(loop, () -> Math.abs(getRawAxis(axis)) > deadband)); @@ -292,43 +293,6 @@ public Trigger axisActive(int axis, double deadband) { return axisActive(axis, deadband, CommandScheduler.getInstance().getDefaultButtonLoop()); } - /** - * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a - * {@code deadband}, attached to the given loop. - * - * @param axis The axis to read, starting at 0 - * @param deadband The value used to deadband the axis value. The trigger returns true if the - * deadbanded value is non-zero. - * @param maxMagnitude The maximum magnitude of the input. Can be infinite. - * @param loop the event loop instance to attach the trigger to. - * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). - */ - public Trigger axisActive(int axis, double deadband, double maxMagnitude, EventLoop loop) { - var cache = m_axisGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); - return cache.computeIfAbsent( - Pair.of(axis, deadband), - k -> - new Trigger( - loop, - () -> MathUtil.applyDeadband(getRawAxis(axis), deadband, maxMagnitude) != 0.0)); - } - - /** - * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a - * {@code deadband}, attached to {@link CommandScheduler#getDefaultButtonLoop() the default - * command scheduler button loop}. - * - * @param axis The axis to read, starting at 0 - * @param deadband The value used to deadband the axis value. The trigger returns true if the - * deadbanded value is non-zero. - * @param maxMagnitude The maximum magnitude of the input. Can be infinite. - * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). - */ - public Trigger axisActive(int axis, double deadband, double maxMagnitude) { - return axisActive( - axis, deadband, maxMagnitude, CommandScheduler.getInstance().getDefaultButtonLoop()); - } - /** * Get the value of the axis. * From d0a997b966b6ef6ed149f12ef6c997a1dade8500 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 16 Sep 2024 03:51:56 +0000 Subject: [PATCH 10/16] Formatting fixes --- .../wpi/first/wpilibj2/command/button/CommandGenericHID.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 043cef1fc78..2eb48f6d497 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,8 +23,7 @@ public class CommandGenericHID { new HashMap<>(); private final Map, Trigger>> m_axisGreaterThanCache = new HashMap<>(); - private final Map, Trigger>> m_axisActive = - new HashMap<>(); + private final Map, Trigger>> m_axisActive = new HashMap<>(); private final Map> m_povCache = new HashMap<>(); /** From b5c2103ea5f58e63a2a1775e1a82679ac385271b Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Mon, 16 Sep 2024 00:02:05 -0400 Subject: [PATCH 11/16] C++ first draft. --- .../frc2/command/button/CommandGenericHID.cpp | 7 +++++++ .../frc2/command/button/CommandGenericHID.h | 16 ++++++++++++++++ 2 files changed, 23 insertions(+) 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 b39d3d48389..fef7ae79f4f 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::AxisActive(int axis, double deadband, + frc::EventLoop* loop) const { + return Trigger(loop, [this, axis, deadband]() { + return std::abs(m_hid.GetRawAxis(axis)) > deadband; + }); +} + 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 563a4b2b5c3..d59a5d81575 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,22 @@ class CommandGenericHID { frc::EventLoop* loop = CommandScheduler::GetInstance().GetDefaultButtonLoop()) const; + /** + * Constructs a Trigger instance that is true when the axis value is active + * (non-zero) given a + * {@code deadband}, attached to the given loop. + * + * @param axis The axis to read, starting at 0 + * @param deadband The value used to deadband the axis value. The trigger + * returns true if the deadbanded value is non-zero. + * @param loop the event loop instance to attach the trigger to. + * @return a Trigger instance that is true when the deadbanded axis value is + * active (non-zero). + */ + Trigger AxisActive(int axis, double deadband, + frc::EventLoop* loop = CommandScheduler::GetInstance() + .GetDefaultButtonLoop()) const; + /** * Set the rumble output for the HID. * From 528bba3289a772237b03ade4518bf8bdcfb9f220 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Mon, 16 Sep 2024 06:50:48 -0400 Subject: [PATCH 12/16] method rename, javadoc reconfigure --- .../command/button/CommandGenericHID.java | 32 +++++++++---------- .../frc2/command/button/CommandGenericHID.cpp | 2 +- .../frc2/command/button/CommandGenericHID.h | 14 ++++---- 3 files changed, 22 insertions(+), 26 deletions(-) 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 2eb48f6d497..5f08b68c01b 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,7 +23,7 @@ public class CommandGenericHID { new HashMap<>(); private final Map, Trigger>> m_axisGreaterThanCache = new HashMap<>(); - private final Map, Trigger>> m_axisActive = new HashMap<>(); + private final Map, Trigger>> m_axisMagnitudeGreaterThanCache = new HashMap<>(); private final Map> m_povCache = new HashMap<>(); /** @@ -262,34 +262,32 @@ public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { } /** - * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a - * {@code deadband}, attached to the given loop. + * 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 deadband The value used to deadband the axis value. The trigger returns true if the - * deadbanded value is non-zero. + * @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 deadbanded axis value is active (non-zero). + * @return a Trigger instance that is true when the axis magnitude value is greater than the provided + * threshold. */ - public Trigger axisActive(int axis, double deadband, EventLoop loop) { - var cache = m_axisActive.computeIfAbsent(loop, k -> new HashMap<>()); + public Trigger axisMagnitudeGreaterThan(int axis, double threshold, EventLoop loop) { + var cache = m_axisMagnitudeGreaterThanCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( - Pair.of(axis, deadband), - k -> new Trigger(loop, () -> Math.abs(getRawAxis(axis)) > deadband)); + Pair.of(axis, threshold), + k -> new Trigger(loop, () -> Math.abs(getRawAxis(axis)) > threshold)); } /** - * Constructs a Trigger instance that is true when the axis value is active (non-zero) given a - * {@code deadband}, attached to {@link CommandScheduler#getDefaultButtonLoop() the default - * command scheduler button loop}. + * 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 deadband The value used to deadband the axis value. The trigger returns true if the - * deadbanded value is non-zero. + * @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 axisActive(int axis, double deadband) { - return axisActive(axis, deadband, CommandScheduler.getInstance().getDefaultButtonLoop()); + public Trigger axisActive(int axis, double threshold) { + return axisMagnitudeGreaterThan(axis, threshold, CommandScheduler.getInstance().getDefaultButtonLoop()); } /** 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 fef7ae79f4f..a749399a7a7 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp @@ -75,7 +75,7 @@ Trigger CommandGenericHID::AxisGreaterThan(int axis, double threshold, }); } -Trigger CommandGenericHID::AxisActive(int axis, double deadband, +Trigger CommandGenericHID::AxisMagnitudeGreaterThan(int axis, double deadband, frc::EventLoop* loop) const { return Trigger(loop, [this, axis, deadband]() { return std::abs(m_hid.GetRawAxis(axis)) > deadband; 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 d59a5d81575..2a4891ea113 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandGenericHID.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandGenericHID.h @@ -231,18 +231,16 @@ class CommandGenericHID { CommandScheduler::GetInstance().GetDefaultButtonLoop()) const; /** - * Constructs a Trigger instance that is true when the axis value is active - * (non-zero) given a - * {@code deadband}, attached to the given loop. + * 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 deadband The value used to deadband the axis value. The trigger - * returns true if the deadbanded value is non-zero. + * @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 deadbanded axis value is - * active (non-zero). + * @return a Trigger instance that is true when the axis magnitude value is greater than the provided + * threshold. */ - Trigger AxisActive(int axis, double deadband, + Trigger AxisMagnitudeGreaterThan(int axis, double threshold, frc::EventLoop* loop = CommandScheduler::GetInstance() .GetDefaultButtonLoop()) const; From 0fb39ca91f6506bcc4e6385c8d50a51586e181a4 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Mon, 16 Sep 2024 10:56:55 +0000 Subject: [PATCH 13/16] Formatting fixes --- .../command/button/CommandGenericHID.java | 14 ++++++++------ .../cpp/frc2/command/button/CommandGenericHID.cpp | 4 ++-- .../frc2/command/button/CommandGenericHID.h | 15 ++++++++------- 3 files changed, 18 insertions(+), 15 deletions(-) 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 5f08b68c01b..fa9b0100578 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,7 +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, Trigger>> + m_axisMagnitudeGreaterThanCache = new HashMap<>(); private final Map> m_povCache = new HashMap<>(); /** @@ -262,14 +263,14 @@ public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { } /** - * Constructs a Trigger instance that is true when the axis magnitude value is greater than {@code + * 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. + * @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<>()); @@ -279,7 +280,7 @@ public Trigger axisMagnitudeGreaterThan(int axis, double threshold, EventLoop lo } /** - * Constructs a Trigger instance that is true when the axis magnitude value is greater than {@code + * 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 @@ -287,7 +288,8 @@ public Trigger axisMagnitudeGreaterThan(int axis, double threshold, EventLoop lo * @return a Trigger instance that is true when the deadbanded axis value is active (non-zero). */ public Trigger axisActive(int axis, double threshold) { - return axisMagnitudeGreaterThan(axis, threshold, CommandScheduler.getInstance().getDefaultButtonLoop()); + return axisMagnitudeGreaterThan( + axis, threshold, CommandScheduler.getInstance().getDefaultButtonLoop()); } /** 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 a749399a7a7..8ddfdf12f05 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp @@ -75,8 +75,8 @@ Trigger CommandGenericHID::AxisGreaterThan(int axis, double threshold, }); } -Trigger CommandGenericHID::AxisMagnitudeGreaterThan(int axis, double deadband, - frc::EventLoop* loop) const { +Trigger CommandGenericHID::AxisMagnitudeGreaterThan( + int axis, double deadband, frc::EventLoop* loop) const { return Trigger(loop, [this, axis, deadband]() { return std::abs(m_hid.GetRawAxis(axis)) > deadband; }); 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 2a4891ea113..334fdc9eea3 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandGenericHID.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/button/CommandGenericHID.h @@ -231,18 +231,19 @@ class CommandGenericHID { 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. + * 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. + * @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; + Trigger AxisMagnitudeGreaterThan( + int axis, double threshold, + frc::EventLoop* loop = + CommandScheduler::GetInstance().GetDefaultButtonLoop()) const; /** * Set the rumble output for the HID. From be4196c998edc6294aa849056df52d106ddbb9d7 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 17 Sep 2024 20:19:19 -0400 Subject: [PATCH 14/16] Update wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandGenericHID.java Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> --- .../wpi/first/wpilibj2/command/button/CommandGenericHID.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 fa9b0100578..542faefac35 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 @@ -281,7 +281,8 @@ public Trigger axisMagnitudeGreaterThan(int axis, double threshold, EventLoop lo /** * Constructs a Trigger instance that is true when the axis magnitude value is greater than {@code - * threshold}, attached to the given loop. + * 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. From ffe0eab664a53824d26cd898c8bcabf8f50f9527 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 17 Sep 2024 20:21:26 -0400 Subject: [PATCH 15/16] format fix --- .../native/cpp/frc2/command/button/CommandGenericHID.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 8ddfdf12f05..b7a8cbe6618 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/button/CommandGenericHID.cpp @@ -76,9 +76,9 @@ Trigger CommandGenericHID::AxisGreaterThan(int axis, double threshold, } Trigger CommandGenericHID::AxisMagnitudeGreaterThan( - int axis, double deadband, frc::EventLoop* loop) const { - return Trigger(loop, [this, axis, deadband]() { - return std::abs(m_hid.GetRawAxis(axis)) > deadband; + int axis, double threshold, frc::EventLoop* loop) const { + return Trigger(loop, [this, axis, threshold]() { + return std::abs(m_hid.GetRawAxis(axis)) > threshold; }); } From e72f25c33cf792863535ec8dee691901e6170f1b Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 17 Sep 2024 20:29:17 -0400 Subject: [PATCH 16/16] rename fix --- .../wpi/first/wpilibj2/command/button/CommandGenericHID.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 542faefac35..392471fb020 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 @@ -288,7 +288,7 @@ public Trigger axisMagnitudeGreaterThan(int axis, double threshold, EventLoop lo * @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 axisActive(int axis, double threshold) { + public Trigger axisMagnitudeGreaterThan(int axis, double threshold) { return axisMagnitudeGreaterThan( axis, threshold, CommandScheduler.getInstance().getDefaultButtonLoop()); }