Repository: robotpy/robotpy-wpilib-utilities

Issue:
Add simulation support for distance sensors
Instead of just displaying an analog input, use a sim device.

Comments:

Patch:
diff --git a/robotpy_ext/common_drivers/distance_sensors.py b/robotpy_ext/common_drivers/distance_sensors.py
index 2255a6a..071e5b4 100644
--- a/robotpy_ext/common_drivers/distance_sensors.py
+++ b/robotpy_ext/common_drivers/distance_sensors.py
@@ -70,7 +70,7 @@ class SharpIR2Y0A21:
         return max(min(d, 80.0), 10.0)
 
 
-class SharpIRGP2Y0A41SK0F:
+class SharpIR2Y0A41:
     """
     Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring
     distances from 4cm to 40cm. Output distance is measured in
@@ -102,3 +102,7 @@ class SharpIRGP2Y0A41SK0F:
 
         # Constrain output
         return max(min(d, 35.0), 4.5)
+
+
+# backwards compat
+SharpIRGP2Y0A41SK0F = SharpIR2Y0A41
diff --git a/robotpy_ext/common_drivers/distance_sensors_sim.py b/robotpy_ext/common_drivers/distance_sensors_sim.py
new file mode 100644
index 0000000..1102736
--- /dev/null
+++ b/robotpy_ext/common_drivers/distance_sensors_sim.py
@@ -0,0 +1,72 @@
+import math
+import wpilib
+
+from wpilib.simulation import AnalogInputSim
+
+from .distance_sensors import SharpIR2Y0A02, SharpIR2Y0A21, SharpIR2Y0A41
+
+
+class SharpIR2Y0A02Sim:
+    """
+    An easy to use simulation interface for a Sharp GP2Y0A02YK0F
+    """
+
+    def __init__(self, sensor: SharpIR2Y0A02) -> None:
+        assert isinstance(sensor, SharpIR2Y0A02)
+        self._sim = AnalogInputSim(sensor.distance)
+        self._distance = 0
+
+    def getDistance(self) -> float:
+        """Get set distance (not distance sensor sees) in centimeters"""
+        return self._distance
+
+    def setDistance(self, d) -> None:
+        """Set distance in centimeters"""
+        self._distance = d
+        d = max(min(d, 145.0), 22.5)
+        v = math.pow(d / 62.28, 1 / -1.092)
+        self._sim.setVoltage(v)
+
+
+class SharpIR2Y0A21Sim:
+    """
+    An easy to use simulation interface for a Sharp GP2Y0A21YK0F
+    """
+
+    def __init__(self, sensor: SharpIR2Y0A21) -> None:
+        assert isinstance(sensor, SharpIR2Y0A21)
+        self._sim = AnalogInputSim(sensor.distance)
+        self._distance = 0
+
+    def getDistance(self) -> float:
+        """Get set distance (not distance sensor sees) in centimeters"""
+        return self._distance
+
+    def setDistance(self, d) -> None:
+        """Set distance in centimeters"""
+        self._distance = d
+        d = max(min(d, 80.0), 10.0)
+        v = math.pow(d / 26.449, 1 / -1.226)
+        self._sim.setVoltage(v)
+
+
+class SharpIR2Y0A41Sim:
+    """
+    An easy to use simulation interface for a Sharp GP2Y0A41SK0F
+    """
+
+    def __init__(self, sensor: SharpIR2Y0A41) -> None:
+        assert isinstance(sensor, SharpIR2Y0A41)
+        self._sim = AnalogInputSim(sensor.distance)
+        self._distance = 0
+
+    def getDistance(self) -> float:
+        """Get set distance (not distance sensor sees) in centimeters"""
+        return self._distance
+
+    def setDistance(self, d) -> None:
+        """Set distance in centimeters"""
+        self._distance = d
+        d = max(min(d, 35.0), 4.5)
+        v = math.pow(d / 12.84, 1 / -0.9824)
+        self._sim.setVoltage(v)


Test patch:
diff --git a/tests/conftest.py b/tests/conftest.py
index 7532386..d5d3b94 100644
--- a/tests/conftest.py
+++ b/tests/conftest.py
@@ -36,3 +36,16 @@ def wpitime():
     yield FakeTime()
 
     hal.simulation.resumeTiming()
+
+
+@pytest.fixture(scope="function")
+def hal(wpitime):
+    import hal.simulation
+
+    yield
+
+    # Reset the HAL handles
+    hal.simulation.resetGlobalHandles()
+
+    # Reset the HAL data
+    hal.simulation.resetAllData()
diff --git a/tests/test_distance_sensors.py b/tests/test_distance_sensors.py
new file mode 100644
index 0000000..72dc2bf
--- /dev/null
+++ b/tests/test_distance_sensors.py
@@ -0,0 +1,79 @@
+from robotpy_ext.common_drivers.distance_sensors import (
+    SharpIR2Y0A02,
+    SharpIR2Y0A21,
+    SharpIR2Y0A41,
+)
+
+from robotpy_ext.common_drivers.distance_sensors_sim import (
+    SharpIR2Y0A02Sim,
+    SharpIR2Y0A21Sim,
+    SharpIR2Y0A41Sim,
+)
+
+import pytest
+
+
+def test_2Y0A02(hal):
+    sensor = SharpIR2Y0A02(0)
+    sim = SharpIR2Y0A02Sim(sensor)
+
+    # 22 to 145cm
+
+    # min
+    sim.setDistance(10)
+    assert sensor.getDistance() == pytest.approx(22.5)
+
+    # max
+    sim.setDistance(200)
+    assert sensor.getDistance() == pytest.approx(145)
+
+    # middle
+    sim.setDistance(50)
+    assert sensor.getDistance() == pytest.approx(50)
+
+    sim.setDistance(100)
+    assert sensor.getDistance() == pytest.approx(100)
+
+
+def test_2Y0A021(hal):
+    sensor = SharpIR2Y0A21(0)
+    sim = SharpIR2Y0A21Sim(sensor)
+
+    # 10 to 80cm
+
+    # min
+    sim.setDistance(5)
+    assert sensor.getDistance() == pytest.approx(10)
+
+    # max
+    sim.setDistance(100)
+    assert sensor.getDistance() == pytest.approx(80)
+
+    # middle
+    sim.setDistance(30)
+    assert sensor.getDistance() == pytest.approx(30)
+
+    sim.setDistance(60)
+    assert sensor.getDistance() == pytest.approx(60)
+
+
+def test_2Y0A041(hal):
+    sensor = SharpIR2Y0A41(0)
+    sim = SharpIR2Y0A41Sim(sensor)
+
+    # 4.5 to 35 cm
+
+    # min
+    sim.setDistance(2)
+    assert sensor.getDistance() == pytest.approx(4.5)
+
+    # max
+    sim.setDistance(50)
+    assert sensor.getDistance() == pytest.approx(35)
+
+    # middle
+    sim.setDistance(10)
+    assert sensor.getDistance() == pytest.approx(10)
+
+    sim.setDistance(25)
+    assert sensor.getDistance() == pytest.approx(25)