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) )