diff --git a/body/nox_daemon.py b/body/nox_daemon.py index a13c34d..f81e260 100644 --- a/body/nox_daemon.py +++ b/body/nox_daemon.py @@ -148,6 +148,41 @@ def _is_sleep_hours(): +def _release_servos(wait_s=3.0): + """Let every servo go limp: pulse width 0 on all channels. + + The robot HAT's MCU keeps sending the last pulse after we stop writing + angles, so "servos off" used to be a flag only. A sleeping dog held every + joint under load, ran warm, and hummed against the hand holding it, even + with all Nox services stopped. The next angle the SDK writes engages the + servo again, so any move wakes it. Call with the dog lying down: it sags. + Returns the number of channels released. + """ + deadline = time.time() + wait_s + if _action_buffer_depth() > 0: + while _action_buffer_depth() > 0 and time.time() < deadline: + time.sleep(0.1) # let a queued "lie" finish first + time.sleep(0.3) # the last frame is still being written + released = 0 + for part in ("legs", "head", "tail"): + for servo in getattr(getattr(dog, part, None), "servo_list", None) or []: + try: + servo.pulse_width(0) + released += 1 + except Exception as e: + print(f"[nox] Servo release failed ({part}): {e}", flush=True) + return released + + +def _servos_reengaged(): + """A move writes angles, which engages released servos again. Back to + "resting" so the watchdog releases them once more instead of believing + they are still asleep (behavior-engine moves don't reset the timer).""" + global _idle_state, _servo_pwm_disabled + if _idle_state == "sleeping": + _idle_state, _servo_pwm_disabled = "resting", False + + def _mark_activity(internal=False): global _last_activity, _idle_state, _servo_pwm_disabled if internal: @@ -178,6 +213,8 @@ def _idle_watchdog(): print(f"[nox] Idle {int(elapsed)}s → deep sleep (servos off, LEDs dimmed)", flush=True) try: with dog_lock: + n = _release_servos() + print(f"[nox] {n} servos released (no holding current)", flush=True) dog.rgb_strip.set_mode("breath", [0, 0, 0] if _is_sleep_hours() else [0, 0, 15], bps=0.15) except Exception as e: print(f"[nox] Deep sleep failed: {e}", flush=True) @@ -406,9 +443,7 @@ def cmd_move(action, steps=3, speed=None, internal=False): if speed is None: speed = WALK_SPEED if action in GAIT_ACTIONS else 80 _mark_activity(internal=internal) - if _idle_state == "sleeping": - # Re-init servos before moving - pass # PiDog re-enables on do_action + _servos_reengaged() with dog_lock: # SDK do_action() swallows unknown actions (prints and returns), so we # must route ourselves. Probe the CLASS for a property: instance @@ -830,12 +865,15 @@ def cmd_wake(): def cmd_sleep(): - """Sleep sequence.""" + """Sleep sequence: lie down, then let the servos go limp.""" + global _idle_state, _servo_pwm_disabled with dog_lock: dog.do_action('lie', speed=50) time.sleep(1) + released = _release_servos() dog.rgb_strip.set_mode('breath', [0, 0, 0] if _is_sleep_hours() else [0, 0, 80], bps=0.3) - return {"ok": True} + _idle_state, _servo_pwm_disabled = "sleeping", True + return {"ok": True, "servos_released": released} def cmd_reset(): @@ -1036,12 +1074,16 @@ def cmd_emergency_stop(): dog.do_action("lie", speed=100) except Exception: pass + released = _release_servos() # no holding force left to fight anything try: dog.rgb_strip.set_mode("boom", [255, 0, 0], bps=2.0) except Exception: pass + # "resting", not "sleeping": the behavior engine sends this itself, and + # its follow-up head moves (internal, they cannot wake the daemon) would + # be refused as "servos sleeping" forever. The watchdog sleeps it later. _idle_state = "resting" - return {"ok": True, "emergency": True, "motion": drained} + return {"ok": True, "emergency": True, "motion": drained, "servos_released": released} def cmd_stop_motion(): @@ -1260,6 +1302,7 @@ def shutdown(signum, frame): dog.rgb_strip.set_mode('monochromatic', [0, 0, 0]) dog.do_action('lie', speed=50) time.sleep(1) + _release_servos() # or they hold — and hum — until power-off dog.close() except: pass diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 0000000..b0241ba --- /dev/null +++ b/tests/conftest.py @@ -0,0 +1,14 @@ +"""Shared test setup.""" +import pytest + + +@pytest.fixture(autouse=True) +def _no_quiet_hours(monkeypatch): + """The behavior engine's quiet hours (22-7) follow the wall clock — without + this, engine tests fail whenever the suite runs at night. Tests about + quiet hours patch them explicitly.""" + try: + import body.nox_behavior_engine as be + except Exception: + return + monkeypatch.setattr(be, "QUIET_HOURS", None) diff --git a/tests/test_servo_release.py b/tests/test_servo_release.py new file mode 100644 index 0000000..452d9c2 --- /dev/null +++ b/tests/test_servo_release.py @@ -0,0 +1,42 @@ +"""'Servos off' must cut the PWM signal (field: a sleeping dog hummed in the +hand with every Nox service stopped). Source-level: the daemon needs the SDK.""" +import re + +SRC = open("body/nox_daemon.py").read() + + +def _body(name): + m = re.search(rf"\ndef {name}\(.*?(?=\ndef |\nif __name__)", SRC, re.S) + assert m, name + return m.group(0) + + +def test_release_writes_pulse_width_zero_on_every_part(): + body = _body("_release_servos") + assert "pulse_width(0)" in body + assert '("legs", "head", "tail")' in body + assert "_action_buffer_depth()" in body # a queued "lie" finishes first + + +def test_deep_sleep_sleep_emergency_and_shutdown_release(): + assert "_release_servos()" in _body("_idle_watchdog") + assert "_release_servos()" in _body("cmd_sleep") + assert "_release_servos()" in _body("cmd_emergency_stop") + assert "_release_servos()" in _body("shutdown") + + +def test_release_comes_after_lying_down(): + for name in ("cmd_sleep", "cmd_emergency_stop", "shutdown"): + body = _body(name) + assert body.index("lie") < body.index("_release_servos()"), name + + +def test_an_emergency_stop_does_not_lock_out_the_engines_head_moves(): + body = _body("cmd_emergency_stop") + assert '_idle_state = "resting"' in body and "_servo_pwm_disabled" not in body.split("released =")[1] + + +def test_a_move_after_release_lets_the_watchdog_release_again(): + assert "_servos_reengaged()" in _body("cmd_move") + body = _body("_servos_reengaged") + assert '"resting", False' in body