Skip to content
This repository has been archived by the owner on Nov 28, 2023. It is now read-only.

Commit

Permalink
Fix critical issue with wheel encoder process
Browse files Browse the repository at this point in the history
- The process approach cannot rely on ValueError since the FSM will
  never see such an exception raised
- Use an additional fault value to indicate when a fault has happened
  • Loading branch information
taesungh committed Jun 3, 2023
1 parent 28921ef commit b8d774b
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 6 deletions.
19 changes: 14 additions & 5 deletions pod-control/src/components/process_encoder.py
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,10 @@ class WheelEncoder:
"""

def __init__(
self, counter_value: Synchronized[int], speed_value: Synchronized[float]
self,
counter_value: Synchronized[int],
speed_value: Synchronized[float],
fault_value: Synchronized[bool],
) -> None:
GPIO.setmode(GPIO.BCM)
GPIO.setup((PIN_ENCODER_A, PIN_ENCODER_B), GPIO.IN)
Expand All @@ -45,6 +48,7 @@ def __init__(

self._counter = counter_value
self._speed = speed_value
self._fault = fault_value
self.reset()
log.info("Process encoder setup complete.")

Expand All @@ -64,7 +68,7 @@ def measure(self) -> None:

if inc == 2:
log.error("WHEEL ENCODER FAULT", state, self._last_state)
raise ValueError
self._fault.value = True

if inc != 0:
log.debug("counter: ", self._counter.value, current_time - self._start_time)
Expand All @@ -85,9 +89,11 @@ def __del__(self) -> None:


def wheel_encoder_process(
counter_value: Synchronized[int], speed_value: Synchronized[float]
counter_value: Synchronized[int],
speed_value: Synchronized[float],
fault_value: Synchronized[bool],
) -> None:
wheel_encoder = WheelEncoder(counter_value, speed_value)
wheel_encoder = WheelEncoder(counter_value, speed_value, fault_value)
while True:
wheel_encoder.measure()
time.sleep(0)
Expand All @@ -98,6 +104,9 @@ def wheel_encoder_process(

counter_value = Value("i", 0)
speed_value = Value("d", 0.0)
p = Process(target=wheel_encoder_process, args=(counter_value, speed_value))
fault_value = Value("b", False)
p = Process(
target=wheel_encoder_process, args=(counter_value, speed_value, fault_value)
)
p.start()
p.join()
12 changes: 11 additions & 1 deletion pod-control/src/fsm.py
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,7 @@ def __init__(self, socket_server: PodSocketServer):
self._brakes.engage()
self._wheel_encoder_counter = Value("i", 0)
self._wheel_encoder_speed = Value("d", 0.0)
self._wheel_encoder_fault = Value("b", 0)
self._pt_downstream = PressureTransducer(ADDRESS_PT_DOWNSTREAM)
self._motors = Motors()
self._signal_light = SignalLight()
Expand All @@ -83,7 +84,11 @@ async def run(self) -> None:
"""Tick the state machine by loop."""
p = Process(
target=wheel_encoder_process,
args=(self._wheel_encoder_counter, self._wheel_encoder_speed),
args=(
self._wheel_encoder_counter,
self._wheel_encoder_speed,
self._wheel_encoder_fault,
),
)
p.start()

Expand Down Expand Up @@ -137,6 +142,11 @@ def _enter_running(self) -> None:
def _running_periodic(self) -> State:
"""Perform operations when the pod is running."""
self._running_tick += 1

if self._wheel_encoder_fault.value:
log.error("Wheel encoder faulted")
return State.STOPPED

self._motors.drive(20)

asyncio.create_task(
Expand Down

0 comments on commit b8d774b

Please sign in to comment.