Skip to content

Fix SystemCommand Command_out getting clobbered same-tick by exec fallback #30

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
grahas wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fix SystemCommand Command_out getting clobbered same-tick by exec fallback #30

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 13 additions & 0 deletions parol6/server/controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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.
Expand Down
12 changes: 12 additions & 0 deletions parol6/server/state.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
)
Expand Down