From 429076e6f6e044afaafd6abe16b7c97b3a859ded Mon Sep 17 00:00:00 2001 From: Taesung Hwang Date: Thu, 1 Jun 2023 20:51:29 -0700 Subject: [PATCH] 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 --- .../src/services/PodSocketClient.ts | 3 +- .../src/views/Dashboard/Dashboard.tsx | 8 +- .../src/views/Dashboard/usePodData.tsx | 3 +- pod-control/src/components/process_encoder.py | 103 ++++++++++++++++++ pod-control/src/fsm.py | 37 ++++--- 5 files changed, 134 insertions(+), 20 deletions(-) create mode 100644 pod-control/src/components/process_encoder.py diff --git a/control-station/src/services/PodSocketClient.ts b/control-station/src/services/PodSocketClient.ts index 32d4215..e99b122 100644 --- a/control-station/src/services/PodSocketClient.ts +++ b/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>; diff --git a/control-station/src/views/Dashboard/Dashboard.tsx b/control-station/src/views/Dashboard/Dashboard.tsx index a13afed..127cef3 100644 --- a/control-station/src/views/Dashboard/Dashboard.tsx +++ b/control-station/src/views/Dashboard/Dashboard.tsx @@ -8,11 +8,9 @@ function Dashboard() {

Dashboard

- {Object.entries(podData).map(([key, value]) => ( -

- {key} - {value} -

- ))} +

downstream pressure - {podData.pressureDownstream}

+

wheel counter - {podData.wheelCounter}

+

wheel speed - {podData.wheelSpeed.toFixed(2)}

diff --git a/control-station/src/views/Dashboard/usePodData.tsx b/control-station/src/views/Dashboard/usePodData.tsx index 9d891fd..3c73166 100644 --- a/control-station/src/views/Dashboard/usePodData.tsx +++ b/control-station/src/views/Dashboard/usePodData.tsx @@ -4,8 +4,9 @@ import PodSocketClient, { PodData } from "services/PodSocketClient"; function usePodData() { const [podData, setPodData] = useState({ tick: 0, - wheel: 0, pressureDownstream: 0.0, + wheelCounter: 0, + wheelSpeed: 0.0, }); const podSocketClient = useMemo(() => new PodSocketClient(setPodData), []); diff --git a/pod-control/src/components/process_encoder.py b/pod-control/src/components/process_encoder.py new file mode 100644 index 0000000..bfd3584 --- /dev/null +++ b/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() diff --git a/pod-control/src/fsm.py b/pod-control/src/fsm.py index cde1c48..4826d31 100644 --- a/pod-control/src/fsm.py +++ b/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