[package]
name = "servo-controller-py"
version = "0.1.0"
description = "Multi-servo bus controller with safe shutdown"
language = "python"

#!/usr/bin/env python3
"""Multi-servo controller with joint limits and safe shutdown."""

import math
import horus
from horus import Node, ServoCommand, JointState, us, ms

# ── Configuration ────────────────────────────────────────────

NUM_SERVOS = 6
HOME_POSITION = 0.0                        # radians — safe resting position
JOINT_LIMIT = math.pi                      # +/- pi radians
TEMP_WARNING = 70.0                        # celsius
SERVO_NAMES = [f"joint_{i}" for i in range(NUM_SERVOS)]

# ── State ────────────────────────────────────────────────────

positions = [HOME_POSITION] * NUM_SERVOS
velocities = [0.0] * NUM_SERVOS
temperatures = [35.0] * NUM_SERVOS

# ── Hardware stubs ───────────────────────────────────────────

def write_servo(servo_id, position):
    """Write position to a single servo — replace with real driver.

    For Dynamixel: use dynamixel_sdk
    For hobby PWM: use RPi.GPIO or pigpio
    """
    if 0 <= servo_id < NUM_SERVOS:
        positions[servo_id] = position

def read_feedback():
    """Read feedback from all servos — replace with real driver."""
    return positions[:], velocities[:], temperatures[:]

# ── Node callbacks ───────────────────────────────────────────

def servo_tick(node):
    # IMPORTANT: always recv() every tick to drain the buffer
    cmd = node.recv("servo.command")
    if cmd is not None:
        # SAFETY: enforce joint limits to prevent mechanical damage
        clamped_pos = max(-JOINT_LIMIT, min(JOINT_LIMIT, cmd.position))
        write_servo(cmd.servo_id, clamped_pos)

    # Read current state from all servos
    pos, vel, temp = read_feedback()

    # WARNING: check for overheating servos
    for i, t in enumerate(temp):
        if t > TEMP_WARNING:
            print(f"WARNING: servo {i} temperature {t:.1f}C exceeds limit")

    # Publish joint state feedback
    feedback = JointState(
        names=SERVO_NAMES,
        positions=pos,
        velocities=vel,
        efforts=[0.0] * NUM_SERVOS,
    )
    node.send("servo.feedback", feedback)

def servo_init(node):
    print(f"ServoController: initialized {NUM_SERVOS} servos")
    # Read current positions from hardware before accepting commands
    pos, _, _ = read_feedback()
    for i, p in enumerate(pos):
        positions[i] = p

def servo_shutdown(node):
    # SAFETY: return ALL servos to home position before exiting
    for i in range(NUM_SERVOS):
        write_servo(i, HOME_POSITION)

    feedback = JointState(
        names=SERVO_NAMES,
        positions=[HOME_POSITION] * NUM_SERVOS,
        velocities=[0.0] * NUM_SERVOS,
        efforts=[0.0] * NUM_SERVOS,
    )
    node.send("servo.feedback", feedback)
    print("ServoController: all servos returned to home position")

# ── Main ─────────────────────────────────────────────────────

servo_node = Node(
    name="ServoController",
    tick=servo_tick,
    init=servo_init,
    shutdown=servo_shutdown,
    rate=100,                      # 100 Hz servo update rate
    order=0,
    subs=["servo.command"],
    pubs=["servo.feedback"],
    budget=800 * us,               # 800 us budget for bus communication
    on_miss="warn",
)

if __name__ == "__main__":
    horus.run(servo_node)

[HORUS] Scheduler running — tick_rate: 1000 Hz
ServoController: initialized 6 servos
[HORUS] Node "ServoController" started (100 Hz)
^C
ServoController: all servos returned to home position
[HORUS] Shutting down...
[HORUS] Node "ServoController" shutdown complete