From c1bde3ba94e767f1d5fd606498708f8ce6fdcc47 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Sat, 14 Dec 2024 02:10:43 -0800 Subject: [PATCH] Joystick deadzones and toggle condition (#144) Co-authored-by: Ryan Cahoon --- .../com/team766/framework3/Conditions.java | 43 +++++++-------- .../java/com/team766/hal/JoystickReader.java | 28 ++++++++++ .../com/team766/hal/mock/MockJoystick.java | 25 ++++++++- .../java/com/team766/hal/wpilib/Joystick.java | 25 ++++++++- src/main/java/com/team766/math/Math.java | 4 ++ src/test/java/com/team766/TestCase3.java | 2 +- .../team766/framework3/ConditionsTest.java | 46 ++++++++++++++++ .../com/team766/hal/JoystickAbstractTest.java | 52 +++++++++++++++++++ .../team766/hal/mock/MockJoystickTest.java | 18 +++++++ .../hal/wpilib/WpilibJoystickTest.java | 24 +++++++++ 10 files changed, 240 insertions(+), 27 deletions(-) create mode 100644 src/test/java/com/team766/framework3/ConditionsTest.java create mode 100644 src/test/java/com/team766/hal/JoystickAbstractTest.java create mode 100644 src/test/java/com/team766/hal/mock/MockJoystickTest.java create mode 100644 src/test/java/com/team766/hal/wpilib/WpilibJoystickTest.java diff --git a/src/main/java/com/team766/framework3/Conditions.java b/src/main/java/com/team766/framework3/Conditions.java index bc032c62a..923023d0b 100644 --- a/src/main/java/com/team766/framework3/Conditions.java +++ b/src/main/java/com/team766/framework3/Conditions.java @@ -1,6 +1,5 @@ package com.team766.framework3; -import com.team766.hal.JoystickReader; import java.util.function.BooleanSupplier; /** @@ -45,34 +44,30 @@ public AwaitRequest(Class clazz, Request request) { } } - // TODO: move this to a more suitable location - public static class JoystickAxisWithDeadzone { - private final JoystickReader joystick; - private final int axis; - private final double deadzone; - - public JoystickAxisWithDeadzone(JoystickReader joystick, int axis, double deadzone) { - this.joystick = joystick; - this.axis = axis; - this.deadzone = deadzone; - } - - public double getAxis() { - double rawValue = joystick.getAxis(axis); - return (rawValue > deadzone) ? rawValue : 0.0; - } - } - - public static class JoystickMoved implements BooleanSupplier { - private JoystickAxisWithDeadzone axis; + /** + * This predicate toggles its value (false -> true, or true -> false) whenever the provided + * predicate changes from false to true (rising edge). Otherwise, it retains its previous value. + * + * This is useful when you want a joystick button to switch between two different modes whenever + * it is pushed (pass `() -> joystick.getButton()` as the constructor argument). + */ + public static final class Toggle implements BooleanSupplier { + private final BooleanSupplier predicate; + private boolean predicatePrevious = false; + private boolean toggleValue = false; - public JoystickMoved(JoystickAxisWithDeadzone axis) { - this.axis = axis; + public Toggle(BooleanSupplier predicate) { + this.predicate = predicate; } @Override public boolean getAsBoolean() { - return axis.getAxis() > 0.0; + final var current = predicate.getAsBoolean(); + if (current && !predicatePrevious) { + toggleValue = !toggleValue; + } + predicatePrevious = current; + return toggleValue; } } diff --git a/src/main/java/com/team766/hal/JoystickReader.java b/src/main/java/com/team766/hal/JoystickReader.java index f5cd830be..37a0acd24 100755 --- a/src/main/java/com/team766/hal/JoystickReader.java +++ b/src/main/java/com/team766/hal/JoystickReader.java @@ -4,11 +4,39 @@ public interface JoystickReader { /** * Get the value of the axis. * + * If a deadzone has been set for this axis, the returned value will be 0 if the value would be + * smaller than the size of the deadzone. + * * @param axis The axis to read, starting at 0. * @return The value of the axis. */ double getAxis(int axis); + /** + * Get whether the axis has an absolute value greater than or equal to the deadzone. + * + * @param axis The axis to read, starting at 0. + * @return True if the axis value is larger than or equal to the deadzone, else false. + */ + boolean isAxisMoved(int axis); + + /** + * Set the size of the deadzone for the given axis. + * + * @param axis The axis to read, starting at 0. + * @param deadzone The size of the deadzone. 0 disables the deadzone. + */ + void setAxisDeadzone(int axis, double deadzone); + + /** + * Set the size of the deadzone for all axes (overriding any previous calls to setAxisDeadzone). + * + * Deadzones for individual axes can be overridden by calling setAxisDeadzone. + * + * @param deadzone The size of the deadzone. 0 disables the deadzone. + */ + void setAllAxisDeadzone(double deadzone); + /** * Get the button value (starting at button 1) * diff --git a/src/main/java/com/team766/hal/mock/MockJoystick.java b/src/main/java/com/team766/hal/mock/MockJoystick.java index caa1859e6..f727b3613 100755 --- a/src/main/java/com/team766/hal/mock/MockJoystick.java +++ b/src/main/java/com/team766/hal/mock/MockJoystick.java @@ -1,6 +1,9 @@ package com.team766.hal.mock; import com.team766.hal.JoystickReader; +import com.team766.math.Math; +import java.util.HashMap; +import java.util.Map; public class MockJoystick implements JoystickReader { @@ -9,6 +12,9 @@ public class MockJoystick implements JoystickReader { private boolean[] prevButtonValues; private int povValue; + private final Map axisDeadzoneMap = new HashMap<>(); + private double defaultAxisDeadzone = 0.0; + public MockJoystick() { axisValues = new double[12]; buttonValues = new boolean[20]; @@ -17,7 +23,24 @@ public MockJoystick() { @Override public double getAxis(final int axis) { - return axisValues[axis]; + return Math.deadzone( + axisValues[axis], axisDeadzoneMap.getOrDefault(axis, defaultAxisDeadzone)); + } + + @Override + public boolean isAxisMoved(int axis) { + return getAxis(axis) >= axisDeadzoneMap.getOrDefault(axis, defaultAxisDeadzone); + } + + @Override + public void setAxisDeadzone(int axis, double deadzone) { + axisDeadzoneMap.put(axis, deadzone); + } + + @Override + public void setAllAxisDeadzone(double deadzone) { + axisDeadzoneMap.clear(); + defaultAxisDeadzone = deadzone; } @Override diff --git a/src/main/java/com/team766/hal/wpilib/Joystick.java b/src/main/java/com/team766/hal/wpilib/Joystick.java index 6462e26cf..b0a790905 100755 --- a/src/main/java/com/team766/hal/wpilib/Joystick.java +++ b/src/main/java/com/team766/hal/wpilib/Joystick.java @@ -1,15 +1,38 @@ package com.team766.hal.wpilib; import com.team766.hal.JoystickReader; +import com.team766.math.Math; +import java.util.HashMap; +import java.util.Map; public class Joystick extends edu.wpi.first.wpilibj.Joystick implements JoystickReader { + private final Map axisDeadzoneMap = new HashMap<>(); + private double defaultAxisDeadzone = 0.0; + public Joystick(final int port) { super(port); } @Override public double getAxis(final int axis) { - return getRawAxis(axis); + return Math.deadzone( + getRawAxis(axis), axisDeadzoneMap.getOrDefault(axis, defaultAxisDeadzone)); + } + + @Override + public boolean isAxisMoved(int axis) { + return getAxis(axis) >= axisDeadzoneMap.getOrDefault(axis, defaultAxisDeadzone); + } + + @Override + public void setAxisDeadzone(int axis, double deadzone) { + axisDeadzoneMap.put(axis, deadzone); + } + + @Override + public void setAllAxisDeadzone(double deadzone) { + axisDeadzoneMap.clear(); + defaultAxisDeadzone = deadzone; } @Override diff --git a/src/main/java/com/team766/math/Math.java b/src/main/java/com/team766/math/Math.java index 8a475412d..d2bec03fa 100644 --- a/src/main/java/com/team766/math/Math.java +++ b/src/main/java/com/team766/math/Math.java @@ -13,6 +13,10 @@ public static double clamp(final double x, final double min, final double max) { return x; } + public static double deadzone(double value, double deadzone) { + return java.lang.Math.abs(value) >= deadzone ? value : 0; + } + /** * Returns the given angle, normalized to be within the range [-180, 180) */ diff --git a/src/test/java/com/team766/TestCase3.java b/src/test/java/com/team766/TestCase3.java index c80178dd4..d368b9cd4 100644 --- a/src/test/java/com/team766/TestCase3.java +++ b/src/test/java/com/team766/TestCase3.java @@ -21,7 +21,7 @@ public abstract class TestCase3 { @BeforeEach - public void setUp() { + public final void setUpFramework() { assert HAL.initialize(500, 0); setRobotEnabled(true); diff --git a/src/test/java/com/team766/framework3/ConditionsTest.java b/src/test/java/com/team766/framework3/ConditionsTest.java new file mode 100644 index 000000000..12a0b7627 --- /dev/null +++ b/src/test/java/com/team766/framework3/ConditionsTest.java @@ -0,0 +1,46 @@ +package com.team766.framework3; + +import static org.junit.jupiter.api.Assertions.*; + +import java.util.function.BooleanSupplier; +import org.junit.jupiter.api.Test; + +public class ConditionsTest { + private static class ValueProxy implements BooleanSupplier { + boolean value = false; + + public boolean getAsBoolean() { + return value; + } + } + + @Test + public void testToggle() { + var v = new ValueProxy(); + + var t = new Conditions.Toggle(v); + + assertFalse(t.getAsBoolean()); + assertFalse(t.getAsBoolean()); + + v.value = true; + + assertTrue(t.getAsBoolean()); + assertTrue(t.getAsBoolean()); + + v.value = false; + + assertTrue(t.getAsBoolean()); + assertTrue(t.getAsBoolean()); + + v.value = true; + + assertFalse(t.getAsBoolean()); + assertFalse(t.getAsBoolean()); + + v.value = false; + + assertFalse(t.getAsBoolean()); + assertFalse(t.getAsBoolean()); + } +} diff --git a/src/test/java/com/team766/hal/JoystickAbstractTest.java b/src/test/java/com/team766/hal/JoystickAbstractTest.java new file mode 100644 index 000000000..621d29fe0 --- /dev/null +++ b/src/test/java/com/team766/hal/JoystickAbstractTest.java @@ -0,0 +1,52 @@ +package com.team766.hal; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import com.team766.TestCase3; +import org.junit.jupiter.api.Test; + +public abstract class JoystickAbstractTest extends TestCase3 { + protected JoystickReader joystick; + + protected abstract void setAxis(int axis, double value); + + @Test + public void testDeadzone() { + // Deadzone should start at 0.0, so the condition should be true even if the value is 0. + setAxis(0, 0.0); + setAxis(1, 0.0); + assertEquals(0.0, joystick.getAxis(0)); + assertEquals(0.0, joystick.getAxis(1)); + assertTrue(joystick.isAxisMoved(0)); + assertTrue(joystick.isAxisMoved(1)); + + // Same result if the deadzone is explicitly set to 0.0. + joystick.setAllAxisDeadzone(0.0); + assertTrue(joystick.isAxisMoved(0)); + assertTrue(joystick.isAxisMoved(1)); + + // Test with the deadzone larger than the axis values. + joystick.setAllAxisDeadzone(0.6); + assertFalse(joystick.isAxisMoved(0)); + assertFalse(joystick.isAxisMoved(1)); + + // Calling setAxisDeadzone after setAllAxisDeadzone should set the deadzone for that axis + // but maintain the deadzone for all other axes. + setAxis(0, 0.5); + setAxis(1, 0.3); + joystick.setAxisDeadzone(1, 0.2); + assertEquals(0.0, joystick.getAxis(0)); + assertEquals(0.3, joystick.getAxis(1)); + assertFalse(joystick.isAxisMoved(0)); + assertTrue(joystick.isAxisMoved(1)); + + // Calling setAllAxisDeadzone should override previously-set per-axis deadzones. + joystick.setAllAxisDeadzone(0.5); + assertEquals(0.5, joystick.getAxis(0)); + assertEquals(0.0, joystick.getAxis(1)); + assertTrue(joystick.isAxisMoved(0)); + assertFalse(joystick.isAxisMoved(1)); + } +} diff --git a/src/test/java/com/team766/hal/mock/MockJoystickTest.java b/src/test/java/com/team766/hal/mock/MockJoystickTest.java new file mode 100644 index 000000000..b7e6aa036 --- /dev/null +++ b/src/test/java/com/team766/hal/mock/MockJoystickTest.java @@ -0,0 +1,18 @@ +package com.team766.hal.mock; + +import com.team766.hal.JoystickAbstractTest; +import org.junit.jupiter.api.BeforeEach; + +public class MockJoystickTest extends JoystickAbstractTest { + private MockJoystick driver; + + @BeforeEach + public void setUp() { + joystick = driver = new MockJoystick(); + } + + @Override + protected void setAxis(int axis, double value) { + driver.setAxisValue(axis, value); + } +} diff --git a/src/test/java/com/team766/hal/wpilib/WpilibJoystickTest.java b/src/test/java/com/team766/hal/wpilib/WpilibJoystickTest.java new file mode 100644 index 000000000..5dfdbd0c6 --- /dev/null +++ b/src/test/java/com/team766/hal/wpilib/WpilibJoystickTest.java @@ -0,0 +1,24 @@ +package com.team766.hal.wpilib; + +import com.team766.hal.JoystickAbstractTest; +import edu.wpi.first.wpilibj.simulation.JoystickSim; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Disabled; + +// TODO: AdvantageKit breaks this test. Try re-enabling this after upgrade to 2025. +@Disabled +public class WpilibJoystickTest extends JoystickAbstractTest { + private JoystickSim driver; + + @BeforeEach + public void setUp() { + joystick = new Joystick(0); + driver = new JoystickSim(0); + } + + @Override + protected void setAxis(int axis, double value) { + driver.setRawAxis(axis, value); + updateDriverStationData(); + } +}