From 5e0fb7d66355e992e22460db0924f6bd40d13b66 Mon Sep 17 00:00:00 2001 From: Graham Harison Date: Mon, 3 Aug 2026 01:59:10 -0600 Subject: [PATCH] Fix SystemCommand Command_out getting clobbered same-tick by exec fallback RESET (and any other one-shot SystemCommand that sets state.Command_out, e.g. to CommandCode.ENABLE) had its signal silently overwritten before it ever reached the firmware: _poll_commands() (which dispatches SystemCommands during the "poll_cmd" phase) runs before _execute_commands() (the "exec" phase) in the same control-loop tick, and _execute_commands()'s "nothing active" fallback unconditionally reset state.Command_out = CommandCode.IDLE whenever no segment/streaming command was active -- which is the case right after a plain RESET, since RESET itself doesn't queue any motion. The practical symptom: RESET appeared to succeed (state.enabled is pure Python state, set unconditionally), but PAROL6.disabled on the firmware never actually got cleared, because the ENABLE(101) command code set by ResetCommand.execute_step() never survived to _write_to_firmware(). Once PAROL6.disabled was latched from an earlier ESTOP, every subsequent HOME/JOG/MOVE was silently dropped by the firmware's `if (PAROL6.disabled == 0)` gate -- while the server-side planner/segment-player pipeline computed and "sent" a perfectly valid trajectory the whole time, believing it succeeded. Fixed with a same-tick lock flag (ControllerState.command_out_locked): set whenever a SystemCommand assigns a non-IDLE Command_out during poll_cmd, consumed by _execute_commands()'s fallback instead of blindly resetting to IDLE, and cleared fresh at the top of every _poll_commands() call. Verified against real hardware: home() on an unhomed-but-referenced robot now actually drives the arm to standby (confirmed via continuous status().angles polling during the move, and visually). Full test suite (90 tests, unit + integration) passes unchanged. --- parol6/server/controller.py | 13 +++++++++++++ parol6/server/state.py | 12 ++++++++++++ 2 files changed, 25 insertions(+) diff --git a/parol6/server/controller.py b/parol6/server/controller.py index ab250d4..f9864af 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -385,6 +385,11 @@ def _execute_commands(self, state: ControllerState) -> None: # Streaming command executor (jog/servo) if self._executor.active_command or self._executor.command_queue: self._executor.execute_active_command() + elif state.command_out_locked: + # A SystemCommand (e.g. RESET) set Command_out earlier this same + # tick during poll_cmd -- consume the lock instead of stomping + # it back to IDLE before _write_to_firmware() sees it. + state.command_out_locked = False else: state.Command_out = CommandCode.IDLE state.Speed_out.fill(0) @@ -591,6 +596,7 @@ def _poll_commands(self, state: ControllerState) -> None: """Poll and process UDP commands (non-blocking).""" assert self.udp_transport is not None + state.command_out_locked = False msgs = self.udp_transport.poll_receive_all(max_count=MAX_POLL_COUNT) for data, addr in msgs: self._process_command(data, addr, state) @@ -802,6 +808,13 @@ def _handle_system_command( command.setup(state) code = command.tick(state) + # This SystemCommand set a real signal (e.g. RESET's ENABLE) for + # firmware to see on this tick's write phase -- don't let + # _execute_commands()'s later "nothing active" fallback stomp it + # back to IDLE before _write_to_firmware() runs. + if state.Command_out != CommandCode.IDLE: + state.command_out_locked = True + # Stop/estop: cancel the motion pipeline, or the segment player # keeps playing the active trajectory (rewriting Command_out and # fresh speeds every tick) and the "stopped" robot drives on. diff --git a/parol6/server/state.py b/parol6/server/state.py index e5a8f03..49a7bd4 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -184,6 +184,18 @@ class ControllerState: # Robot telemetry and command buffers - using ndarray for efficiency Command_out: CommandCode = CommandCode.IDLE # The command code to send to firmware + # True for the remainder of the tick in which a SystemCommand (RESET's + # ENABLE, ESTOP/STOP's IDLE, etc.) explicitly set Command_out to a + # meaningful value during poll_cmd. Without this, _execute_commands()'s + # "nothing active" fallback (which also runs every tick, after poll_cmd) + # unconditionally overwrites Command_out back to IDLE before + # _write_to_firmware() ever sees the SystemCommand's signal -- so e.g. + # RESET's ENABLE(101) never actually reaches the firmware, leaving + # PAROL6.disabled latched from an earlier ESTOP forever. Reset to False + # at the top of every _poll_commands() call; consumed (and cleared) by + # _execute_commands()'s fallback the same tick it's set. + command_out_locked: bool = False + Position_out: np.ndarray = field( default_factory=lambda: np.zeros((6,), dtype=np.int32) )