Joystick deadzones and toggle condition (#144)

Co-authored-by: Ryan Cahoon <[email protected]>

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<S> clazz, Request<S> 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;
         }
     }
 

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)
      *

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<Integer, Double> 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

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<Integer, Double> 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

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)
      */

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);

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());
+    }
+}

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));
+    }
+}

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);
+    }
+}

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();
+    }
+}