Convert wheel encoder to multiprocessing design

- The main FSM tick cannot sample the wheel encoder fast enough
- Use multiprocessing for process-based parallelism to concurrently
  sample the wheel encoder at a faster rate to avoid skipping samples

control-station/src/services/PodSocketClient.ts

@@ -17,8 +17,9 @@ interface ClientToServerEvents {
 
 export interface PodData {
 	tick: number;
-	wheel: number;
 	pressureDownstream: number;
+	wheelCounter: number;
+	wheelSpeed: number;
 }
 
 type SetPodData = Dispatch<SetStateAction<PodData>>;

control-station/src/views/Dashboard/Dashboard.tsx

@@ -8,11 +8,9 @@ function Dashboard() {
 			<h1>Dashboard</h1>
 			<button onClick={() => podSocketClient.sendPing()}>Send Ping</button>
 			<div>
-				{Object.entries(podData).map(([key, value]) => (
-					<p key={key}>
-						{key} - {value}
-					</p>
-				))}
+				<p>downstream pressure - {podData.pressureDownstream}</p>
+				<p>wheel counter - {podData.wheelCounter}</p>
+				<p>wheel speed - {podData.wheelSpeed.toFixed(2)}</p>
 			</div>
 			<button onClick={() => podSocketClient.sendService()}>service</button>
 			<button onClick={() => podSocketClient.sendStart()}>start</button>

control-station/src/views/Dashboard/usePodData.tsx

@@ -4,8 +4,9 @@ import PodSocketClient, { PodData } from "services/PodSocketClient";
 function usePodData() {
 	const [podData, setPodData] = useState<PodData>({
 		tick: 0,
-		wheel: 0,
 		pressureDownstream: 0.0,
+		wheelCounter: 0,
+		wheelSpeed: 0.0,
 	});
 
 	const podSocketClient = useMemo(() => new PodSocketClient(setPodData), []);

pod-control/src/components/process_encoder.py

@@ -0,0 +1,103 @@
+import time
+from logging import getLogger
+from multiprocessing import Value
+from multiprocessing.sharedctypes import Synchronized
+from typing import Literal, cast
+
+try:
+    import RPi.GPIO as GPIO
+except ImportError:
+    from fake_rpi.RPi import GPIO as GPIO
+
+log = getLogger(__name__)
+
+PIN_ENCODER_A = 14
+PIN_ENCODER_B = 15
+
+
+EncoderState = Literal[0, 1, 2, 3]
+EncoderDiff = Literal[-1, 0, 1, 2]
+
+
+def encode(a: bool, b: bool) -> EncoderState:
+    """Produce a two-bit gray code."""
+    return cast(EncoderState, (a << 1) + (a ^ b))
+
+
+def difference(p: EncoderState, q: EncoderState) -> EncoderDiff:
+    """Provide the difference in states in the range -1 to 2."""
+    return cast(EncoderDiff, (p - q + 1) % 4 - 1)
+
+
+class WheelEncoder:
+    """
+    Process based wheel encoder.
+    Note: the type system for multiprocessing is currently incomplete.
+    """
+
+    def __init__(
+        self, counter_value: Synchronized[int], speed_value: Synchronized[float]
+    ) -> None:
+        GPIO.setmode(GPIO.BCM)
+        GPIO.setup((PIN_ENCODER_A, PIN_ENCODER_B), GPIO.IN)
+
+        self._start_time = time.time()
+
+        self._counter = counter_value
+        self._speed = speed_value
+        self.reset()
+        log.info("Process encoder setup complete.")
+
+    def reset(self) -> None:
+        self._counter.value = 0
+        self._last_time = time.time()
+        self._last_state = self._read_state()
+
+    def measure(self) -> None:
+        current_time = time.time()
+        state = self._read_state()
+
+        delta_d = 1 / 16
+
+        calc = 0.0
+        inc = difference(state, self._last_state)
+
+        if inc == 2:
+            log.error("WHEEL ENCODER FAULT", state, self._last_state)
+            raise ValueError
+
+        if inc != 0:
+            log.debug("counter: ", self._counter.value, current_time - self._start_time)
+            delta_t = current_time - self._last_time
+            calc = inc * delta_d / delta_t
+            log.debug("Instantaneous Speed:", calc)
+            self._last_time = current_time
+
+            self._last_state = state
+            self._speed.value = calc
+            self._counter.value += inc
+
+    def _read_state(self) -> EncoderState:
+        return encode(GPIO.input(PIN_ENCODER_A), GPIO.input(PIN_ENCODER_B))
+
+    def __del__(self) -> None:
+        GPIO.cleanup((PIN_ENCODER_A, PIN_ENCODER_B))
+
+
+def wheel_encoder_process(
+    counter_value: Synchronized[int], speed_value: Synchronized[float]
+) -> None:
+    wheel_encoder = WheelEncoder(counter_value, speed_value)
+    while True:
+        wheel_encoder.measure()
+        time.sleep(0)
+
+
+if __name__ == "__main__":
+    from multiprocessing import Process
+
+    counter_value = Value("i", 0)
+    speed_value = Value("d", 0.0)
+    p = Process(target=wheel_encoder_process, args=(counter_value, speed_value))
+    p.start()
+    p.join()

pod-control/src/fsm.py

@@ -2,15 +2,16 @@
 from enum import Enum
 from logging import getLogger
 from math import pi
+from multiprocessing import Process, Value
 from typing import Callable, Coroutine, Mapping, Optional, Union
 
 from components.brakes import Brakes
 # from components.high_voltage_system import HighVoltageSystem
 from components.motors import Motors
 from components.pressure_transducer import PressureTransducer
+from components.process_encoder import wheel_encoder_process
 from components.signal_light import SignalLight
 
-from components.wheel_encoder import WheelEncoder
 from services.pod_socket_server import PodSocketServer
 
 log = getLogger(__name__)
@@ -72,16 +73,26 @@ def __init__(self, socket_server: PodSocketServer):
 
         self._brakes = Brakes()
         self._brakes.engage()
-        self._wheel_encoder = WheelEncoder()
+        self._wheel_encoder_counter = Value("i", 0)
+        self._wheel_encoder_speed = Value("d", 0.0)
         self._pt_downstream = PressureTransducer(ADDRESS_PT_DOWNSTREAM)
         self._motors = Motors()
         self._signal_light = SignalLight()
 
     async def run(self) -> None:
         """Tick the state machine by loop."""
-        while True:
-            self.tick()
-            await asyncio.sleep(0.001)
+        p = Process(
+            target=wheel_encoder_process,
+            args=(self._wheel_encoder_counter, self._wheel_encoder_speed),
+        )
+        p.start()
+
+        try:
+            while True:
+                self.tick()
+                await asyncio.sleep(0.001)
+        finally:
+            p.join()
 
     def tick(self) -> None:
         """Tick the state machine by running the action for the current state."""
@@ -129,17 +140,17 @@ def _running_periodic(self) -> State:
         """Perform operations when the pod is running."""
         self._running_tick += 1
 
-        try:
-            self._wheel_encoder.measure()
-        except ValueError:
-            log.error("Wheel encoder faulted")
-            # return State.STOPPED
-
         asyncio.create_task(
-            self.socket.emit_stats({"wheel": self._wheel_encoder.counter})
+            self.socket.emit_stats(
+                {
+                    "wheelCounter": self._wheel_encoder_counter.value,
+                    "wheelSpeed": self._wheel_encoder_speed.value,
+                }
+            )
         )
+        log.info(f"wheel encoder speed: {self._wheel_encoder_speed.value}")
 
-        if self._wheel_encoder.counter > STOP_THRESHOLD:
+        if self._wheel_encoder_counter.value > STOP_THRESHOLD:
             return State.STOPPED
 
         return State.RUNNING