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 b9ee710f978..e0919d96e36 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.Pair; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -18,7 +19,7 @@ public class CommandGenericHID { private final GenericHID m_hid; private final Map> m_buttonCache = new HashMap<>(); - private final Map> m_axisCache = new HashMap<>(); + private final Map, Trigger>> m_axisCache = new HashMap<>(); private final Map> m_povCache = new HashMap<>(); /** @@ -223,7 +224,7 @@ public Trigger axisLessThan(int axis, double threshold) { public Trigger axisLessThan(int axis, double threshold, EventLoop loop) { var cache = m_axisCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( - axis * 100 - threshold, k -> new Trigger(loop, () -> getRawAxis(axis) < threshold)); + new Pair<>(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) < threshold)); } /** @@ -253,7 +254,7 @@ public Trigger axisGreaterThan(int axis, double threshold) { public Trigger axisGreaterThan(int axis, double threshold, EventLoop loop) { var cache = m_axisCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( - axis * 100 + threshold, k -> new Trigger(loop, () -> getRawAxis(axis) > threshold)); + new Pair<>(axis, threshold), k -> new Trigger(loop, () -> getRawAxis(axis) > threshold)); } /** 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 dccbc14a5f9..c5992de9a92 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java @@ -6,6 +6,7 @@ import edu.wpi.first.hal.DriverStationJNI; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Pair; import edu.wpi.first.wpilibj.event.BooleanEvent; import edu.wpi.first.wpilibj.event.EventLoop; import java.util.HashMap; @@ -98,7 +99,8 @@ public static HIDType of(int value) { private int m_leftRumble; private int m_rightRumble; private final Map> m_buttonCache = new HashMap<>(); - private final Map> m_axisCache = new HashMap<>(); + private final Map, BooleanEvent>> m_axisCache = + new HashMap<>(); private final Map> m_povCache = new HashMap<>(); /** @@ -344,7 +346,8 @@ public BooleanEvent povCenter(EventLoop loop) { public BooleanEvent axisLessThan(int axis, double threshold, EventLoop loop) { var cache = m_axisCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( - axis * 100 - threshold, k -> new BooleanEvent(loop, () -> getRawAxis(axis) < threshold)); + new Pair<>(axis, -threshold), + k -> new BooleanEvent(loop, () -> getRawAxis(axis) < threshold)); } /** @@ -360,7 +363,8 @@ public BooleanEvent axisLessThan(int axis, double threshold, EventLoop loop) { public BooleanEvent axisGreaterThan(int axis, double threshold, EventLoop loop) { var cache = m_axisCache.computeIfAbsent(loop, k -> new HashMap<>()); return cache.computeIfAbsent( - axis * 100 + threshold, k -> new BooleanEvent(loop, () -> getRawAxis(axis) > threshold)); + new Pair<>(axis, threshold), + k -> new BooleanEvent(loop, () -> getRawAxis(axis) > threshold)); } /**