From b5e9f7f2bfd6381fcd2167ce2e82a17e02f77072 Mon Sep 17 00:00:00 2001 From: danic85 <6583012+danic85@users.noreply.github.com> Date: Sun, 29 Mar 2026 15:07:38 +0100 Subject: [PATCH 1/4] Tested waveshare backend --- environments/cody.yml | 4 +- src/modules/actuators/bus_servo/bus_servo.py | 17 +++++- .../bus_servo/libraries/rustypot_backend.py | 57 ++++++++++++++----- src/modules/personality/personality.py | 6 +- 4 files changed, 62 insertions(+), 22 deletions(-) diff --git a/environments/cody.yml b/environments/cody.yml index 7c5f220b..9b2905db 100644 --- a/environments/cody.yml +++ b/environments/cody.yml @@ -19,6 +19,7 @@ bno055: bus_servo: enabled: true config: + backend: 'waveshare' poses: - legs_forward: {'leg_r_tilt': 246.7, 'leg_r_hip': 174.8, 'leg_r_knee': 118.6, 'leg_r_ankle': 242.0, 'neck_pan': 149.3, 'neck_tilt': 34.6, 'leg_l_tilt': 142.6, 'leg_l_hip': 90.2, 'leg_l_knee': 231.6, 'leg_l_ankle': 137.7} - sit: {'leg_r_tilt': 246.7, 'leg_r_hip': 181.3, 'leg_r_knee': 161.7, 'leg_r_ankle': 319.7, 'neck_pan': 149.2, 'neck_tilt': 34.6, 'leg_l_tilt': 142.8, 'leg_l_hip': 74.9, 'leg_l_knee': 189.7, 'leg_l_ankle': 53.9} @@ -29,6 +30,7 @@ bus_servo: - sit_edge: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 243.5, 'leg_r_ankle': 251.0, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.7, 'leg_l_knee': 117.1, 'leg_l_ankle': 137.2} - sit_edge_swing_l: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 243.5, 'leg_r_ankle': 251.0, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.9, 'leg_l_knee': 157.2, 'leg_l_ankle': 176.2} - sit_edge_swing_r: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 195.3, 'leg_r_ankle': 220.2, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.6, 'leg_l_knee': 118.2, 'leg_l_ankle': 134.7} + - stand_low_narrow: {'leg_r_tilt': 221.0989010989011, 'leg_r_hip': 38.417582417582416, 'leg_r_knee': 72.17582417582418, 'leg_r_ankle': 258.1978021978022, 'neck_pan': 150.24175824175825, 'neck_tilt': 34.604105571847505, 'leg_l_tilt': 160.26373626373626, 'leg_l_hip': 235.07692307692307, 'leg_l_knee': 284.65934065934067, 'leg_l_ankle': 107.86813186813187} instances: - name: "leg_r_tilt" model: 'ST3215' @@ -89,7 +91,7 @@ bus_servo: - name: "leg_l_tilt" model: 'ST3215' id: 7 - range: [0.7, 157.8] # converted from raw (4095 = 360 degrees) + range: [0.7, 161] # converted from raw (4095 = 360 degrees) start: 142.1 # calibrate_on_boot: true - name: "leg_l_hip" diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py index b15d9a0c..3a48072f 100644 --- a/src/modules/actuators/bus_servo/bus_servo.py +++ b/src/modules/actuators/bus_servo/bus_servo.py @@ -12,6 +12,7 @@ def __init__(self, **kwargs): Servo class """ self.backend = kwargs.get('backend', 'waveshare') + print(f"Creating servo with backend {self.backend}") self.identifier = kwargs.get('name') self.model = kwargs.get('model', 'ST') self.index = kwargs.get('id') @@ -162,10 +163,20 @@ def move_relative(self, delta): def is_moving(self): - if self.backend_servo.get_moving() == 1: + try: + moving = self.backend_servo.get_moving() + except Exception as e: + self.log(f"Exception in get_moving for servo {self.identifier}: {e}", level='error') + return False + if moving == 1: return True - elif abs(self.pos - self.get_position()) > 2: - print(f"Warning: Servo {self.identifier} is not reporting as moving but position {self.get_position()} does not match target position {self.pos}") + try: + pos = self.get_position() + except Exception as e: + self.log(f"Exception in get_position for servo {self.identifier}: {e}", level='error') + return False + if abs(self.pos - pos) > 2: + self.log(f"Warning: Servo {self.identifier} is not reporting as moving but position {pos} does not match target position {self.pos}", level='warning') return False def get_position(self): diff --git a/src/modules/actuators/bus_servo/libraries/rustypot_backend.py b/src/modules/actuators/bus_servo/libraries/rustypot_backend.py index 78cff81d..3dedb2d0 100644 --- a/src/modules/actuators/bus_servo/libraries/rustypot_backend.py +++ b/src/modules/actuators/bus_servo/libraries/rustypot_backend.py @@ -3,21 +3,42 @@ import numpy as np class RustypotBusServo(BusServoBase): - + # Class-level dictionary to store controller singletons by (port, baudrate) + _controller_singletons = {} + def __init__(self, servo_id, model, port, baudrate=1000000, range=None, range_degrees=None, **kwargs): super().__init__(servo_id, model, port, baudrate, range, **kwargs) - if model.startswith('ST'): - from rustypot import Sts3215PyController - self.controller = Sts3215PyController(port, baudrate, 0.1) - self.max_rad = np.deg2rad(360) - self.min_rad = 0 - elif model.startswith('SC'): - from rustypot import Scs0009PyController - self.controller = Scs0009PyController(port, baudrate, 0.1) - self.max_rad = np.deg2rad(300) - self.min_rad = 0 + key = (port) + if key in RustypotBusServo._controller_singletons: + self.controller = RustypotBusServo._controller_singletons[key]['controller'] + # Set max_rad/min_rad based on the controller type already created + self.max_rad = RustypotBusServo._controller_singletons[key]['max_rad'] + self.min_rad = RustypotBusServo._controller_singletons[key]['min_rad'] + existing_model = RustypotBusServo._controller_singletons[key]['model'] + if (model.startswith('ST') and not existing_model.startswith('ST')) or (model.startswith('SC') and not existing_model.startswith('SC')): + self.log(f"Warning: Attempted to create a {model} controller for port {port}, but a {existing_model} controller already exists. Reusing the existing controller.") else: - raise ValueError(f"Unknown model: {model}") + if model.startswith('ST'): + from rustypot import Sts3215PyController + controller = Sts3215PyController(port, baudrate, 0.1) + max_rad = np.deg2rad(360) + min_rad = 0 + elif model.startswith('SC'): + from rustypot import Scs0009PyController + controller = Scs0009PyController(port, baudrate, 0.1) + max_rad = np.deg2rad(300) + min_rad = 0 + else: + raise ValueError(f"Unknown model: {model}") + RustypotBusServo._controller_singletons[key] = { + 'controller': controller, + 'model': model, + 'max_rad': max_rad, + 'min_rad': min_rad + } + self.controller = controller + self.max_rad = max_rad + self.min_rad = min_rad def get_speed(self, unit='degrees'): rad_s = self.controller.read_present_speed(self.servo_id) @@ -69,13 +90,19 @@ def move_to(self, value, unit='degrees'): def get_position(self, unit='degrees'): rad = self.controller.read_present_position(self.servo_id) + # Ensure rad is a scalar, not a numpy array + if isinstance(rad, np.ndarray): + rad = float(rad.item()) if unit == 'degrees': - return np.rad2deg(rad) + deg = np.rad2deg(rad) + if isinstance(deg, np.ndarray): + deg = float(deg.item()) + return deg elif unit == 'radians': - return rad + return float(rad) elif unit == 'raw': # Not directly supported; return radians as raw - return rad + return float(rad) else: raise ValueError(f"Unknown unit: {unit}") diff --git a/src/modules/personality/personality.py b/src/modules/personality/personality.py index eb19afe6..916414fd 100644 --- a/src/modules/personality/personality.py +++ b/src/modules/personality/personality.py @@ -222,7 +222,7 @@ def loop_10(self): # self.servos['leg_l_ankle'].move_relative(50) # self.scan_vision() # self.output_current_pose() - # self.publish('servo/pose', pose_name='wave_1') # For testing pose movement + # self.publish('servo/pose', pose_name='stand_low_narrow') # For testing pose movement # time.sleep(1) # current_pose = self.estimate_current_pose() # if current_pose not in self.servos['leg_r_tilt'].poses: @@ -252,8 +252,8 @@ def loop_second(self): now = time.time() self.cycle_display() self.balance() - self.chicken_head() - self.one_leg_balance() + # self.chicken_head() + # self.one_leg_balance() self.estimate_current_pose() # Handle ongoing object reaction From f663e493d0057c182467c4377fff2c90978c612d Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sun, 29 Mar 2026 16:51:22 +0000 Subject: [PATCH 2/4] Introduce ServoManager for centralised bus servo control Agent-Logs-Url: https://github.com/makerforgetech/modular-biped/sessions/ac60dd13-c688-4151-b48b-7f769e416658 Co-authored-by: danic85 <6583012+danic85@users.noreply.github.com> --- environments/cody.yml | 160 +++--- environments/laptop.yml | 158 +++--- src/modules/actuators/bus_servo/README.md | 157 ++++-- src/modules/actuators/bus_servo/bus_servo.py | 512 ++++++++++-------- src/modules/actuators/bus_servo/config.yml | 2 +- .../bus_servo/libraries/waveshare_backend.py | 89 ++- .../bus_servo/tests/test_bus_servo.py | 171 ++++-- src/modules/actuators/tests/bus_servo_test.py | 288 ++++++---- 8 files changed, 931 insertions(+), 606 deletions(-) diff --git a/environments/cody.yml b/environments/cody.yml index 9b2905db..44df7a8d 100644 --- a/environments/cody.yml +++ b/environments/cody.yml @@ -31,85 +31,85 @@ bus_servo: - sit_edge_swing_l: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 243.5, 'leg_r_ankle': 251.0, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.9, 'leg_l_knee': 157.2, 'leg_l_ankle': 176.2} - sit_edge_swing_r: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 195.3, 'leg_r_ankle': 220.2, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.6, 'leg_l_knee': 118.2, 'leg_l_ankle': 134.7} - stand_low_narrow: {'leg_r_tilt': 221.0989010989011, 'leg_r_hip': 38.417582417582416, 'leg_r_knee': 72.17582417582418, 'leg_r_ankle': 258.1978021978022, 'neck_pan': 150.24175824175825, 'neck_tilt': 34.604105571847505, 'leg_l_tilt': 160.26373626373626, 'leg_l_hip': 235.07692307692307, 'leg_l_knee': 284.65934065934067, 'leg_l_ankle': 107.86813186813187} - instances: - - name: "leg_r_tilt" - model: 'ST3215' - id: 1 - range: [220.9, 346.6] # converted from raw (4095 = 360 degrees) - start: 246.9 - # calibrate_on_boot: true - - name: "leg_r_hip" - model: 'ST3215' - id: 3 - range: [0.0, 181.3] - # calibrate_on_boot: true - # start: 148.4 - - name: "leg_r_knee" - model: 'ST3215' - id: 2 - range: [62.9, 295.7] - # start: 118.2 - - name: "leg_r_ankle" - model: 'ST3215' - id: 6 - range: [216.1, 340.5] - # start: 243.0 - - name: "neck_pan" - model: 'ST3215' - id: 12 - range: [0.0, 299.1] # converted from raw (4095 = 360 degrees) - start: 149.9 - # calibrate_on_boot: true - # demonstrate_on_boot: true - # - name: "neck_pan" - # model: 'SC09' - # baudrate: 115200 - # id: 4 - # range: [0.0, 300.0] - # start: 150.0 - # calibrate_on_boot: true - # demonstrate_on_boot: true - # speed: 60 # tested up to 600 - - name: "neck_tilt" - model: 'SC09' - baudrate: 115200 - id: 11 - range: [8.8, 64.5] # converted from raw (1023 = 300 degrees) - start: 35.2 - speed: 60 # max=0, 1 - 3000 tested - # acceleration: 0 # max (no significant difference between this and 1 - 3000) - # demonstrate_on_boot: true - # - name: "neck_tilt2" - # model: 'SC09' - # baudrate: 115200 - # id: 5 - # range: [0.0, 179.0] # converted from raw (1023 = 300 degrees) - # start: 89.5 - # speed: 300 - # acceleration: 1 - # demonstrate_on_boot: true - - name: "leg_l_tilt" - model: 'ST3215' - id: 7 - range: [0.7, 161] # converted from raw (4095 = 360 degrees) - start: 142.1 - # calibrate_on_boot: true - - name: "leg_l_hip" - model: 'ST3215' - id: 8 - range: [74.9, 279.2] - # start: 215.4 - - name: "leg_l_knee" - model: 'ST3215' - id: 9 - range: [117.1, 299.3] - # start: 232.2 - - name: "leg_l_ankle" - model: 'ST3215' - id: 10 - range: [53.9, 199.3] - # start: 137.6 - # demonstrate_on_boot: true + servos: + - name: "leg_r_tilt" + model: 'ST3215' + id: 1 + range: [220.9, 346.6] # converted from raw (4095 = 360 degrees) + start: 246.9 + # calibrate_on_boot: true + - name: "leg_r_hip" + model: 'ST3215' + id: 3 + range: [0.0, 181.3] + # calibrate_on_boot: true + # start: 148.4 + - name: "leg_r_knee" + model: 'ST3215' + id: 2 + range: [62.9, 295.7] + # start: 118.2 + - name: "leg_r_ankle" + model: 'ST3215' + id: 6 + range: [216.1, 340.5] + # start: 243.0 + - name: "neck_pan" + model: 'ST3215' + id: 12 + range: [0.0, 299.1] # converted from raw (4095 = 360 degrees) + start: 149.9 + # calibrate_on_boot: true + # demonstrate_on_boot: true + # - name: "neck_pan" + # model: 'SC09' + # baudrate: 115200 + # id: 4 + # range: [0.0, 300.0] + # start: 150.0 + # calibrate_on_boot: true + # demonstrate_on_boot: true + # speed: 60 # tested up to 600 + - name: "neck_tilt" + model: 'SC09' + baudrate: 115200 + id: 11 + range: [8.8, 64.5] # converted from raw (1023 = 300 degrees) + start: 35.2 + speed: 60 # max=0, 1 - 3000 tested + # acceleration: 0 # max (no significant difference between this and 1 - 3000) + # demonstrate_on_boot: true + # - name: "neck_tilt2" + # model: 'SC09' + # baudrate: 115200 + # id: 5 + # range: [0.0, 179.0] # converted from raw (1023 = 300 degrees) + # start: 89.5 + # speed: 300 + # acceleration: 1 + # demonstrate_on_boot: true + - name: "leg_l_tilt" + model: 'ST3215' + id: 7 + range: [0.7, 161] # converted from raw (4095 = 360 degrees) + start: 142.1 + # calibrate_on_boot: true + - name: "leg_l_hip" + model: 'ST3215' + id: 8 + range: [74.9, 279.2] + # start: 215.4 + - name: "leg_l_knee" + model: 'ST3215' + id: 9 + range: [117.1, 299.3] + # start: 232.2 + - name: "leg_l_ankle" + model: 'ST3215' + id: 10 + range: [53.9, 199.3] + # start: 137.6 + # demonstrate_on_boot: true chatgpt: enabled: true @@ -335,7 +335,7 @@ oled_display: personality: enabled: true inject: - servos: "Servo_*" + servos: ServoManager imu.head: BNO055_imu_head imu.body: BNO055_imu_body vision: Vision diff --git a/environments/laptop.yml b/environments/laptop.yml index 89517bc9..2f4ab557 100644 --- a/environments/laptop.yml +++ b/environments/laptop.yml @@ -30,82 +30,82 @@ bus_servo: - sit_edge: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 243.5, 'leg_r_ankle': 251.0, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.7, 'leg_l_knee': 117.1, 'leg_l_ankle': 137.2} - sit_edge_swing_l: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 243.5, 'leg_r_ankle': 251.0, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.9, 'leg_l_knee': 157.2, 'leg_l_ankle': 176.2} - sit_edge_swing_r: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 195.3, 'leg_r_ankle': 220.2, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.6, 'leg_l_knee': 118.2, 'leg_l_ankle': 134.7} - instances: - - name: "leg_r_tilt" - model: 'ST3215' - id: 1 - range: [220.9, 346.6] # converted from raw (4095 = 360 degrees) - start: 246.9 - # calibrate_on_boot: true - - name: "leg_r_hip" - model: 'ST3215' - id: 3 - range: [0.0, 181.3] - # calibrate_on_boot: true - # start: 148.4 - - name: "leg_r_knee" - model: 'ST3215' - id: 2 - range: [62.9, 295.7] - # start: 118.2 - - name: "leg_r_ankle" - model: 'ST3215' - id: 6 - range: [216.1, 340.5] - # start: 243.0 - - name: "neck_pan" - model: 'ST3215' - id: 12 - range: [0.0, 299.1] # converted from raw (4095 = 360 degrees) - start: 149.9 - # calibrate_on_boot: true - # demonstrate_on_boot: true - # - name: "neck_pan" - # model: 'SC09' - # baudrate: 115200 - # id: 4 - # range: [0.0, 300.0] - # start: 150.0 - # calibrate_on_boot: true - # demonstrate_on_boot: true - # speed: 60 # tested up to 600 - - name: "neck_tilt" - model: 'SC09' - baudrate: 115200 - id: 11 - range: [8.8, 64.5] # converted from raw (1023 = 300 degrees) - start: 35.2 - speed: 60 # max=0, 1 - 3000 tested - # acceleration: 0 # max (no significant difference between this and 1 - 3000) - # demonstrate_on_boot: true - # - name: "neck_tilt2" - # model: 'SC09' - # baudrate: 115200 - # id: 5 - # range: [0.0, 179.0] # converted from raw (1023 = 300 degrees) - # start: 89.5 - # speed: 300 - # acceleration: 1 - # demonstrate_on_boot: true - - name: "leg_l_tilt" - model: 'ST3215' - id: 7 - range: [0.7, 157.8] # converted from raw (4095 = 360 degrees) - start: 142.1 - # calibrate_on_boot: true - - name: "leg_l_hip" - model: 'ST3215' - id: 8 - range: [74.9, 279.2] - # start: 215.4 - - name: "leg_l_knee" - model: 'ST3215' - id: 9 - range: [117.1, 299.3] - # start: 232.2 - - name: "leg_l_ankle" - model: 'ST3215' - id: 10 - range: [53.9, 199.3] - # start: 137.6 - # demonstrate_on_boot: true \ No newline at end of file + servos: + - name: "leg_r_tilt" + model: 'ST3215' + id: 1 + range: [220.9, 346.6] # converted from raw (4095 = 360 degrees) + start: 246.9 + # calibrate_on_boot: true + - name: "leg_r_hip" + model: 'ST3215' + id: 3 + range: [0.0, 181.3] + # calibrate_on_boot: true + # start: 148.4 + - name: "leg_r_knee" + model: 'ST3215' + id: 2 + range: [62.9, 295.7] + # start: 118.2 + - name: "leg_r_ankle" + model: 'ST3215' + id: 6 + range: [216.1, 340.5] + # start: 243.0 + - name: "neck_pan" + model: 'ST3215' + id: 12 + range: [0.0, 299.1] # converted from raw (4095 = 360 degrees) + start: 149.9 + # calibrate_on_boot: true + # demonstrate_on_boot: true + # - name: "neck_pan" + # model: 'SC09' + # baudrate: 115200 + # id: 4 + # range: [0.0, 300.0] + # start: 150.0 + # calibrate_on_boot: true + # demonstrate_on_boot: true + # speed: 60 # tested up to 600 + - name: "neck_tilt" + model: 'SC09' + baudrate: 115200 + id: 11 + range: [8.8, 64.5] # converted from raw (1023 = 300 degrees) + start: 35.2 + speed: 60 # max=0, 1 - 3000 tested + # acceleration: 0 # max (no significant difference between this and 1 - 3000) + # demonstrate_on_boot: true + # - name: "neck_tilt2" + # model: 'SC09' + # baudrate: 115200 + # id: 5 + # range: [0.0, 179.0] # converted from raw (1023 = 300 degrees) + # start: 89.5 + # speed: 300 + # acceleration: 1 + # demonstrate_on_boot: true + - name: "leg_l_tilt" + model: 'ST3215' + id: 7 + range: [0.7, 157.8] # converted from raw (4095 = 360 degrees) + start: 142.1 + # calibrate_on_boot: true + - name: "leg_l_hip" + model: 'ST3215' + id: 8 + range: [74.9, 279.2] + # start: 215.4 + - name: "leg_l_knee" + model: 'ST3215' + id: 9 + range: [117.1, 299.3] + # start: 232.2 + - name: "leg_l_ankle" + model: 'ST3215' + id: 10 + range: [53.9, 199.3] + # start: 137.6 + # demonstrate_on_boot: true \ No newline at end of file diff --git a/src/modules/actuators/bus_servo/README.md b/src/modules/actuators/bus_servo/README.md index 632827f8..82427424 100644 --- a/src/modules/actuators/bus_servo/README.md +++ b/src/modules/actuators/bus_servo/README.md @@ -2,99 +2,144 @@ ## Overview -Serial bus servos allow for efficient communication and control of multiple servos over a single bus. The `Servo` class provides a high-level interface to manage these servos, including setting positions, speeds, and handling configurations. The hardware interface is now fully backend-agnostic, supporting multiple libraries and simulation. +Serial bus servos allow for efficient communication and control of multiple servos over a single bus. The `ServoManager` class provides a centralised, high-level interface to manage all servos on a bus, including setting positions, coordinating multi-servo motions, and handling configuration. The hardware interface is backend-agnostic, supporting multiple libraries and simulation. ## Architecture -The bus_servo module now supports multiple backends: +### ServoManager -- **Waveshare**: Uses the official Waveshare ST/SC SDKs for hardware control. -- **Rustypot**: Uses the rustypot library for Feetech and compatible servos. -- **Simulation**: A software-only backend for debugging and development, which logs all actions instead of controlling hardware. +`ServoManager` is the single `BaseModule` responsible for all bus servo operations: -You can select the backend in your configuration. All backends implement the same interface, so you can switch between them without changing your code. +- Owns one shared hardware controller per physical serial bus (port + baudrate). Multiple servos on the same bus open the port **only once**, avoiding contention. +- Maintains a registry of lightweight `ServoState` objects keyed by servo name. +- Handles all messaging subscriptions for every managed servo. +- Processes per-servo move queues each system-loop cycle. +- Provides high-level APIs for coordinated motion: `move_to_pose()`, `group_move()`. -All servo configuration (range, start, poses) is now in **degrees** for clarity and cross-backend compatibility. +### ServoState -## Configuration +`ServoState` is a lightweight per-servo holder: -The configuration file (e.g. `environments/cody.yml`) contains the configuration for each bus servo. The `instances` section defines the servos, their IDs, and their initial positions, all in degrees. The `poses` section defines named poses, also in degrees. +- Stores only configuration and logical state: name, ID, model, range, speed, acceleration, current position, move queue, and poses. +- Does **not** own the serial port or controller. +- Delegates every hardware operation (move, read position, torque enable/disable) to the owning `ServoManager`. -## Getting Started / Calibration +### Backends -To calibrate the servos, each must have their ID set individually. To achieve this, connect one servo to the driver board and run `modules/actuators/bus_servo/change_id.py`. This script will prompt you to enter the ID for the connected servo, which will then be saved permanently on the servo. +| Backend | Class | Notes | +|---------|-------|-------| +| `waveshare` | `WaveshareBusServo` | Official Waveshare ST/SC SDKs (included). Port shared via class-level singleton. | +| `rustypot` | `RustypotBusServo` | Rustypot library for Feetech-compatible servos. | +| `simulation` | `SimulationBusServo` | Software-only, logs all actions; no hardware required. | -If you have challenges understanding the current ID of the servo, run `modules/actuators/bus_servo/libraries/waveshare/STServo_examples/read_all.py` or `modules/actuators/bus_servo/libraries/waveshare/SCServo_examples/read_all.py` depending on the servo type. This script will read and display the current ID of the connected servo. +All backends implement the same `BusServoBase` interface. -Once the ID has been set for all servos, you can use the `Servo` class to control them by enabling it in the environment yaml file. +## Configuration -### Centering -The servos raw value range is between 0-4095 for ST servos and 0-1024 for SC servos, this equates to 360 and 300 degrees respectively. The position readout can wrap around, so the servos should be set to the midpoint before mounting them in the robot otherwise it can result in the servo passing the wrong way through the range of values, which could damage the robot. +Servos are declared in the environment YAML file (e.g. `environments/cody.yml`) under a single `bus_servo` entry. All servo instances are listed under `config.servos`; the `ServoManager` is instantiated **once** and manages all of them. -The `change_id.py` script will set the servo to the midpoint when changing the ID. The servo should then be mounted in the robot at roughly the midpoint position. +```yaml +bus_servo: + enabled: true + config: + backend: 'waveshare' # waveshare | rustypot | simulation + poses: + - stand: {leg_r_tilt: 246.7, leg_l_tilt: 142.6, ...} + servos: + - name: leg_r_tilt + model: ST3215 + id: 1 + range: [220.9, 346.6] + start: 246.9 + - name: neck_tilt + model: SC09 + baudrate: 115200 # overrides the default baudrate for this servo + id: 11 + range: [8.8, 64.5] + start: 35.2 + speed: 60 +``` -You can also enable `center_on_boot` for each servo in the configuration file, which will move the servo to the center of its range on initialization. +## Injecting into other modules +Inject the `ServoManager` into modules that need direct servo access: -### Calibration -To calibrate the servo positions, set the flag `calibrate_on_boot` to `true` in the configuration file for each instance (servo). This will cause the servo to output it's current position in the debug log, which can then be copied into the start position, or range. Servos can be manually moved to any position to identify their range or certain poses. +```yaml +my_module: + enabled: true + inject: + servos: ServoManager +``` -Finally, set `calibrate_on_boot` to false and re-run the program to start using the servos with their configured positions. +Because `ServoManager` exposes a dict-like interface, existing code that accesses servos via `self.servos[name]` continues to work unchanged: -### Demonstration +```python +self.servos['neck_tilt'].move_relative(pitch) +self.servos['neck_pan'].move(150) +for name, servo in self.servos.items(): + servo.detach() +``` -To demonstrate the servo movement on boot, set the flag `demonstrate_on_boot` to `true` in the configuration file for each instance (servo). This will cause the servo to move to its minimum and maximum positions once on initialization, allowing you to see the range of motion. +## Messaging -## Subscriptions and direct command +The manager subscribes automatically for every servo listed in `config.servos`: -The `Servo` class subscribes to the following topics: -`servo::mv` - to move the servo to a specific position relative to it's current position. -`servo::mvabs` - to move the servo to a specific position with absolute values. +| Topic | Action | +|-------|--------| +| `servo::mvabs` | Queue absolute move (degrees) | +| `servo::mv` | Queue relative move (delta degrees) | +| `servo::queue` | Alias for absolute move | +| `servo/pose` | Move all servos to a named pose | -The environment configuration can also inject the servos into a module for direct control: +## Coordinated motion -``` -my_module: - enabled: true - inject: - servos: "Servo_*" +Use `group_move()` to queue moves for multiple servos in one call: + +```python +self.servo_manager.group_move({ + 'leg_l_hip': 180, + 'leg_r_hip': 180, + 'leg_l_knee': 90, + 'leg_r_knee': 90, +}) ``` -You can then directly call the servo such as: +Or publish a named pose to move all servos simultaneously: -``` -self.servos['neck_tilt'].move_relative(pitch) -self.servos['neck_tilt'].move(pitch) +```python +self.publish('servo/pose', pose_name='stand_low') ``` -## Smooth initialization +## Getting Started / Calibration -Because the `Servo` class gets the current position of the servo on initialization, there is no danger of a servo jumping from an unknown position to the start position. This is especially useful when the servos are powered on in a random position and is an advantage over hobby servos. +To calibrate the servos, each must have their ID set individually. Connect one servo to the driver board and run `modules/actuators/bus_servo/change_id.py`. This script will prompt you to enter the ID for the connected servo. -## SC vs ST servos +If you need to discover a servo's current ID, run `modules/actuators/bus_servo/libraries/waveshare/STServo_examples/read_all.py` (ST) or the SC equivalent. -The `Servo` class supports both ST and SC series servos from Waveshare, Feetech and compatible servos. The type of servo is determined by the `model` variable in the configuration file, and the backend is selected via the `backend` parameter. The class will automatically use the appropriate backend for the specified servo type. +### Centering + +Servo raw values range from 0–4095 (ST, 360°) or 0–1024 (SC, 300°). Set servos to their midpoint before mounting to avoid wrapping through the wrong direction. -There are some limitations to the SC servos as they do not support continuous rotation and have a lower rotational range compared to the ST servos. The simulation backend is useful for development and debugging without hardware. +### Calibration via `calibrate_on_boot` -## Backends +Set `calibrate_on_boot: true` for a servo in the config to continuously log its current position. Move the servo manually, then copy min/max values into `range`. Disable the flag when done. -- **WaveshareBusServo**: Uses the official Waveshare SDKs for ST/SC servos (this library is included in this repo). -- **RustypotBusServo**: Uses the rustypot library for Feetech and compatible servos (this library is referenced via python import). -- **SimulationBusServo**: Simulates servo behavior and logs all actions for debugging (no library required). +### Demonstration -All backends implement the same methods, including movement, speed, torque, calibration, and error handling. +Set `demonstrate_on_boot: true` to sweep a servo through its full range on startup. -## Notes during testing +## SC vs ST servos -- Speed: between 0 and 3000 for SC servos tested, 0 is max, then 1-3000 increases speed. -- Acceleration: between 0 and 3000 for SC servos tested, 0 is max but no significant difference observed between values. -- Overload error occurring fairly frequently on SC servos using Waveshare library. However when testing with scheduled movements the issue is far less frequent. This may be due to sending commands too quickly in succession. Cycling torque off/on seems to resolve the error, otherwise you need to power cycle the servo. -- To disable torque on SC servos use: `self.packetHandler.write1ByteTxRx(self.portHandler, self.index, ADDR_TORQUE_ENABLE, 0)`, change the 0 to 1 to enable torque. -- In some cases the overload error does seem to be caused by load on the servo, in which case toggling the torque does not resolve this. The neck pan is an example of this where the SC servo needed to be replaced with the more powerful ST servo. +Both families are supported via the same backend; the `model` field selects the correct SDK. ST servos (e.g. ST3215) have 360° range and support continuous-rotation mode. SC servos (e.g. SC09) have a 300° range. + +## Notes + +- Speed: 0 = max on SC servos; 1–3000 slows it down. +- Overload errors on SC servos can sometimes be resolved by toggling torque off/on. ## References -https://www.waveshare.com/wiki/ST3215_Servo -https://www.waveshare.com/wiki/SC09_Servo -https://www.waveshare.com/wiki/Bus_Servo_Adapter_(A) -https://github.com/pollen-robotics/rustypot \ No newline at end of file + +- https://www.waveshare.com/wiki/ST3215_Servo +- https://www.waveshare.com/wiki/SC09_Servo +- https://www.waveshare.com/wiki/Bus_Servo_Adapter_(A) +- https://github.com/pollen-robotics/rustypot diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py index 3a48072f..a7e573bc 100644 --- a/src/modules/actuators/bus_servo/bus_servo.py +++ b/src/modules/actuators/bus_servo/bus_servo.py @@ -6,99 +6,44 @@ import time from modules.base_module import BaseModule from modules.actuators.bus_servo.libraries.factory import BusServoFactory -class Servo(BaseModule): - def __init__(self, **kwargs): - """ - Servo class - """ - self.backend = kwargs.get('backend', 'waveshare') - print(f"Creating servo with backend {self.backend}") + + +class ServoState: + """ + Lightweight servo configuration and state holder. + + Stores per-servo identity and logical state (position, queue) only. + All hardware operations are delegated to the owning ServoManager so + that a single shared controller instance handles every servo on a bus. + """ + + def __init__(self, manager, **kwargs): + self._manager = manager self.identifier = kwargs.get('name') self.model = kwargs.get('model', 'ST') self.index = kwargs.get('id') self.range = kwargs.get('range') - self.start = kwargs.get('start') # Default start position - self.poses = kwargs.get('poses') # Dictionary of poses + self.start = kwargs.get('start') + self.port = kwargs.get('port', '/dev/ttyAMA0') self.baudrate = kwargs.get('baudrate', 1000000) - self.port = kwargs.get('port', '/dev/ttyAMA0') # Change as needed, find with `ls /dev/ttyAMA*` - self.calibrate_on_boot = kwargs.get('calibrate_on_boot', False) # Loop to show position for manual configuration - self.demonstrate_on_boot = kwargs.get('demonstrate_on_boot', False) # Move to min and max to demonstrate range - self.center_on_boot = kwargs.get('center_on_boot', False) # Move to center of range on boot - self.pos = None - self.speed = kwargs.get('speed', 300) # 3073 + self.speed = kwargs.get('speed', 300) self.acceleration = kwargs.get('acceleration', 50) + self.pos = None self._move_queue = collections.deque() - poses_list = kwargs.get('poses', []) - self.poses = {list(pose.keys())[0]: list(pose.values())[0] for pose in poses_list} - - # Backend selection - - self.backend_servo = BusServoFactory.create( - backend=self.backend, - model=self.model, - servo_id=self.index, - port=self.port, - baudrate=self.baudrate, - range=self.range, - ) - - def detach(self): - # Detach servo using backend - self.backend_servo.detach() - - def exit(self): - self.detach() - self.backend_servo.exit() - def setup_messaging(self): - self.subscribe('servo:' + self.identifier + ':mvabs', self.move) - self.subscribe('servo:' + self.identifier + ':mv', self.move_relative) - self.subscribe('servo:' + self.identifier + ':queue', self.move) - self.subscribe('system/exit', self.exit) - self.subscribe('servo/pose', self.move_to_pose) - - if self.calibrate_on_boot: - self.calibrate_dynamic() # Log will show current position repeatedly to help with manual configuration - - self.pos = self.get_position() # Get initial position to avoid jumping from unknown position - - if self.center_on_boot: - self.calibrate_to_center() - - if self.demonstrate_on_boot: - self.log(f"Demonstrating servo {self.identifier} movement, speed={self.speed}, acceleration={self.acceleration}") - if self.range is not None: - self.move(self.range[0]) # Move to min range - self.move((self.range[0] + self.range[1]) // 2) # Move to center - self.move(self.range[1]) # Move to max range - else: - self.log(f"Range not set for servo {self.identifier}, cannot demonstrate movement", level='warning') - - # Move to start position - # if self.get_pose_value('stand') is not None: - # self.start = self.get_pose_value('stand') - if self.start is not None: - self.move(self.start) - - def move_to_pose(self, pose_name): - # print(self.poses) - pose_value = self.poses.get(pose_name) - # print(f"{self.identifier} - Pose '{pose_name}' value: {pose_value}") - my_pose_value = pose_value.get(self.identifier) - print(f"Moving servo {self.identifier} to pose '{pose_name}' with value {my_pose_value}") - if my_pose_value is not None: - self.move(my_pose_value) + # Accept either a pre-converted poses dict or a list of single-key dicts + poses = kwargs.get('poses', []) + if isinstance(poses, dict): + self.poses = poses else: - self.log(f"Pose '{pose_name}' not found for servo {self.identifier}", level='warning') - + self.poses = {list(p.keys())[0]: list(p.values())[0] for p in poses} + + # ------------------------------------------------------------------ + # Queue helpers + # ------------------------------------------------------------------ + def move(self, position, speed=None, acceleration=None, delay=0, **kwargs): - """ - Add a move request to the queue. - :param position: Target position - :param speed: Optional speed override - :param acceleration: Optional acceleration override - :param delay: Optional delay in seconds before executing (for animation) - """ + """Queue a move to an absolute position (degrees).""" self._move_queue.append({ 'position': position, 'speed': speed if speed is not None else self.speed, @@ -107,158 +52,293 @@ def move(self, position, speed=None, acceleration=None, delay=0, **kwargs): 'delay': delay, }) + def move_relative(self, delta): + """Queue a relative move from the current position.""" + if self.pos is None: + return + new_position = round(self.pos + delta) + if self.range: + if new_position < self.range[0] or new_position > self.range[1]: + new_position = self.range[0] if new_position < self.range[0] else self.range[1] + self.move(new_position) + + # ------------------------------------------------------------------ + # Hardware delegates → ServoManager + # ------------------------------------------------------------------ + + def detach(self): + """Disable torque via the manager.""" + self._manager.detach_servo(self.identifier) + + def attach(self): + """Enable torque via the manager.""" + self._manager.attach_servo(self.identifier) + + def get_position(self): + """Return the current hardware position (degrees) via the manager.""" + return self._manager.get_servo_position(self.identifier) + + def is_moving(self): + """Return True if the servo is still moving (via the manager).""" + return self._manager.is_servo_moving(self.identifier) + + def calibrate_to_center(self): + """Move the servo to the centre of its range via the manager.""" + self._manager.calibrate_servo_to_center(self.identifier) + + +class ServoManager(BaseModule): + """ + Central manager for all bus servos. + + Owns the shared hardware backend controller(s) and manages every servo + operation, including coordinated group moves and pose transitions. + + ServoState instances are lightweight holders that delegate all hardware + access through this manager, ensuring: + - a single serial port is opened per bus; + - motion commands can be batched, sequenced, or synchronised; + - all hardware access is serialised through one place. + + The manager exposes a dict-like interface (``__getitem__``, ``items()``, + ``values()``, ``get()``) so existing code that treats the injected + ``servos`` attribute as a ``{name: servo}`` mapping continues to work + without modification. + """ + + def __init__(self, **kwargs): + self.backend = kwargs.get('backend', 'waveshare') + self._default_port = kwargs.get('port', '/dev/ttyAMA0') + self._default_baudrate = kwargs.get('baudrate', 1000000) + self._default_speed = kwargs.get('speed', 300) + self._default_acceleration = kwargs.get('acceleration', 50) + + # Shared poses: {pose_name: {servo_name: position_degrees}} + poses_list = kwargs.get('poses', []) + self._poses = {list(p.keys())[0]: list(p.values())[0] for p in poses_list} + + self._servos = {} # {name: ServoState} + self._servo_backends = {} # {name: BusServoBase} + + for servo_cfg in kwargs.get('servos', []): + # Apply manager-level defaults where the servo config does not + # supply its own value, then override with servo-specific values. + full_cfg = { + 'port': self._default_port, + 'baudrate': self._default_baudrate, + 'speed': self._default_speed, + 'acceleration': self._default_acceleration, + **servo_cfg, + 'poses': self._poses, # share the global poses dict + } + servo_state = ServoState(manager=self, **full_cfg) + self._servos[servo_state.identifier] = servo_state + + # Create per-servo backend. Port/controller sharing is handled + # transparently by class-level singletons inside each backend class. + backend = BusServoFactory.create( + backend=self.backend, + model=servo_state.model, + servo_id=servo_state.index, + port=servo_state.port, + baudrate=servo_state.baudrate, + range=servo_state.range, + ) + self._servo_backends[servo_state.identifier] = backend + + # ------------------------------------------------------------------ + # Dict-like interface for backward-compatible injection + # ------------------------------------------------------------------ + + def __getitem__(self, key): + return self._servos[key] + + def __iter__(self): + return iter(self._servos) + + def __contains__(self, key): + return key in self._servos + + def items(self): + return self._servos.items() + + def values(self): + return self._servos.values() + + def get(self, key, default=None): + return self._servos.get(key, default) + + # ------------------------------------------------------------------ + # BaseModule hooks + # ------------------------------------------------------------------ + + def setup_messaging(self): + """Subscribe to per-servo and global topics for all managed servos.""" + for name, servo in self._servos.items(): + self.subscribe(f'servo:{name}:mvabs', servo.move) + self.subscribe(f'servo:{name}:mv', servo.move_relative) + self.subscribe(f'servo:{name}:queue', servo.move) + self.subscribe('servo/pose', self.move_to_pose) + self.subscribe('system/exit', self.exit) + + # Initialise positions and queue start moves + for name, servo in self._servos.items(): + try: + servo.pos = self.get_servo_position(name) + except Exception: + if servo.start is not None: + servo.pos = servo.start + elif servo.range: + servo.pos = servo.range[0] + else: + servo.pos = 0 + self.log( + f"No range or start configured for servo {name}; " + f"initialising position to 0", + level='warning', + ) + if servo.start is not None: + servo.move(servo.start) + def loop(self): - """Called every system loop cycle to drain the move queue.""" - self._process_queue() + """Process every servo's move queue once per system-loop cycle.""" + for servo in self._servos.values(): + self._process_servo_queue(servo) - def _process_queue(self, **kwargs): - """ - Process the next item in the move queue if the servo is not moving. - Called every loop cycle. - """ - if not self._move_queue: + # ------------------------------------------------------------------ + # Internal queue processing + # ------------------------------------------------------------------ + + def _process_servo_queue(self, servo): + """Drain one item from the given servo's queue if the servo is idle.""" + if not servo._move_queue: return - if self.is_moving(): + if self.is_servo_moving(servo.identifier): return - next_item = self._move_queue[0] + next_item = servo._move_queue[0] if time.time() - next_item['timestamp'] >= next_item['delay']: - self._move_queue.popleft() - self._do_move(next_item['position'], next_item['speed'], next_item['acceleration']) + servo._move_queue.popleft() + self._do_move(servo, next_item['position'], + next_item['speed'], next_item['acceleration']) - def _do_move(self, position, speed=None, acceleration=None): - """ - Move the servo to an absolute position. - :param position: Position to move to - :param speed: Optional speed override - :param acceleration: Optional acceleration override - """ + def _do_move(self, servo, position, speed=None, acceleration=None): + """Execute a single hardware move for the given ServoState.""" if position is None: - self.log(f"Position is None for servo {self.identifier}, cannot move", level='error') + self.log(f"Position is None for servo {servo.identifier}, cannot move", + level='error') return - if position < self.range[0] or position > self.range[1]: - self.log(f"Position {position} out of range ({self.range[0]}-{self.range[1]})", level='error') + if servo.range and (position < servo.range[0] or position > servo.range[1]): + self.log( + f"Position {position} out of range " + f"({servo.range[0]}-{servo.range[1]})", + level='error', + ) return - # Apply per-move speed/acceleration overrides if supported by backend - if speed is not None and hasattr(self.backend_servo, 'set_speed'): - self.backend_servo.set_speed(speed) - if acceleration is not None and hasattr(self.backend_servo, 'set_acceleration'): - self.backend_servo.set_acceleration(acceleration) - # Delegate to backend - self.backend_servo.move_to(position, unit='degrees') - self.pos = position - - def move_relative(self, delta): + backend = self._servo_backends.get(servo.identifier) + if backend is None: + return + if speed is not None and hasattr(backend, 'set_speed'): + backend.set_speed(speed) + if acceleration is not None and hasattr(backend, 'set_acceleration'): + backend.set_acceleration(acceleration) + backend.move_to(position, unit='degrees') + servo.pos = position + + # ------------------------------------------------------------------ + # High-level coordination APIs + # ------------------------------------------------------------------ + + def move_to_pose(self, pose_name): + """Queue moves for all servos to the positions defined in the named pose.""" + pose_values = self._poses.get(pose_name, {}) + if not pose_values: + self.log(f"Pose '{pose_name}' not found", level='warning') + return + for servo_name, position in pose_values.items(): + if servo_name in self._servos: + self._servos[servo_name].move(position) + + def group_move(self, moves): """ - Move the servo relative to its current position. - :param delta: Change in position (can be negative) + Queue coordinated moves for multiple servos. + + :param moves: ``{servo_name: position}`` dict or iterable of + ``(servo_name, position)`` pairs. """ - # self.log(f"Moving servo {self.identifier} from {self.pos} by delta {delta}") - new_position = round(self.pos + delta) - if new_position < self.range[0] or new_position > self.range[1]: - self.log(f"Position {new_position} out of range ({self.range[0]}-{self.range[1]}). Adjusting", level='warning') - new_position = self.range[0] if new_position < self.range[0] else self.range[1] - - # Move to new position - self.move(new_position) - - - def is_moving(self): + if isinstance(moves, dict): + moves = moves.items() + for servo_name, position in moves: + if servo_name in self._servos: + self._servos[servo_name].move(position) + + # ------------------------------------------------------------------ + # Per-servo hardware delegates + # ------------------------------------------------------------------ + + def get_servo_position(self, servo_name): + """Return the current hardware position (degrees) of the named servo.""" + backend = self._servo_backends.get(servo_name) + if backend is None: + return None + return backend.get_position(unit='degrees') + + def is_servo_moving(self, servo_name): + """Return True if the named servo is still moving.""" + servo = self._servos.get(servo_name) + backend = self._servo_backends.get(servo_name) + if servo is None or backend is None: + return False try: - moving = self.backend_servo.get_moving() + moving = backend.get_moving() except Exception as e: - self.log(f"Exception in get_moving for servo {self.identifier}: {e}", level='error') + self.log(f"Exception in get_moving for servo {servo_name}: {e}", + level='error') return False if moving == 1: return True - try: - pos = self.get_position() - except Exception as e: - self.log(f"Exception in get_position for servo {self.identifier}: {e}", level='error') - return False - if abs(self.pos - pos) > 2: - self.log(f"Warning: Servo {self.identifier} is not reporting as moving but position {pos} does not match target position {self.pos}", level='warning') + if servo.pos is not None: + try: + pos = self.get_servo_position(servo_name) + if pos is not None and abs(servo.pos - pos) > 2: + self.log( + f"Servo {servo_name} not reported as moving but position " + f"mismatch: target={servo.pos}, current={pos}", + level='warning', + ) + except Exception: + pass return False - - def get_position(self): - """ - Get the current position of the servo. - """ - return self.backend_servo.get_position(unit='degrees') - - def get_pose_value(self, pose_name): - """ - Returns the position value for the given pose name from self.poses. - """ - if not self.poses: - return None - return self.poses.get(pose_name) - def calibrate_to_center(self): - """ - Move the servo to the center of its range using the backend implementation. - """ - self.backend_servo.calibrate_to_center() + def detach_servo(self, servo_name): + """Disable torque on the named servo.""" + backend = self._servo_backends.get(servo_name) + if backend: + backend.detach() - def calibrate(self): - """ - Move each servo to capture min and max positions for calibration. - """ - self.log(f"Move servo {self.identifier} to minimum position and press any key...") - getch() # Waits for a single key press - min = self.get_position() - self.log(f"Captured minimum position: {min}") - self.log(f"Move servo {self.identifier} to maximum position and press any key...") - getch() # Waits for a single key press - max = self.get_position() - self.log(f"Captured maximum position: {max}") - self.range = (min, max) - if self.start is not None and (self.start < min or self.start > max): - self.start = (min + max) // 2 - self.log(f"Start position {self.start} out of new range, setting to midpoint {self.start}") - self.log(f"Updated range for {self.identifier}: {self.range}. Start position: {self.start}") - - def calibrate_dynamic(self): - """ - Continuously log the current position to help with manual calibration. - Store min an max as they are found. - Complete on key press. and store in self.range - """ - self.log(f"Calibrating servo {self.identifier}. Move the servo to find min and max positions. Press any key to finish...") - self.detach() - min_pos = None - max_pos = None - try: - while True: - pos = self.get_position() - if pos is None: - self.log(f"Failed to get position for servo {self.identifier}", level='warning') - continue - if min_pos is None or pos < min_pos: - min_pos = pos - if max_pos is None or pos > max_pos: - max_pos = pos - # Print on the same line, pad with spaces to clear previous content - # (4095 = 360 degrees, so 1264 = 111 degrees) - range_degrees = max_pos - min_pos if min_pos is not None and max_pos is not None else 'N/A' - print(f"\rCurrent position: {pos}, Min: {min_pos}, Max: {max_pos} Range: {range_degrees}", end='', flush=True) - time.sleep(0.05) - if sys.stdin in select.select([sys.stdin], [], [], 0)[0]: - sys.stdin.read(1) # Consume the key so buffer is cleared - break - except KeyboardInterrupt: - # No need to handle as this is to allow changing selected servo - pass - print() # Move to next line after loop - if min_pos is not None and max_pos is not None: - self.range = (min_pos, max_pos) - self.log(f"Calibration complete for {self.identifier}. Range: {self.range}") - else: - self.log(f"No positions recorded during calibration for {self.identifier}.", level='warning') - - if self.start is not None and (self.start < min_pos or self.start > max_pos): - self.start = (min_pos + max_pos) // 2 - self.log(f"Start position {self.start} out of new range, setting to midpoint {self.start}") + def attach_servo(self, servo_name): + """Enable torque on the named servo.""" + backend = self._servo_backends.get(servo_name) + if backend: + backend.attach() + + def calibrate_servo_to_center(self, servo_name): + """Move the named servo to the centre of its configured range.""" + backend = self._servo_backends.get(servo_name) + if backend: + backend.calibrate_to_center() + + def detach_all(self): + """Disable torque on every managed servo.""" + for name in self._servos: + self.detach_servo(name) - + def exit(self, **kwargs): + """Disable torque and release all backends on system exit.""" + self.detach_all() + for backend in self._servo_backends.values(): + try: + backend.exit() + except Exception: + pass diff --git a/src/modules/actuators/bus_servo/config.yml b/src/modules/actuators/bus_servo/config.yml index 83efef60..bd371b47 100644 --- a/src/modules/actuators/bus_servo/config.yml +++ b/src/modules/actuators/bus_servo/config.yml @@ -1,5 +1,5 @@ bus_servo: - class: Servo + class: ServoManager dependencies: python: - rustypot diff --git a/src/modules/actuators/bus_servo/libraries/waveshare_backend.py b/src/modules/actuators/bus_servo/libraries/waveshare_backend.py index 5fc3c431..0600172e 100644 --- a/src/modules/actuators/bus_servo/libraries/waveshare_backend.py +++ b/src/modules/actuators/bus_servo/libraries/waveshare_backend.py @@ -15,34 +15,68 @@ COMM_SUCCESS = 0 # Communication success (matches value in both ST and SC SDKs) class WaveshareBusServo(BusServoBase): + # Class-level cache: one PortHandler + PacketHandler per (port, baudrate, model_type). + # All servo instances sharing a physical bus reuse the same port handle, so the + # serial port is opened only once regardless of how many servos are managed. + _port_singletons = {} # {(port, baudrate, model_type): {portHandler, packetHandler, ...}} + def __init__(self, servo_id, model, port, baudrate=1000000, range=None, range_degrees=None, **kwargs): super().__init__(servo_id, model, port, baudrate, range, **kwargs) self.speed = kwargs.get('speed', 300) self.acceleration = kwargs.get('acceleration', 50) - # Import and initialize the correct SDK based on model - if model.startswith('ST'): - from .waveshare.STservo_sdk import PortHandler, sts - self.portHandler = PortHandler(port) - self.packetHandler = sts(self.portHandler) - self.max_raw = 4095 - self.min_raw = 0 - self.max_deg = 360 - self.min_deg = 0 - elif model.startswith('SC'): - from .waveshare.SCservo_sdk import PortHandler, PacketHandler - self.portHandler = PortHandler(port) - self.packetHandler = PacketHandler(1) - self.max_raw = 1023 - self.min_raw = 0 - self.max_deg = 300 - self.min_deg = 0 - else: + + model_type = 'ST' if model.startswith('ST') else ('SC' if model.startswith('SC') else None) + if model_type is None: raise ValueError(f"Unknown model: {model}") - # Open port and set baudrate - if not self.portHandler.openPort(): - raise RuntimeError(f"Failed to open port {port} for servo {servo_id}") - if not self.portHandler.setBaudRate(baudrate): - raise RuntimeError(f"Failed to set baudrate {baudrate} for servo {servo_id}") + key = (port, baudrate, model_type) + + if key in WaveshareBusServo._port_singletons: + # Reuse the already-open port; just grab the shared handles. + singleton = WaveshareBusServo._port_singletons[key] + self.portHandler = singleton['portHandler'] + self.packetHandler = singleton['packetHandler'] + self.max_raw = singleton['max_raw'] + self.min_raw = singleton['min_raw'] + self.max_deg = singleton['max_deg'] + self.min_deg = singleton['min_deg'] + singleton['refcount'] += 1 + else: + # First servo on this (port, baudrate, model_type) — open the port. + if model.startswith('ST'): + from .waveshare.STservo_sdk import PortHandler, sts + portHandler = PortHandler(port) + packetHandler = sts(portHandler) + max_raw, min_raw = 4095, 0 + max_deg, min_deg = 360, 0 + elif model.startswith('SC'): + from .waveshare.SCservo_sdk import PortHandler, PacketHandler + portHandler = PortHandler(port) + packetHandler = PacketHandler(1) + max_raw, min_raw = 1023, 0 + max_deg, min_deg = 300, 0 + else: + raise ValueError(f"Unknown model: {model}") + + if not portHandler.openPort(): + raise RuntimeError(f"Failed to open port {port} for servo {servo_id}") + if not portHandler.setBaudRate(baudrate): + raise RuntimeError(f"Failed to set baudrate {baudrate} for servo {servo_id}") + + WaveshareBusServo._port_singletons[key] = { + 'portHandler': portHandler, + 'packetHandler': packetHandler, + 'max_raw': max_raw, + 'min_raw': min_raw, + 'max_deg': max_deg, + 'min_deg': min_deg, + 'refcount': 1, + } + self.portHandler = portHandler + self.packetHandler = packetHandler + self.max_raw = max_raw + self.min_raw = min_raw + self.max_deg = max_deg + self.min_deg = min_deg def move_to(self, value, unit='degrees'): if unit == 'degrees': @@ -85,7 +119,14 @@ def attach(self): self.packetHandler.write1ByteTxRx(self.portHandler, self.servo_id, ADDR_TORQUE_ENABLE, 1) def exit(self): - self.portHandler.closePort() + model_type = 'ST' if self.model.startswith('ST') else 'SC' + key = (self.port, self.baudrate, model_type) + singleton = WaveshareBusServo._port_singletons.get(key) + if singleton: + singleton['refcount'] -= 1 + if singleton['refcount'] <= 0: + self.portHandler.closePort() + del WaveshareBusServo._port_singletons[key] def move_to_raw(self, raw_value): if self.model.startswith('ST'): diff --git a/src/modules/actuators/bus_servo/tests/test_bus_servo.py b/src/modules/actuators/bus_servo/tests/test_bus_servo.py index e40d5873..58213e34 100644 --- a/src/modules/actuators/bus_servo/tests/test_bus_servo.py +++ b/src/modules/actuators/bus_servo/tests/test_bus_servo.py @@ -1,7 +1,6 @@ import sys import unittest -from unittest.mock import MagicMock -from modules.actuators.bus_servo.bus_servo import Servo +from unittest.mock import MagicMock, patch from modules.actuators.bus_servo.libraries.simulation_backend import SimulationBusServo from modules.actuators.bus_servo.libraries.factory import BusServoFactory from modules.actuators.bus_servo.libraries.waveshare_backend import WaveshareBusServo @@ -18,6 +17,8 @@ def setUp(self): sts=mock_sts, PortHandler=mock_PortHandler ) + # Clear any cached port singletons from previous tests + WaveshareBusServo._port_singletons.clear() self.servo = WaveshareBusServo(1, 'ST3215', '/dev/ttyUSB0', range=[0, 180]) self.servo.packetHandler = self.mock_packet self.servo.portHandler = MagicMock() @@ -29,16 +30,16 @@ def setUp(self): self.mock_packet.WritePosEx.return_value = (0, 0) self.mock_packet.WheelMode.return_value = (0, 0) self.mock_packet.WriteSpec.return_value = (0, 0) + def tearDown(self): sys.modules.pop('modules.actuators.bus_servo.libraries.waveshare.STservo_sdk', None) sys.modules.pop('numpy', None) + WaveshareBusServo._port_singletons.clear() def test_move_to(self): self.servo.move_to(90, unit='degrees') self.servo.move_to(1.57, unit='radians') self.servo.move_to(2048, unit='raw') - # Should call move_to_raw - # No exception means pass def test_get_position(self): self.servo.get_position_raw = MagicMock(return_value=2048) @@ -56,8 +57,18 @@ def test_attach_detach(self): self.servo.attach() self.servo.detach() - def test_exit(self): + def test_exit_decrements_refcount(self): + # After __init__ the refcount should be 1 in the singleton + model_type = 'ST' + key = (self.servo.port, self.servo.baudrate, model_type) + # Patch portHandler.closePort so it doesn't error + self.servo.portHandler.closePort = MagicMock() + initial_refcount = WaveshareBusServo._port_singletons.get(key, {}).get('refcount', 0) self.servo.exit() + # Either the key is removed (refcount hit 0) or refcount decreased + if key in WaveshareBusServo._port_singletons: + self.assertLess(WaveshareBusServo._port_singletons[key]['refcount'], initial_refcount) + # No exception = pass def test_move_to_raw(self): self.servo.move_to_raw(1234) @@ -104,14 +115,28 @@ def test_calibrate_to_center(self): self.servo.model = 'ST3215' self.servo.calibrate_to_center() + def test_shared_port_singleton(self): + """Two servos on the same port/baudrate/model share one portHandler.""" + # First servo was created in setUp. Create a second on the same bus. + second = WaveshareBusServo(2, 'ST3215', '/dev/ttyUSB0', range=[0, 180]) + key = ('/dev/ttyUSB0', 1000000, 'ST') + singleton_port = WaveshareBusServo._port_singletons[key]['portHandler'] + # Both instances must use the singleton's portHandler (not distinct ones) + self.assertIs(second.portHandler, singleton_port) + self.assertEqual(WaveshareBusServo._port_singletons[key]['refcount'], 2) + + class TestRustypotBusServo(unittest.TestCase): def setUp(self): - # Mock numpy before importing backend, since it may not be installed in test env. - sys.modules['numpy'] = MagicMock( - deg2rad=lambda x: x, - rad2deg=lambda x: x, - ) - # Mock the dynamic import of rustypot + # Mock numpy with real ndarray type so isinstance checks work + import types + mock_np = MagicMock() + mock_np.deg2rad = lambda x: x + mock_np.rad2deg = lambda x: x + # Make ndarray a real type so isinstance(rad, np.ndarray) doesn't raise + mock_np.ndarray = type('ndarray', (), {}) + sys.modules['numpy'] = mock_np + self.mock_controller = MagicMock() mock_rustypot = MagicMock() mock_rustypot.Sts3215PyController.return_value = self.mock_controller @@ -120,6 +145,7 @@ def setUp(self): from modules.actuators.bus_servo.libraries.rustypot_backend import RustypotBusServo self.servo = RustypotBusServo(1, 'ST3215', '/dev/ttyUSB0', range=[0, 180]) self.servo.controller = self.mock_controller + def tearDown(self): sys.modules.pop('rustypot', None) sys.modules.pop('numpy', None) @@ -186,35 +212,109 @@ def test_calibrate_to_center(self): self.servo.range = [0, 180] self.servo.calibrate_to_center() self.servo.move_to.assert_called() -class TestBusServo(unittest.TestCase): + + +class TestServoManager(unittest.TestCase): + def _make_manager(self, **extra_cfg): + from modules.actuators.bus_servo.bus_servo import ServoManager, ServoState + + mock_backend = MagicMock() + mock_backend.get_moving.return_value = 0 + mock_backend.get_position.return_value = 90.0 + + servo_cfg = { + 'name': 'test', + 'id': 1, + 'range': [0, 180], + 'model': 'ST3215', + **extra_cfg, + } + + with patch.object(BusServoFactory, 'create', return_value=mock_backend): + mgr = ServoManager(backend='simulation', servos=[servo_cfg]) + + return mgr, mgr._servos['test'], mock_backend + + def test_init_creates_servo_state(self): + from modules.actuators.bus_servo.bus_servo import ServoState + mgr, sv, _ = self._make_manager() + self.assertIsInstance(sv, ServoState) + self.assertEqual(sv.identifier, 'test') + self.assertEqual(sv.range, [0, 180]) + def test_init_simulation_backend(self): - servo = Servo(name='test', id=1, range=[0, 180], model='ST3215', backend='simulation') - self.assertEqual(servo.identifier, 'test') - self.assertEqual(servo.index, 1) - self.assertEqual(servo.range, [0, 180]) - self.assertEqual(servo.model, 'ST3215') - # Should use simulation backend - self.assertIsInstance(servo.backend_servo, SimulationBusServo) - - def test_move_to_degrees(self): - servo = Servo(name='test', id=1, range=[0, 180], model='ST3215', backend='simulation') - # Should log the move and update position - servo.move(90, unit='degrees') - self.assertEqual(servo.backend_servo.position, 90) - - def test_factory_simulation(self): sim = BusServoFactory.create( - backend='simulation', - model='ST3215', - servo_id=1, - port='sim', - range=[0, 180] + backend='simulation', model='ST3215', servo_id=1, port='sim', range=[0, 180] ) self.assertIsInstance(sim, SimulationBusServo) - sim.move_to(45, unit='degrees') - self.assertEqual(sim.position, 45) - def test_simulation_methods(self): + def test_do_move_calls_backend(self): + mgr, sv, backend = self._make_manager() + sv.pos = 90 + mgr._do_move(sv, 90) + backend.move_to.assert_called_once_with(90, unit='degrees') + self.assertEqual(sv.pos, 90) + + def test_do_move_out_of_range_logs_error(self): + mgr, sv, backend = self._make_manager() + mgr.log = MagicMock() + mgr._do_move(sv, 999) + backend.move_to.assert_not_called() + mgr.log.assert_called_once() + + def test_get_servo_position(self): + mgr, sv, backend = self._make_manager() + backend.get_position.return_value = 45.0 + pos = mgr.get_servo_position('test') + self.assertEqual(pos, 45.0) + backend.get_position.assert_called_once_with(unit='degrees') + + def test_detach_servo(self): + mgr, sv, backend = self._make_manager() + mgr.detach_servo('test') + backend.detach.assert_called_once() + + def test_attach_servo(self): + mgr, sv, backend = self._make_manager() + mgr.attach_servo('test') + backend.attach.assert_called_once() + + def test_exit_calls_detach_all_and_backend_exit(self): + mgr, sv, backend = self._make_manager() + mgr.exit() + backend.detach.assert_called() + backend.exit.assert_called() + + def test_group_move(self): + mgr, sv, _ = self._make_manager() + sv.move = MagicMock() + mgr.group_move({'test': 120}) + sv.move.assert_called_once_with(120) + + def test_move_to_pose(self): + mgr, sv, _ = self._make_manager() + mgr._poses = {'stand': {'test': 100}} + sv.move = MagicMock() + mgr.move_to_pose('stand') + sv.move.assert_called_once_with(100) + + def test_dict_like_getitem(self): + mgr, sv, _ = self._make_manager() + self.assertIs(mgr['test'], sv) + + def test_dict_like_contains(self): + mgr, _, _ = self._make_manager() + self.assertIn('test', mgr) + + def test_dict_like_iter(self): + mgr, _, _ = self._make_manager() + self.assertEqual(list(mgr), ['test']) + + def test_dict_like_items(self): + mgr, sv, _ = self._make_manager() + self.assertEqual(dict(mgr.items()), {'test': sv}) + + def test_simulation_backend_methods(self): sim = SimulationBusServo(1, 'ST3215', 'sim', range=[0, 180]) sim.set_speed(10) self.assertEqual(sim.speed, 10) @@ -228,5 +328,6 @@ def test_simulation_methods(self): sim.calibrate_to_center() self.assertEqual(sim.position, 90) + if __name__ == '__main__': unittest.main() diff --git a/src/modules/actuators/tests/bus_servo_test.py b/src/modules/actuators/tests/bus_servo_test.py index 80f55376..eeb9ce28 100644 --- a/src/modules/actuators/tests/bus_servo_test.py +++ b/src/modules/actuators/tests/bus_servo_test.py @@ -5,197 +5,255 @@ from unittest.mock import MagicMock, patch, call -def _make_servo(model='ST', speed=300, acceleration=50, range=(0, 4095)): +def _make_manager(model='ST', speed=300, acceleration=50, range=(0, 4095)): """ - Build a Servo instance with all hardware dependencies mocked out. - The messaging service is wired up so subscribe/publish work via simple - dict-based stubs (avoiding the real pub/sub library). + Build a ServoManager with a single servo and all hardware dependencies + mocked out. Returns (manager, servo_state, mock_backend). """ from modules.actuators.bus_servo import bus_servo as servo_mod - # Create a mock backend servo (replaces WaveshareBusServo, etc.) mock_backend = MagicMock() mock_backend.get_moving.return_value = 0 mock_backend.get_position.return_value = 100 - # Patch BusServoFactory.create so no hardware SDK is imported + servo_cfg = { + 'name': 'test_servo', + 'model': model, + 'id': 1, + 'range': list(range), + 'speed': speed, + 'acceleration': acceleration, + 'baudrate': 1000000, + 'port': '/dev/ttyAMA0', + 'start': None, + } + with patch.object(servo_mod.BusServoFactory, 'create', return_value=mock_backend): - kwargs = { - 'name': 'test_servo', - 'model': model, - 'id': 1, - 'range': list(range), - 'speed': speed, - 'acceleration': acceleration, - 'baudrate': 1000000, - 'port': '/dev/ttyAMA0', - 'calibrate_on_boot': False, - 'demonstrate_on_boot': False, - 'center_on_boot': False, - 'start': None, - 'poses': [], - } - sv = servo_mod.Servo(**kwargs) - - sv.pos = 100 # Set a known current position + manager = servo_mod.ServoManager( + backend='simulation', + servos=[servo_cfg], + ) + + servo_state = manager._servos['test_servo'] + servo_state.pos = 100 # Set a known current position # Wire up a mock messaging service messaging_service = MagicMock() - sv._messaging_service = messaging_service + manager._messaging_service = messaging_service - return sv, mock_backend + return manager, servo_state, mock_backend -class TestBusServoQueue(unittest.TestCase): +class TestServoManagerQueue(unittest.TestCase): def setUp(self): - self.sv, self.ph = _make_servo() + self.manager, self.servo_state, self.mock_backend = _make_manager() # ------------------------------------------------------------------ # move (queues the request) # ------------------------------------------------------------------ def test_queue_move_adds_item(self): - self.sv.move(500) - self.assertEqual(len(self.sv._move_queue), 1) - item = self.sv._move_queue[0] + self.servo_state.move(500) + self.assertEqual(len(self.servo_state._move_queue), 1) + item = self.servo_state._move_queue[0] self.assertEqual(item['position'], 500) - self.assertEqual(item['speed'], self.sv.speed) - self.assertEqual(item['acceleration'], self.sv.acceleration) + self.assertEqual(item['speed'], self.servo_state.speed) + self.assertEqual(item['acceleration'], self.servo_state.acceleration) self.assertEqual(item['delay'], 0) self.assertAlmostEqual(item['timestamp'], time.time(), delta=1.0) def test_queue_move_speed_acceleration_override(self): - self.sv.move(200, speed=100, acceleration=10) - item = self.sv._move_queue[0] + self.servo_state.move(200, speed=100, acceleration=10) + item = self.servo_state._move_queue[0] self.assertEqual(item['speed'], 100) self.assertEqual(item['acceleration'], 10) def test_queue_move_delay_stored(self): - self.sv.move(300, delay=2.5) - item = self.sv._move_queue[0] + self.servo_state.move(300, delay=2.5) + item = self.servo_state._move_queue[0] self.assertEqual(item['delay'], 2.5) def test_queue_move_multiple_items(self): - self.sv.move(100) - self.sv.move(200) - self.sv.move(300) - self.assertEqual(len(self.sv._move_queue), 3) - positions = [item['position'] for item in self.sv._move_queue] + self.servo_state.move(100) + self.servo_state.move(200) + self.servo_state.move(300) + self.assertEqual(len(self.servo_state._move_queue), 3) + positions = [item['position'] for item in self.servo_state._move_queue] self.assertEqual(positions, [100, 200, 300]) # ------------------------------------------------------------------ - # _process_queue + # _process_servo_queue # ------------------------------------------------------------------ def test_process_queue_empty_does_nothing(self): - self.sv._do_move = MagicMock() - self.sv._process_queue() - self.sv._do_move.assert_not_called() + self.manager._do_move = MagicMock() + self.manager._process_servo_queue(self.servo_state) + self.manager._do_move.assert_not_called() def test_process_queue_calls_move_when_idle(self): - self.sv.is_moving = MagicMock(return_value=False) - self.sv._do_move = MagicMock() - self.sv.move(500) - self.sv._process_queue() - self.sv._do_move.assert_called_once_with(500, self.sv.speed, self.sv.acceleration) - self.assertEqual(len(self.sv._move_queue), 0) + self.manager.is_servo_moving = MagicMock(return_value=False) + self.manager._do_move = MagicMock() + self.servo_state.move(500) + self.manager._process_servo_queue(self.servo_state) + self.manager._do_move.assert_called_once_with( + self.servo_state, 500, + self.servo_state.speed, self.servo_state.acceleration, + ) + self.assertEqual(len(self.servo_state._move_queue), 0) def test_process_queue_does_not_move_while_moving(self): - self.sv.is_moving = MagicMock(return_value=True) - self.sv._do_move = MagicMock() - self.sv.move(500) - self.sv._process_queue() - self.sv._do_move.assert_not_called() - self.assertEqual(len(self.sv._move_queue), 1) + self.manager.is_servo_moving = MagicMock(return_value=True) + self.manager._do_move = MagicMock() + self.servo_state.move(500) + self.manager._process_servo_queue(self.servo_state) + self.manager._do_move.assert_not_called() + self.assertEqual(len(self.servo_state._move_queue), 1) def test_process_queue_respects_delay(self): - self.sv.is_moving = MagicMock(return_value=False) - self.sv._do_move = MagicMock() - # Add item with a future delay - self.sv.move(500, delay=100) - self.sv._process_queue() - # Should NOT move yet because delay has not elapsed - self.sv._do_move.assert_not_called() - self.assertEqual(len(self.sv._move_queue), 1) + self.manager.is_servo_moving = MagicMock(return_value=False) + self.manager._do_move = MagicMock() + self.servo_state.move(500, delay=100) + self.manager._process_servo_queue(self.servo_state) + self.manager._do_move.assert_not_called() + self.assertEqual(len(self.servo_state._move_queue), 1) def test_process_queue_executes_after_delay_elapsed(self): - self.sv.is_moving = MagicMock(return_value=False) - self.sv._do_move = MagicMock() - self.sv.move(500, delay=0) - # Backdate the timestamp to simulate delay already elapsed - self.sv._move_queue[0]['timestamp'] = time.time() - 5 - self.sv._process_queue() - self.sv._do_move.assert_called_once() + self.manager.is_servo_moving = MagicMock(return_value=False) + self.manager._do_move = MagicMock() + self.servo_state.move(500, delay=0) + self.servo_state._move_queue[0]['timestamp'] = time.time() - 5 + self.manager._process_servo_queue(self.servo_state) + self.manager._do_move.assert_called_once() def test_process_queue_processes_one_item_per_call(self): - self.sv.is_moving = MagicMock(return_value=False) - self.sv._do_move = MagicMock() - self.sv.move(100) - self.sv.move(200) - self.sv._process_queue() - # Only first item should be processed - self.assertEqual(len(self.sv._move_queue), 1) - self.sv._do_move.assert_called_once_with(100, self.sv.speed, self.sv.acceleration) + self.manager.is_servo_moving = MagicMock(return_value=False) + self.manager._do_move = MagicMock() + self.servo_state.move(100) + self.servo_state.move(200) + self.manager._process_servo_queue(self.servo_state) + self.assertEqual(len(self.servo_state._move_queue), 1) + self.manager._do_move.assert_called_once_with( + self.servo_state, 100, + self.servo_state.speed, self.servo_state.acceleration, + ) def test_process_queue_uses_per_item_speed_acceleration(self): - self.sv.is_moving = MagicMock(return_value=False) - self.sv._do_move = MagicMock() - self.sv.move(400, speed=50, acceleration=5) - self.sv._process_queue() - self.sv._do_move.assert_called_once_with(400, 50, 5) + self.manager.is_servo_moving = MagicMock(return_value=False) + self.manager._do_move = MagicMock() + self.servo_state.move(400, speed=50, acceleration=5) + self.manager._process_servo_queue(self.servo_state) + self.manager._do_move.assert_called_once_with(self.servo_state, 400, 50, 5) # ------------------------------------------------------------------ # _do_move – no blocking sleep, delegates to backend # ------------------------------------------------------------------ def test_move_does_not_block(self): - """_do_move() must not call time.sleep (blocking removed).""" + """_do_move() must not call time.sleep.""" with patch('time.sleep') as mock_sleep: - self.sv._do_move(500) + self.manager._do_move(self.servo_state, 500) mock_sleep.assert_not_called() def test_move_accepts_speed_acceleration_params(self): - """_do_move() should delegate the move to the backend servo.""" - self.sv._do_move(500, speed=10, acceleration=2) - self.ph.move_to.assert_called_once_with(500, unit='degrees') + """_do_move() should delegate the move to the backend.""" + self.manager._do_move(self.servo_state, 500, speed=10, acceleration=2) + self.mock_backend.move_to.assert_called_once_with(500, unit='degrees') - def test_move_uses_instance_defaults_when_not_supplied(self): - self.sv._do_move(500) - self.ph.move_to.assert_called_once_with(500, unit='degrees') + def test_move_uses_backend_when_not_supplied(self): + self.manager._do_move(self.servo_state, 500) + self.mock_backend.move_to.assert_called_once_with(500, unit='degrees') # ------------------------------------------------------------------ - # setup_messaging subscriptions + # loop() # ------------------------------------------------------------------ - def test_loop_calls_process_queue(self): - """loop() should call _process_queue to drain the move queue.""" - with patch.object(self.sv, '_process_queue') as mock_pq: - self.sv.loop() - mock_pq.assert_called_once() - - def test_setup_messaging_does_not_subscribe_to_system_loop(self): - """system/loop subscription replaced by direct loop() call — should not be in subscriptions.""" - messaging_service = MagicMock() - with patch.object(self.sv, 'get_position', return_value=100): - self.sv.messaging_service = messaging_service - - topics = [c.args[0] for c in messaging_service.subscribe.call_args_list] - self.assertNotIn('system/loop', topics) + def test_loop_processes_all_servos(self): + """loop() should call _process_servo_queue for every managed servo.""" + with patch.object(self.manager, '_process_servo_queue') as mock_pq: + self.manager.loop() + mock_pq.assert_called_once_with(self.servo_state) + # ------------------------------------------------------------------ + # setup_messaging subscriptions + # ------------------------------------------------------------------ def test_setup_messaging_subscribes_queue_topic(self): messaging_service = MagicMock() - with patch.object(self.sv, 'get_position', return_value=100): - self.sv.messaging_service = messaging_service + with patch.object(self.manager, 'get_servo_position', return_value=100): + self.manager.messaging_service = messaging_service topics = [c.args[0] for c in messaging_service.subscribe.call_args_list] self.assertIn('servo:test_servo:queue', topics) def test_setup_messaging_mvabs_uses_queue_move(self): messaging_service = MagicMock() - with patch.object(self.sv, 'get_position', return_value=100): - self.sv.messaging_service = messaging_service + with patch.object(self.manager, 'get_servo_position', return_value=100): + self.manager.messaging_service = messaging_service - # Find the callback registered for ':mvabs' subscriptions = {c.args[0]: c.args[1] for c in messaging_service.subscribe.call_args_list} self.assertIn('servo:test_servo:mvabs', subscriptions) - self.assertEqual(subscriptions['servo:test_servo:mvabs'], self.sv.move) + self.assertEqual(subscriptions['servo:test_servo:mvabs'], self.servo_state.move) + + # ------------------------------------------------------------------ + # Dict-like interface + # ------------------------------------------------------------------ + def test_getitem_returns_servo_state(self): + from modules.actuators.bus_servo.bus_servo import ServoState + self.assertIsInstance(self.manager['test_servo'], ServoState) + + def test_iter_yields_servo_names(self): + self.assertIn('test_servo', list(self.manager)) + + def test_contains_servo_name(self): + self.assertIn('test_servo', self.manager) + + def test_items_returns_name_state_pairs(self): + items = dict(self.manager.items()) + self.assertIn('test_servo', items) + + # ------------------------------------------------------------------ + # Group move / pose + # ------------------------------------------------------------------ + def test_group_move_queues_for_all_servos(self): + self.servo_state.move = MagicMock() + self.manager.group_move({'test_servo': 200}) + self.servo_state.move.assert_called_once_with(200) + + def test_move_to_pose_unknown_logs_warning(self): + self.manager.log = MagicMock() + self.manager.move_to_pose('nonexistent_pose') + self.manager.log.assert_called_once() + + def test_move_to_pose_known_queues_moves(self): + self.manager._poses = {'test_pose': {'test_servo': 150}} + self.servo_state.move = MagicMock() + self.manager.move_to_pose('test_pose') + self.servo_state.move.assert_called_once_with(150) + + # ------------------------------------------------------------------ + # ServoState delegates to manager + # ------------------------------------------------------------------ + def test_servo_state_detach_delegates(self): + self.manager.detach_servo = MagicMock() + self.servo_state.detach() + self.manager.detach_servo.assert_called_once_with('test_servo') + + def test_servo_state_attach_delegates(self): + self.manager.attach_servo = MagicMock() + self.servo_state.attach() + self.manager.attach_servo.assert_called_once_with('test_servo') + + def test_servo_state_get_position_delegates(self): + self.manager.get_servo_position = MagicMock(return_value=42.0) + pos = self.servo_state.get_position() + self.assertEqual(pos, 42.0) + + def test_servo_state_is_moving_delegates(self): + self.manager.is_servo_moving = MagicMock(return_value=True) + self.assertTrue(self.servo_state.is_moving()) + + def test_servo_state_move_relative_clamps_to_range(self): + self.servo_state.pos = 100 + self.servo_state.range = [0, 110] + self.servo_state.move = MagicMock() + self.servo_state.move_relative(50) # would reach 150, clamps to 110 + self.servo_state.move.assert_called_once_with(110) if __name__ == '__main__': From d802452075c8f593a7357373f9525279d29c787e Mon Sep 17 00:00:00 2001 From: danic85 <6583012+danic85@users.noreply.github.com> Date: Sat, 11 Apr 2026 19:20:00 +0100 Subject: [PATCH 3/4] WIP changes --- environments/cody.yml | 165 +++--- environments/laptop.yml | 158 +++--- src/modules/actuators/bus_servo/README.md | 157 ++---- src/modules/actuators/bus_servo/bus_servo.py | 512 ++++++++---------- src/modules/actuators/bus_servo/config.yml | 2 +- .../bus_servo/libraries/waveshare_backend.py | 89 +-- .../bus_servo/tests/test_bus_servo.py | 171 ++---- src/modules/actuators/tests/bus_servo_test.py | 288 ++++------ src/modules/personality/personality.py | 4 +- 9 files changed, 613 insertions(+), 933 deletions(-) diff --git a/environments/cody.yml b/environments/cody.yml index 44df7a8d..59e45caa 100644 --- a/environments/cody.yml +++ b/environments/cody.yml @@ -31,85 +31,90 @@ bus_servo: - sit_edge_swing_l: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 243.5, 'leg_r_ankle': 251.0, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.9, 'leg_l_knee': 157.2, 'leg_l_ankle': 176.2} - sit_edge_swing_r: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 195.3, 'leg_r_ankle': 220.2, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.6, 'leg_l_knee': 118.2, 'leg_l_ankle': 134.7} - stand_low_narrow: {'leg_r_tilt': 221.0989010989011, 'leg_r_hip': 38.417582417582416, 'leg_r_knee': 72.17582417582418, 'leg_r_ankle': 258.1978021978022, 'neck_pan': 150.24175824175825, 'neck_tilt': 34.604105571847505, 'leg_l_tilt': 160.26373626373626, 'leg_l_hip': 235.07692307692307, 'leg_l_knee': 284.65934065934067, 'leg_l_ankle': 107.86813186813187} - servos: - - name: "leg_r_tilt" - model: 'ST3215' - id: 1 - range: [220.9, 346.6] # converted from raw (4095 = 360 degrees) - start: 246.9 - # calibrate_on_boot: true - - name: "leg_r_hip" - model: 'ST3215' - id: 3 - range: [0.0, 181.3] - # calibrate_on_boot: true - # start: 148.4 - - name: "leg_r_knee" - model: 'ST3215' - id: 2 - range: [62.9, 295.7] - # start: 118.2 - - name: "leg_r_ankle" - model: 'ST3215' - id: 6 - range: [216.1, 340.5] - # start: 243.0 - - name: "neck_pan" - model: 'ST3215' - id: 12 - range: [0.0, 299.1] # converted from raw (4095 = 360 degrees) - start: 149.9 - # calibrate_on_boot: true - # demonstrate_on_boot: true - # - name: "neck_pan" - # model: 'SC09' - # baudrate: 115200 - # id: 4 - # range: [0.0, 300.0] - # start: 150.0 - # calibrate_on_boot: true - # demonstrate_on_boot: true - # speed: 60 # tested up to 600 - - name: "neck_tilt" - model: 'SC09' - baudrate: 115200 - id: 11 - range: [8.8, 64.5] # converted from raw (1023 = 300 degrees) - start: 35.2 - speed: 60 # max=0, 1 - 3000 tested - # acceleration: 0 # max (no significant difference between this and 1 - 3000) - # demonstrate_on_boot: true - # - name: "neck_tilt2" - # model: 'SC09' - # baudrate: 115200 - # id: 5 - # range: [0.0, 179.0] # converted from raw (1023 = 300 degrees) - # start: 89.5 - # speed: 300 - # acceleration: 1 - # demonstrate_on_boot: true - - name: "leg_l_tilt" - model: 'ST3215' - id: 7 - range: [0.7, 161] # converted from raw (4095 = 360 degrees) - start: 142.1 - # calibrate_on_boot: true - - name: "leg_l_hip" - model: 'ST3215' - id: 8 - range: [74.9, 279.2] - # start: 215.4 - - name: "leg_l_knee" - model: 'ST3215' - id: 9 - range: [117.1, 299.3] - # start: 232.2 - - name: "leg_l_ankle" - model: 'ST3215' - id: 10 - range: [53.9, 199.3] - # start: 137.6 - # demonstrate_on_boot: true + instances: + - name: "leg_r_tilt" + model: 'ST3215' + id: 1 + range: [212.4, 315.69] # converted from raw (4095 = 360 degrees) + start: 242.9 + # calibrate_on_boot: true + - name: "leg_r_hip" + model: 'ST3215' + id: 3 + range: [0.0, 101.3] # Need to re-center + # calibrate_on_boot: true + start: 65 + - name: "leg_r_knee" + model: 'ST3215' + id: 2 + range: [52.13, 296.7] + # calibrate_on_boot: true + # start: 55.3 + - name: "leg_r_ankle" + model: 'ST3215' + id: 6 + range: [111.74, 323.16] + # calibrate_on_boot: true + # start: 256.35 + - name: "neck_pan" + model: 'ST3215' + id: 12 + range: [0.0, 299.1] # converted from raw (4095 = 360 degrees) + start: 149.9 + # calibrate_on_boot: true + # demonstrate_on_boot: true + # - name: "neck_pan" + # model: 'SC09' + # baudrate: 115200 + # id: 4 + # range: [0.0, 300.0] + # start: 150.0 + # calibrate_on_boot: true + # demonstrate_on_boot: true + # speed: 60 # tested up to 600 + - name: "neck_tilt" + model: 'SC09' + baudrate: 115200 + id: 11 + range: [8.8, 64.5] # converted from raw (1023 = 300 degrees) + start: 35.2 + speed: 60 # max=0, 1 - 3000 tested + # acceleration: 0 # max (no significant difference between this and 1 - 3000) + # demonstrate_on_boot: true + # - name: "neck_tilt2" + # model: 'SC09' + # baudrate: 115200 + # id: 5 + # range: [0.0, 179.0] # converted from raw (1023 = 300 degrees) + # start: 89.5 + # speed: 300 + # acceleration: 1 + # demonstrate_on_boot: true + - name: "leg_l_tilt" + model: 'ST3215' + id: 7 + range: [53.01, 160.44] # converted from raw (4095 = 360 degrees) + start: 137.32 + # calibrate_on_boot: true + - name: "leg_l_hip" + model: 'ST3215' + id: 8 + range: [144.35, 336] # Need to re-center + # calibrate_on_boot: true + start: 214.33 + - name: "leg_l_knee" + model: 'ST3215' + id: 9 + range: [75.78, 299.3] + # calibrate_on_boot: true + # start: 299 + - name: "leg_l_ankle" + model: 'ST3215' + id: 10 + range: [47.47, 262.15] + # calibrate_on_boot: true + # start: 113.58 + # demonstrate_on_boot: true chatgpt: enabled: true @@ -335,7 +340,7 @@ oled_display: personality: enabled: true inject: - servos: ServoManager + servos: "Servo_*" imu.head: BNO055_imu_head imu.body: BNO055_imu_body vision: Vision diff --git a/environments/laptop.yml b/environments/laptop.yml index 2f4ab557..89517bc9 100644 --- a/environments/laptop.yml +++ b/environments/laptop.yml @@ -30,82 +30,82 @@ bus_servo: - sit_edge: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 243.5, 'leg_r_ankle': 251.0, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.7, 'leg_l_knee': 117.1, 'leg_l_ankle': 137.2} - sit_edge_swing_l: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 243.5, 'leg_r_ankle': 251.0, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.9, 'leg_l_knee': 157.2, 'leg_l_ankle': 176.2} - sit_edge_swing_r: {'leg_r_tilt': 245.3, 'leg_r_hip': 168.7, 'leg_r_knee': 195.3, 'leg_r_ankle': 220.2, 'neck_pan': 149.8, 'neck_tilt': 36.6, 'leg_l_tilt': 138.7, 'leg_l_hip': 101.6, 'leg_l_knee': 118.2, 'leg_l_ankle': 134.7} - servos: - - name: "leg_r_tilt" - model: 'ST3215' - id: 1 - range: [220.9, 346.6] # converted from raw (4095 = 360 degrees) - start: 246.9 - # calibrate_on_boot: true - - name: "leg_r_hip" - model: 'ST3215' - id: 3 - range: [0.0, 181.3] - # calibrate_on_boot: true - # start: 148.4 - - name: "leg_r_knee" - model: 'ST3215' - id: 2 - range: [62.9, 295.7] - # start: 118.2 - - name: "leg_r_ankle" - model: 'ST3215' - id: 6 - range: [216.1, 340.5] - # start: 243.0 - - name: "neck_pan" - model: 'ST3215' - id: 12 - range: [0.0, 299.1] # converted from raw (4095 = 360 degrees) - start: 149.9 - # calibrate_on_boot: true - # demonstrate_on_boot: true - # - name: "neck_pan" - # model: 'SC09' - # baudrate: 115200 - # id: 4 - # range: [0.0, 300.0] - # start: 150.0 - # calibrate_on_boot: true - # demonstrate_on_boot: true - # speed: 60 # tested up to 600 - - name: "neck_tilt" - model: 'SC09' - baudrate: 115200 - id: 11 - range: [8.8, 64.5] # converted from raw (1023 = 300 degrees) - start: 35.2 - speed: 60 # max=0, 1 - 3000 tested - # acceleration: 0 # max (no significant difference between this and 1 - 3000) - # demonstrate_on_boot: true - # - name: "neck_tilt2" - # model: 'SC09' - # baudrate: 115200 - # id: 5 - # range: [0.0, 179.0] # converted from raw (1023 = 300 degrees) - # start: 89.5 - # speed: 300 - # acceleration: 1 - # demonstrate_on_boot: true - - name: "leg_l_tilt" - model: 'ST3215' - id: 7 - range: [0.7, 157.8] # converted from raw (4095 = 360 degrees) - start: 142.1 - # calibrate_on_boot: true - - name: "leg_l_hip" - model: 'ST3215' - id: 8 - range: [74.9, 279.2] - # start: 215.4 - - name: "leg_l_knee" - model: 'ST3215' - id: 9 - range: [117.1, 299.3] - # start: 232.2 - - name: "leg_l_ankle" - model: 'ST3215' - id: 10 - range: [53.9, 199.3] - # start: 137.6 - # demonstrate_on_boot: true \ No newline at end of file + instances: + - name: "leg_r_tilt" + model: 'ST3215' + id: 1 + range: [220.9, 346.6] # converted from raw (4095 = 360 degrees) + start: 246.9 + # calibrate_on_boot: true + - name: "leg_r_hip" + model: 'ST3215' + id: 3 + range: [0.0, 181.3] + # calibrate_on_boot: true + # start: 148.4 + - name: "leg_r_knee" + model: 'ST3215' + id: 2 + range: [62.9, 295.7] + # start: 118.2 + - name: "leg_r_ankle" + model: 'ST3215' + id: 6 + range: [216.1, 340.5] + # start: 243.0 + - name: "neck_pan" + model: 'ST3215' + id: 12 + range: [0.0, 299.1] # converted from raw (4095 = 360 degrees) + start: 149.9 + # calibrate_on_boot: true + # demonstrate_on_boot: true + # - name: "neck_pan" + # model: 'SC09' + # baudrate: 115200 + # id: 4 + # range: [0.0, 300.0] + # start: 150.0 + # calibrate_on_boot: true + # demonstrate_on_boot: true + # speed: 60 # tested up to 600 + - name: "neck_tilt" + model: 'SC09' + baudrate: 115200 + id: 11 + range: [8.8, 64.5] # converted from raw (1023 = 300 degrees) + start: 35.2 + speed: 60 # max=0, 1 - 3000 tested + # acceleration: 0 # max (no significant difference between this and 1 - 3000) + # demonstrate_on_boot: true + # - name: "neck_tilt2" + # model: 'SC09' + # baudrate: 115200 + # id: 5 + # range: [0.0, 179.0] # converted from raw (1023 = 300 degrees) + # start: 89.5 + # speed: 300 + # acceleration: 1 + # demonstrate_on_boot: true + - name: "leg_l_tilt" + model: 'ST3215' + id: 7 + range: [0.7, 157.8] # converted from raw (4095 = 360 degrees) + start: 142.1 + # calibrate_on_boot: true + - name: "leg_l_hip" + model: 'ST3215' + id: 8 + range: [74.9, 279.2] + # start: 215.4 + - name: "leg_l_knee" + model: 'ST3215' + id: 9 + range: [117.1, 299.3] + # start: 232.2 + - name: "leg_l_ankle" + model: 'ST3215' + id: 10 + range: [53.9, 199.3] + # start: 137.6 + # demonstrate_on_boot: true \ No newline at end of file diff --git a/src/modules/actuators/bus_servo/README.md b/src/modules/actuators/bus_servo/README.md index 82427424..632827f8 100644 --- a/src/modules/actuators/bus_servo/README.md +++ b/src/modules/actuators/bus_servo/README.md @@ -2,144 +2,99 @@ ## Overview -Serial bus servos allow for efficient communication and control of multiple servos over a single bus. The `ServoManager` class provides a centralised, high-level interface to manage all servos on a bus, including setting positions, coordinating multi-servo motions, and handling configuration. The hardware interface is backend-agnostic, supporting multiple libraries and simulation. +Serial bus servos allow for efficient communication and control of multiple servos over a single bus. The `Servo` class provides a high-level interface to manage these servos, including setting positions, speeds, and handling configurations. The hardware interface is now fully backend-agnostic, supporting multiple libraries and simulation. ## Architecture -### ServoManager +The bus_servo module now supports multiple backends: -`ServoManager` is the single `BaseModule` responsible for all bus servo operations: +- **Waveshare**: Uses the official Waveshare ST/SC SDKs for hardware control. +- **Rustypot**: Uses the rustypot library for Feetech and compatible servos. +- **Simulation**: A software-only backend for debugging and development, which logs all actions instead of controlling hardware. -- Owns one shared hardware controller per physical serial bus (port + baudrate). Multiple servos on the same bus open the port **only once**, avoiding contention. -- Maintains a registry of lightweight `ServoState` objects keyed by servo name. -- Handles all messaging subscriptions for every managed servo. -- Processes per-servo move queues each system-loop cycle. -- Provides high-level APIs for coordinated motion: `move_to_pose()`, `group_move()`. +You can select the backend in your configuration. All backends implement the same interface, so you can switch between them without changing your code. -### ServoState +All servo configuration (range, start, poses) is now in **degrees** for clarity and cross-backend compatibility. -`ServoState` is a lightweight per-servo holder: +## Configuration -- Stores only configuration and logical state: name, ID, model, range, speed, acceleration, current position, move queue, and poses. -- Does **not** own the serial port or controller. -- Delegates every hardware operation (move, read position, torque enable/disable) to the owning `ServoManager`. +The configuration file (e.g. `environments/cody.yml`) contains the configuration for each bus servo. The `instances` section defines the servos, their IDs, and their initial positions, all in degrees. The `poses` section defines named poses, also in degrees. -### Backends +## Getting Started / Calibration -| Backend | Class | Notes | -|---------|-------|-------| -| `waveshare` | `WaveshareBusServo` | Official Waveshare ST/SC SDKs (included). Port shared via class-level singleton. | -| `rustypot` | `RustypotBusServo` | Rustypot library for Feetech-compatible servos. | -| `simulation` | `SimulationBusServo` | Software-only, logs all actions; no hardware required. | +To calibrate the servos, each must have their ID set individually. To achieve this, connect one servo to the driver board and run `modules/actuators/bus_servo/change_id.py`. This script will prompt you to enter the ID for the connected servo, which will then be saved permanently on the servo. -All backends implement the same `BusServoBase` interface. +If you have challenges understanding the current ID of the servo, run `modules/actuators/bus_servo/libraries/waveshare/STServo_examples/read_all.py` or `modules/actuators/bus_servo/libraries/waveshare/SCServo_examples/read_all.py` depending on the servo type. This script will read and display the current ID of the connected servo. -## Configuration +Once the ID has been set for all servos, you can use the `Servo` class to control them by enabling it in the environment yaml file. -Servos are declared in the environment YAML file (e.g. `environments/cody.yml`) under a single `bus_servo` entry. All servo instances are listed under `config.servos`; the `ServoManager` is instantiated **once** and manages all of them. +### Centering +The servos raw value range is between 0-4095 for ST servos and 0-1024 for SC servos, this equates to 360 and 300 degrees respectively. The position readout can wrap around, so the servos should be set to the midpoint before mounting them in the robot otherwise it can result in the servo passing the wrong way through the range of values, which could damage the robot. -```yaml -bus_servo: - enabled: true - config: - backend: 'waveshare' # waveshare | rustypot | simulation - poses: - - stand: {leg_r_tilt: 246.7, leg_l_tilt: 142.6, ...} - servos: - - name: leg_r_tilt - model: ST3215 - id: 1 - range: [220.9, 346.6] - start: 246.9 - - name: neck_tilt - model: SC09 - baudrate: 115200 # overrides the default baudrate for this servo - id: 11 - range: [8.8, 64.5] - start: 35.2 - speed: 60 -``` +The `change_id.py` script will set the servo to the midpoint when changing the ID. The servo should then be mounted in the robot at roughly the midpoint position. -## Injecting into other modules +You can also enable `center_on_boot` for each servo in the configuration file, which will move the servo to the center of its range on initialization. -Inject the `ServoManager` into modules that need direct servo access: -```yaml -my_module: - enabled: true - inject: - servos: ServoManager -``` +### Calibration +To calibrate the servo positions, set the flag `calibrate_on_boot` to `true` in the configuration file for each instance (servo). This will cause the servo to output it's current position in the debug log, which can then be copied into the start position, or range. Servos can be manually moved to any position to identify their range or certain poses. -Because `ServoManager` exposes a dict-like interface, existing code that accesses servos via `self.servos[name]` continues to work unchanged: +Finally, set `calibrate_on_boot` to false and re-run the program to start using the servos with their configured positions. -```python -self.servos['neck_tilt'].move_relative(pitch) -self.servos['neck_pan'].move(150) -for name, servo in self.servos.items(): - servo.detach() -``` - -## Messaging +### Demonstration -The manager subscribes automatically for every servo listed in `config.servos`: +To demonstrate the servo movement on boot, set the flag `demonstrate_on_boot` to `true` in the configuration file for each instance (servo). This will cause the servo to move to its minimum and maximum positions once on initialization, allowing you to see the range of motion. -| Topic | Action | -|-------|--------| -| `servo::mvabs` | Queue absolute move (degrees) | -| `servo::mv` | Queue relative move (delta degrees) | -| `servo::queue` | Alias for absolute move | -| `servo/pose` | Move all servos to a named pose | +## Subscriptions and direct command -## Coordinated motion +The `Servo` class subscribes to the following topics: +`servo::mv` - to move the servo to a specific position relative to it's current position. +`servo::mvabs` - to move the servo to a specific position with absolute values. -Use `group_move()` to queue moves for multiple servos in one call: +The environment configuration can also inject the servos into a module for direct control: -```python -self.servo_manager.group_move({ - 'leg_l_hip': 180, - 'leg_r_hip': 180, - 'leg_l_knee': 90, - 'leg_r_knee': 90, -}) ``` - -Or publish a named pose to move all servos simultaneously: - -```python -self.publish('servo/pose', pose_name='stand_low') +my_module: + enabled: true + inject: + servos: "Servo_*" ``` -## Getting Started / Calibration - -To calibrate the servos, each must have their ID set individually. Connect one servo to the driver board and run `modules/actuators/bus_servo/change_id.py`. This script will prompt you to enter the ID for the connected servo. +You can then directly call the servo such as: -If you need to discover a servo's current ID, run `modules/actuators/bus_servo/libraries/waveshare/STServo_examples/read_all.py` (ST) or the SC equivalent. +``` +self.servos['neck_tilt'].move_relative(pitch) +self.servos['neck_tilt'].move(pitch) +``` -### Centering +## Smooth initialization -Servo raw values range from 0–4095 (ST, 360°) or 0–1024 (SC, 300°). Set servos to their midpoint before mounting to avoid wrapping through the wrong direction. +Because the `Servo` class gets the current position of the servo on initialization, there is no danger of a servo jumping from an unknown position to the start position. This is especially useful when the servos are powered on in a random position and is an advantage over hobby servos. -### Calibration via `calibrate_on_boot` +## SC vs ST servos -Set `calibrate_on_boot: true` for a servo in the config to continuously log its current position. Move the servo manually, then copy min/max values into `range`. Disable the flag when done. +The `Servo` class supports both ST and SC series servos from Waveshare, Feetech and compatible servos. The type of servo is determined by the `model` variable in the configuration file, and the backend is selected via the `backend` parameter. The class will automatically use the appropriate backend for the specified servo type. -### Demonstration +There are some limitations to the SC servos as they do not support continuous rotation and have a lower rotational range compared to the ST servos. The simulation backend is useful for development and debugging without hardware. -Set `demonstrate_on_boot: true` to sweep a servo through its full range on startup. +## Backends -## SC vs ST servos +- **WaveshareBusServo**: Uses the official Waveshare SDKs for ST/SC servos (this library is included in this repo). +- **RustypotBusServo**: Uses the rustypot library for Feetech and compatible servos (this library is referenced via python import). +- **SimulationBusServo**: Simulates servo behavior and logs all actions for debugging (no library required). -Both families are supported via the same backend; the `model` field selects the correct SDK. ST servos (e.g. ST3215) have 360° range and support continuous-rotation mode. SC servos (e.g. SC09) have a 300° range. +All backends implement the same methods, including movement, speed, torque, calibration, and error handling. -## Notes +## Notes during testing -- Speed: 0 = max on SC servos; 1–3000 slows it down. -- Overload errors on SC servos can sometimes be resolved by toggling torque off/on. +- Speed: between 0 and 3000 for SC servos tested, 0 is max, then 1-3000 increases speed. +- Acceleration: between 0 and 3000 for SC servos tested, 0 is max but no significant difference observed between values. +- Overload error occurring fairly frequently on SC servos using Waveshare library. However when testing with scheduled movements the issue is far less frequent. This may be due to sending commands too quickly in succession. Cycling torque off/on seems to resolve the error, otherwise you need to power cycle the servo. +- To disable torque on SC servos use: `self.packetHandler.write1ByteTxRx(self.portHandler, self.index, ADDR_TORQUE_ENABLE, 0)`, change the 0 to 1 to enable torque. +- In some cases the overload error does seem to be caused by load on the servo, in which case toggling the torque does not resolve this. The neck pan is an example of this where the SC servo needed to be replaced with the more powerful ST servo. ## References - -- https://www.waveshare.com/wiki/ST3215_Servo -- https://www.waveshare.com/wiki/SC09_Servo -- https://www.waveshare.com/wiki/Bus_Servo_Adapter_(A) -- https://github.com/pollen-robotics/rustypot +https://www.waveshare.com/wiki/ST3215_Servo +https://www.waveshare.com/wiki/SC09_Servo +https://www.waveshare.com/wiki/Bus_Servo_Adapter_(A) +https://github.com/pollen-robotics/rustypot \ No newline at end of file diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py index a7e573bc..9e098c25 100644 --- a/src/modules/actuators/bus_servo/bus_servo.py +++ b/src/modules/actuators/bus_servo/bus_servo.py @@ -6,44 +6,99 @@ import time from modules.base_module import BaseModule from modules.actuators.bus_servo.libraries.factory import BusServoFactory - - -class ServoState: - """ - Lightweight servo configuration and state holder. - - Stores per-servo identity and logical state (position, queue) only. - All hardware operations are delegated to the owning ServoManager so - that a single shared controller instance handles every servo on a bus. - """ - - def __init__(self, manager, **kwargs): - self._manager = manager +class Servo(BaseModule): + def __init__(self, **kwargs): + """ + Servo class + """ + self.backend = kwargs.get('backend', 'waveshare') + print(f"Creating servo with backend {self.backend}") self.identifier = kwargs.get('name') self.model = kwargs.get('model', 'ST') self.index = kwargs.get('id') self.range = kwargs.get('range') - self.start = kwargs.get('start') - self.port = kwargs.get('port', '/dev/ttyAMA0') + self.start = kwargs.get('start') # Default start position + self.poses = kwargs.get('poses') # Dictionary of poses self.baudrate = kwargs.get('baudrate', 1000000) - self.speed = kwargs.get('speed', 300) - self.acceleration = kwargs.get('acceleration', 50) + self.port = kwargs.get('port', '/dev/ttyAMA0') # Change as needed, find with `ls /dev/ttyAMA*` + self.calibrate_on_boot = kwargs.get('calibrate_on_boot', False) # Loop to show position for manual configuration + self.demonstrate_on_boot = kwargs.get('demonstrate_on_boot', False) # Move to min and max to demonstrate range + self.center_on_boot = kwargs.get('center_on_boot', False) # Move to center of range on boot self.pos = None + self.speed = kwargs.get('speed', 300) # 3073 + self.acceleration = kwargs.get('acceleration', 50) self._move_queue = collections.deque() + poses_list = kwargs.get('poses', []) + self.poses = {list(pose.keys())[0]: list(pose.values())[0] for pose in poses_list} + + # Backend selection + + self.backend_servo = BusServoFactory.create( + backend=self.backend, + model=self.model, + servo_id=self.index, + port=self.port, + baudrate=self.baudrate, + range=self.range, + ) + + def detach(self): + # Detach servo using backend + self.backend_servo.detach() + + def exit(self): + self.detach() + self.backend_servo.exit() - # Accept either a pre-converted poses dict or a list of single-key dicts - poses = kwargs.get('poses', []) - if isinstance(poses, dict): - self.poses = poses + def setup_messaging(self): + self.subscribe('servo:' + self.identifier + ':mvabs', self.move) + self.subscribe('servo:' + self.identifier + ':mv', self.move_relative) + self.subscribe('servo:' + self.identifier + ':queue', self.move) + self.subscribe('system/exit', self.exit) + self.subscribe('servo/pose', self.move_to_pose) + + if self.calibrate_on_boot: + self.calibrate_dynamic() # Log will show current position repeatedly to help with manual configuration + + self.pos = self.get_position() # Get initial position to avoid jumping from unknown position + + if self.center_on_boot: + self.calibrate_to_center() + + if self.demonstrate_on_boot: + self.log(f"Demonstrating servo {self.identifier} movement, speed={self.speed}, acceleration={self.acceleration}") + if self.range is not None: + self.move(self.range[0]) # Move to min range + self.move((self.range[0] + self.range[1]) // 2) # Move to center + self.move(self.range[1]) # Move to max range + else: + self.log(f"Range not set for servo {self.identifier}, cannot demonstrate movement", level='warning') + + # Move to start position + # if self.get_pose_value('stand') is not None: + # self.start = self.get_pose_value('stand') + if self.start is not None: + self.move(self.start) + + def move_to_pose(self, pose_name): + # print(self.poses) + pose_value = self.poses.get(pose_name) + # print(f"{self.identifier} - Pose '{pose_name}' value: {pose_value}") + my_pose_value = pose_value.get(self.identifier) + print(f"Moving servo {self.identifier} to pose '{pose_name}' with value {my_pose_value}") + if my_pose_value is not None: + self.move(my_pose_value) else: - self.poses = {list(p.keys())[0]: list(p.values())[0] for p in poses} - - # ------------------------------------------------------------------ - # Queue helpers - # ------------------------------------------------------------------ - + self.log(f"Pose '{pose_name}' not found for servo {self.identifier}", level='warning') + def move(self, position, speed=None, acceleration=None, delay=0, **kwargs): - """Queue a move to an absolute position (degrees).""" + """ + Add a move request to the queue. + :param position: Target position + :param speed: Optional speed override + :param acceleration: Optional acceleration override + :param delay: Optional delay in seconds before executing (for animation) + """ self._move_queue.append({ 'position': position, 'speed': speed if speed is not None else self.speed, @@ -52,293 +107,158 @@ def move(self, position, speed=None, acceleration=None, delay=0, **kwargs): 'delay': delay, }) - def move_relative(self, delta): - """Queue a relative move from the current position.""" - if self.pos is None: - return - new_position = round(self.pos + delta) - if self.range: - if new_position < self.range[0] or new_position > self.range[1]: - new_position = self.range[0] if new_position < self.range[0] else self.range[1] - self.move(new_position) - - # ------------------------------------------------------------------ - # Hardware delegates → ServoManager - # ------------------------------------------------------------------ - - def detach(self): - """Disable torque via the manager.""" - self._manager.detach_servo(self.identifier) - - def attach(self): - """Enable torque via the manager.""" - self._manager.attach_servo(self.identifier) - - def get_position(self): - """Return the current hardware position (degrees) via the manager.""" - return self._manager.get_servo_position(self.identifier) - - def is_moving(self): - """Return True if the servo is still moving (via the manager).""" - return self._manager.is_servo_moving(self.identifier) - - def calibrate_to_center(self): - """Move the servo to the centre of its range via the manager.""" - self._manager.calibrate_servo_to_center(self.identifier) - - -class ServoManager(BaseModule): - """ - Central manager for all bus servos. - - Owns the shared hardware backend controller(s) and manages every servo - operation, including coordinated group moves and pose transitions. - - ServoState instances are lightweight holders that delegate all hardware - access through this manager, ensuring: - - a single serial port is opened per bus; - - motion commands can be batched, sequenced, or synchronised; - - all hardware access is serialised through one place. - - The manager exposes a dict-like interface (``__getitem__``, ``items()``, - ``values()``, ``get()``) so existing code that treats the injected - ``servos`` attribute as a ``{name: servo}`` mapping continues to work - without modification. - """ - - def __init__(self, **kwargs): - self.backend = kwargs.get('backend', 'waveshare') - self._default_port = kwargs.get('port', '/dev/ttyAMA0') - self._default_baudrate = kwargs.get('baudrate', 1000000) - self._default_speed = kwargs.get('speed', 300) - self._default_acceleration = kwargs.get('acceleration', 50) - - # Shared poses: {pose_name: {servo_name: position_degrees}} - poses_list = kwargs.get('poses', []) - self._poses = {list(p.keys())[0]: list(p.values())[0] for p in poses_list} - - self._servos = {} # {name: ServoState} - self._servo_backends = {} # {name: BusServoBase} - - for servo_cfg in kwargs.get('servos', []): - # Apply manager-level defaults where the servo config does not - # supply its own value, then override with servo-specific values. - full_cfg = { - 'port': self._default_port, - 'baudrate': self._default_baudrate, - 'speed': self._default_speed, - 'acceleration': self._default_acceleration, - **servo_cfg, - 'poses': self._poses, # share the global poses dict - } - servo_state = ServoState(manager=self, **full_cfg) - self._servos[servo_state.identifier] = servo_state - - # Create per-servo backend. Port/controller sharing is handled - # transparently by class-level singletons inside each backend class. - backend = BusServoFactory.create( - backend=self.backend, - model=servo_state.model, - servo_id=servo_state.index, - port=servo_state.port, - baudrate=servo_state.baudrate, - range=servo_state.range, - ) - self._servo_backends[servo_state.identifier] = backend - - # ------------------------------------------------------------------ - # Dict-like interface for backward-compatible injection - # ------------------------------------------------------------------ - - def __getitem__(self, key): - return self._servos[key] - - def __iter__(self): - return iter(self._servos) - - def __contains__(self, key): - return key in self._servos - - def items(self): - return self._servos.items() - - def values(self): - return self._servos.values() - - def get(self, key, default=None): - return self._servos.get(key, default) - - # ------------------------------------------------------------------ - # BaseModule hooks - # ------------------------------------------------------------------ - - def setup_messaging(self): - """Subscribe to per-servo and global topics for all managed servos.""" - for name, servo in self._servos.items(): - self.subscribe(f'servo:{name}:mvabs', servo.move) - self.subscribe(f'servo:{name}:mv', servo.move_relative) - self.subscribe(f'servo:{name}:queue', servo.move) - self.subscribe('servo/pose', self.move_to_pose) - self.subscribe('system/exit', self.exit) - - # Initialise positions and queue start moves - for name, servo in self._servos.items(): - try: - servo.pos = self.get_servo_position(name) - except Exception: - if servo.start is not None: - servo.pos = servo.start - elif servo.range: - servo.pos = servo.range[0] - else: - servo.pos = 0 - self.log( - f"No range or start configured for servo {name}; " - f"initialising position to 0", - level='warning', - ) - if servo.start is not None: - servo.move(servo.start) - def loop(self): - """Process every servo's move queue once per system-loop cycle.""" - for servo in self._servos.values(): - self._process_servo_queue(servo) + """Called every system loop cycle to drain the move queue.""" + self._process_queue() - # ------------------------------------------------------------------ - # Internal queue processing - # ------------------------------------------------------------------ - - def _process_servo_queue(self, servo): - """Drain one item from the given servo's queue if the servo is idle.""" - if not servo._move_queue: + def _process_queue(self, **kwargs): + """ + Process the next item in the move queue if the servo is not moving. + Called every loop cycle. + """ + if not self._move_queue: return - if self.is_servo_moving(servo.identifier): + if self.is_moving(): return - next_item = servo._move_queue[0] + next_item = self._move_queue[0] if time.time() - next_item['timestamp'] >= next_item['delay']: - servo._move_queue.popleft() - self._do_move(servo, next_item['position'], - next_item['speed'], next_item['acceleration']) + self._move_queue.popleft() + self._do_move(next_item['position'], next_item['speed'], next_item['acceleration']) - def _do_move(self, servo, position, speed=None, acceleration=None): - """Execute a single hardware move for the given ServoState.""" + def _do_move(self, position, speed=None, acceleration=None): + """ + Move the servo to an absolute position. + :param position: Position to move to + :param speed: Optional speed override + :param acceleration: Optional acceleration override + """ if position is None: - self.log(f"Position is None for servo {servo.identifier}, cannot move", - level='error') - return - if servo.range and (position < servo.range[0] or position > servo.range[1]): - self.log( - f"Position {position} out of range " - f"({servo.range[0]}-{servo.range[1]})", - level='error', - ) - return - backend = self._servo_backends.get(servo.identifier) - if backend is None: + self.log(f"Position is None for servo {self.identifier}, cannot move", level='error') return - if speed is not None and hasattr(backend, 'set_speed'): - backend.set_speed(speed) - if acceleration is not None and hasattr(backend, 'set_acceleration'): - backend.set_acceleration(acceleration) - backend.move_to(position, unit='degrees') - servo.pos = position - - # ------------------------------------------------------------------ - # High-level coordination APIs - # ------------------------------------------------------------------ - - def move_to_pose(self, pose_name): - """Queue moves for all servos to the positions defined in the named pose.""" - pose_values = self._poses.get(pose_name, {}) - if not pose_values: - self.log(f"Pose '{pose_name}' not found", level='warning') + if position < self.range[0] or position > self.range[1]: + self.log(f"Position {position} out of range ({self.range[0]}-{self.range[1]})", level='error') return - for servo_name, position in pose_values.items(): - if servo_name in self._servos: - self._servos[servo_name].move(position) - - def group_move(self, moves): + # Apply per-move speed/acceleration overrides if supported by backend + if speed is not None and hasattr(self.backend_servo, 'set_speed'): + self.backend_servo.set_speed(speed) + if acceleration is not None and hasattr(self.backend_servo, 'set_acceleration'): + self.backend_servo.set_acceleration(acceleration) + # Delegate to backend + self.backend_servo.move_to(position, unit='degrees') + self.pos = position + + def move_relative(self, delta): """ - Queue coordinated moves for multiple servos. - - :param moves: ``{servo_name: position}`` dict or iterable of - ``(servo_name, position)`` pairs. + Move the servo relative to its current position. + :param delta: Change in position (can be negative) """ - if isinstance(moves, dict): - moves = moves.items() - for servo_name, position in moves: - if servo_name in self._servos: - self._servos[servo_name].move(position) - - # ------------------------------------------------------------------ - # Per-servo hardware delegates - # ------------------------------------------------------------------ - - def get_servo_position(self, servo_name): - """Return the current hardware position (degrees) of the named servo.""" - backend = self._servo_backends.get(servo_name) - if backend is None: - return None - return backend.get_position(unit='degrees') - - def is_servo_moving(self, servo_name): - """Return True if the named servo is still moving.""" - servo = self._servos.get(servo_name) - backend = self._servo_backends.get(servo_name) - if servo is None or backend is None: - return False + # self.log(f"Moving servo {self.identifier} from {self.pos} by delta {delta}") + new_position = round(self.pos + delta) + if new_position < self.range[0] or new_position > self.range[1]: + self.log(f"Position {new_position} out of range ({self.range[0]}-{self.range[1]}). Adjusting", level='warning') + new_position = self.range[0] if new_position < self.range[0] else self.range[1] + + # Move to new position + self.move(new_position) + + + def is_moving(self): try: - moving = backend.get_moving() + moving = self.backend_servo.get_moving() except Exception as e: - self.log(f"Exception in get_moving for servo {servo_name}: {e}", - level='error') + self.log(f"Exception in get_moving for servo {self.identifier}: {e}", level='error') return False if moving == 1: return True - if servo.pos is not None: - try: - pos = self.get_servo_position(servo_name) - if pos is not None and abs(servo.pos - pos) > 2: - self.log( - f"Servo {servo_name} not reported as moving but position " - f"mismatch: target={servo.pos}, current={pos}", - level='warning', - ) - except Exception: - pass + try: + pos = self.get_position() + except Exception as e: + self.log(f"Exception in get_position for servo {self.identifier}: {e}", level='error') + return False + if abs(self.pos - pos) > 2: + self.log(f"Warning: Servo {self.identifier} is not reporting as moving but position {pos} does not match target position {self.pos}", level='warning') return False + + def get_position(self): + """ + Get the current position of the servo. + """ + return self.backend_servo.get_position(unit='degrees') + + def get_pose_value(self, pose_name): + """ + Returns the position value for the given pose name from self.poses. + """ + if not self.poses: + return None + return self.poses.get(pose_name) - def detach_servo(self, servo_name): - """Disable torque on the named servo.""" - backend = self._servo_backends.get(servo_name) - if backend: - backend.detach() - - def attach_servo(self, servo_name): - """Enable torque on the named servo.""" - backend = self._servo_backends.get(servo_name) - if backend: - backend.attach() - - def calibrate_servo_to_center(self, servo_name): - """Move the named servo to the centre of its configured range.""" - backend = self._servo_backends.get(servo_name) - if backend: - backend.calibrate_to_center() + def calibrate_to_center(self): + """ + Move the servo to the center of its range using the backend implementation. + """ + self.backend_servo.calibrate_to_center() - def detach_all(self): - """Disable torque on every managed servo.""" - for name in self._servos: - self.detach_servo(name) + def calibrate(self): + """ + Move each servo to capture min and max positions for calibration. + """ + self.log(f"Move servo {self.identifier} to minimum position and press any key...") + getch() # Waits for a single key press + min = self.get_position() + self.log(f"Captured minimum position: {min}") + self.log(f"Move servo {self.identifier} to maximum position and press any key...") + getch() # Waits for a single key press + max = self.get_position() + self.log(f"Captured maximum position: {max}") + self.range = (min, max) + if self.start is not None and (self.start < min or self.start > max): + self.start = (min + max) // 2 + self.log(f"Start position {self.start} out of new range, setting to midpoint {self.start}") + self.log(f"Updated range for {self.identifier}: {self.range}. Start position: {self.start}") + + def calibrate_dynamic(self): + """ + Continuously log the current position to help with manual calibration. + Store min an max as they are found. + Complete on key press. and store in self.range + """ + self.log(f"Calibrating servo {self.identifier}. Move the servo to find min and max positions. Press any key to finish...") + self.detach() + min_pos = None + max_pos = None + try: + while True: + pos = round(self.get_position(), 2) + if pos is None: + self.log(f"Failed to get position for servo {self.identifier}", level='warning') + continue + if min_pos is None or pos < min_pos: + min_pos = pos + if max_pos is None or pos > max_pos: + max_pos = pos + # Print on the same line, pad with spaces to clear previous content + # (4095 = 360 degrees, so 1264 = 111 degrees) + range_degrees = max_pos - min_pos if min_pos is not None and max_pos is not None else 'N/A' + print(f"\rCurrent position: {pos}, Min: {min_pos}, Max: {max_pos} Range: {range_degrees}", end='', flush=True) + time.sleep(0.05) + if sys.stdin in select.select([sys.stdin], [], [], 0)[0]: + sys.stdin.read(1) # Consume the key so buffer is cleared + break + except KeyboardInterrupt: + # No need to handle as this is to allow changing selected servo + pass + print() # Move to next line after loop + if min_pos is not None and max_pos is not None: + self.range = (min_pos, max_pos) + self.log(f"Calibration complete for {self.identifier}. Range: {self.range}") + else: + self.log(f"No positions recorded during calibration for {self.identifier}.", level='warning') + + if self.start is not None and (self.start < min_pos or self.start > max_pos): + self.start = (min_pos + max_pos) // 2 + self.log(f"Start position {self.start} out of new range, setting to midpoint {self.start}") - def exit(self, **kwargs): - """Disable torque and release all backends on system exit.""" - self.detach_all() - for backend in self._servo_backends.values(): - try: - backend.exit() - except Exception: - pass + diff --git a/src/modules/actuators/bus_servo/config.yml b/src/modules/actuators/bus_servo/config.yml index bd371b47..83efef60 100644 --- a/src/modules/actuators/bus_servo/config.yml +++ b/src/modules/actuators/bus_servo/config.yml @@ -1,5 +1,5 @@ bus_servo: - class: ServoManager + class: Servo dependencies: python: - rustypot diff --git a/src/modules/actuators/bus_servo/libraries/waveshare_backend.py b/src/modules/actuators/bus_servo/libraries/waveshare_backend.py index 0600172e..5fc3c431 100644 --- a/src/modules/actuators/bus_servo/libraries/waveshare_backend.py +++ b/src/modules/actuators/bus_servo/libraries/waveshare_backend.py @@ -15,68 +15,34 @@ COMM_SUCCESS = 0 # Communication success (matches value in both ST and SC SDKs) class WaveshareBusServo(BusServoBase): - # Class-level cache: one PortHandler + PacketHandler per (port, baudrate, model_type). - # All servo instances sharing a physical bus reuse the same port handle, so the - # serial port is opened only once regardless of how many servos are managed. - _port_singletons = {} # {(port, baudrate, model_type): {portHandler, packetHandler, ...}} - def __init__(self, servo_id, model, port, baudrate=1000000, range=None, range_degrees=None, **kwargs): super().__init__(servo_id, model, port, baudrate, range, **kwargs) self.speed = kwargs.get('speed', 300) self.acceleration = kwargs.get('acceleration', 50) - - model_type = 'ST' if model.startswith('ST') else ('SC' if model.startswith('SC') else None) - if model_type is None: - raise ValueError(f"Unknown model: {model}") - key = (port, baudrate, model_type) - - if key in WaveshareBusServo._port_singletons: - # Reuse the already-open port; just grab the shared handles. - singleton = WaveshareBusServo._port_singletons[key] - self.portHandler = singleton['portHandler'] - self.packetHandler = singleton['packetHandler'] - self.max_raw = singleton['max_raw'] - self.min_raw = singleton['min_raw'] - self.max_deg = singleton['max_deg'] - self.min_deg = singleton['min_deg'] - singleton['refcount'] += 1 + # Import and initialize the correct SDK based on model + if model.startswith('ST'): + from .waveshare.STservo_sdk import PortHandler, sts + self.portHandler = PortHandler(port) + self.packetHandler = sts(self.portHandler) + self.max_raw = 4095 + self.min_raw = 0 + self.max_deg = 360 + self.min_deg = 0 + elif model.startswith('SC'): + from .waveshare.SCservo_sdk import PortHandler, PacketHandler + self.portHandler = PortHandler(port) + self.packetHandler = PacketHandler(1) + self.max_raw = 1023 + self.min_raw = 0 + self.max_deg = 300 + self.min_deg = 0 else: - # First servo on this (port, baudrate, model_type) — open the port. - if model.startswith('ST'): - from .waveshare.STservo_sdk import PortHandler, sts - portHandler = PortHandler(port) - packetHandler = sts(portHandler) - max_raw, min_raw = 4095, 0 - max_deg, min_deg = 360, 0 - elif model.startswith('SC'): - from .waveshare.SCservo_sdk import PortHandler, PacketHandler - portHandler = PortHandler(port) - packetHandler = PacketHandler(1) - max_raw, min_raw = 1023, 0 - max_deg, min_deg = 300, 0 - else: - raise ValueError(f"Unknown model: {model}") - - if not portHandler.openPort(): - raise RuntimeError(f"Failed to open port {port} for servo {servo_id}") - if not portHandler.setBaudRate(baudrate): - raise RuntimeError(f"Failed to set baudrate {baudrate} for servo {servo_id}") - - WaveshareBusServo._port_singletons[key] = { - 'portHandler': portHandler, - 'packetHandler': packetHandler, - 'max_raw': max_raw, - 'min_raw': min_raw, - 'max_deg': max_deg, - 'min_deg': min_deg, - 'refcount': 1, - } - self.portHandler = portHandler - self.packetHandler = packetHandler - self.max_raw = max_raw - self.min_raw = min_raw - self.max_deg = max_deg - self.min_deg = min_deg + raise ValueError(f"Unknown model: {model}") + # Open port and set baudrate + if not self.portHandler.openPort(): + raise RuntimeError(f"Failed to open port {port} for servo {servo_id}") + if not self.portHandler.setBaudRate(baudrate): + raise RuntimeError(f"Failed to set baudrate {baudrate} for servo {servo_id}") def move_to(self, value, unit='degrees'): if unit == 'degrees': @@ -119,14 +85,7 @@ def attach(self): self.packetHandler.write1ByteTxRx(self.portHandler, self.servo_id, ADDR_TORQUE_ENABLE, 1) def exit(self): - model_type = 'ST' if self.model.startswith('ST') else 'SC' - key = (self.port, self.baudrate, model_type) - singleton = WaveshareBusServo._port_singletons.get(key) - if singleton: - singleton['refcount'] -= 1 - if singleton['refcount'] <= 0: - self.portHandler.closePort() - del WaveshareBusServo._port_singletons[key] + self.portHandler.closePort() def move_to_raw(self, raw_value): if self.model.startswith('ST'): diff --git a/src/modules/actuators/bus_servo/tests/test_bus_servo.py b/src/modules/actuators/bus_servo/tests/test_bus_servo.py index 58213e34..e40d5873 100644 --- a/src/modules/actuators/bus_servo/tests/test_bus_servo.py +++ b/src/modules/actuators/bus_servo/tests/test_bus_servo.py @@ -1,6 +1,7 @@ import sys import unittest -from unittest.mock import MagicMock, patch +from unittest.mock import MagicMock +from modules.actuators.bus_servo.bus_servo import Servo from modules.actuators.bus_servo.libraries.simulation_backend import SimulationBusServo from modules.actuators.bus_servo.libraries.factory import BusServoFactory from modules.actuators.bus_servo.libraries.waveshare_backend import WaveshareBusServo @@ -17,8 +18,6 @@ def setUp(self): sts=mock_sts, PortHandler=mock_PortHandler ) - # Clear any cached port singletons from previous tests - WaveshareBusServo._port_singletons.clear() self.servo = WaveshareBusServo(1, 'ST3215', '/dev/ttyUSB0', range=[0, 180]) self.servo.packetHandler = self.mock_packet self.servo.portHandler = MagicMock() @@ -30,16 +29,16 @@ def setUp(self): self.mock_packet.WritePosEx.return_value = (0, 0) self.mock_packet.WheelMode.return_value = (0, 0) self.mock_packet.WriteSpec.return_value = (0, 0) - def tearDown(self): sys.modules.pop('modules.actuators.bus_servo.libraries.waveshare.STservo_sdk', None) sys.modules.pop('numpy', None) - WaveshareBusServo._port_singletons.clear() def test_move_to(self): self.servo.move_to(90, unit='degrees') self.servo.move_to(1.57, unit='radians') self.servo.move_to(2048, unit='raw') + # Should call move_to_raw + # No exception means pass def test_get_position(self): self.servo.get_position_raw = MagicMock(return_value=2048) @@ -57,18 +56,8 @@ def test_attach_detach(self): self.servo.attach() self.servo.detach() - def test_exit_decrements_refcount(self): - # After __init__ the refcount should be 1 in the singleton - model_type = 'ST' - key = (self.servo.port, self.servo.baudrate, model_type) - # Patch portHandler.closePort so it doesn't error - self.servo.portHandler.closePort = MagicMock() - initial_refcount = WaveshareBusServo._port_singletons.get(key, {}).get('refcount', 0) + def test_exit(self): self.servo.exit() - # Either the key is removed (refcount hit 0) or refcount decreased - if key in WaveshareBusServo._port_singletons: - self.assertLess(WaveshareBusServo._port_singletons[key]['refcount'], initial_refcount) - # No exception = pass def test_move_to_raw(self): self.servo.move_to_raw(1234) @@ -115,28 +104,14 @@ def test_calibrate_to_center(self): self.servo.model = 'ST3215' self.servo.calibrate_to_center() - def test_shared_port_singleton(self): - """Two servos on the same port/baudrate/model share one portHandler.""" - # First servo was created in setUp. Create a second on the same bus. - second = WaveshareBusServo(2, 'ST3215', '/dev/ttyUSB0', range=[0, 180]) - key = ('/dev/ttyUSB0', 1000000, 'ST') - singleton_port = WaveshareBusServo._port_singletons[key]['portHandler'] - # Both instances must use the singleton's portHandler (not distinct ones) - self.assertIs(second.portHandler, singleton_port) - self.assertEqual(WaveshareBusServo._port_singletons[key]['refcount'], 2) - - class TestRustypotBusServo(unittest.TestCase): def setUp(self): - # Mock numpy with real ndarray type so isinstance checks work - import types - mock_np = MagicMock() - mock_np.deg2rad = lambda x: x - mock_np.rad2deg = lambda x: x - # Make ndarray a real type so isinstance(rad, np.ndarray) doesn't raise - mock_np.ndarray = type('ndarray', (), {}) - sys.modules['numpy'] = mock_np - + # Mock numpy before importing backend, since it may not be installed in test env. + sys.modules['numpy'] = MagicMock( + deg2rad=lambda x: x, + rad2deg=lambda x: x, + ) + # Mock the dynamic import of rustypot self.mock_controller = MagicMock() mock_rustypot = MagicMock() mock_rustypot.Sts3215PyController.return_value = self.mock_controller @@ -145,7 +120,6 @@ def setUp(self): from modules.actuators.bus_servo.libraries.rustypot_backend import RustypotBusServo self.servo = RustypotBusServo(1, 'ST3215', '/dev/ttyUSB0', range=[0, 180]) self.servo.controller = self.mock_controller - def tearDown(self): sys.modules.pop('rustypot', None) sys.modules.pop('numpy', None) @@ -212,109 +186,35 @@ def test_calibrate_to_center(self): self.servo.range = [0, 180] self.servo.calibrate_to_center() self.servo.move_to.assert_called() - - -class TestServoManager(unittest.TestCase): - def _make_manager(self, **extra_cfg): - from modules.actuators.bus_servo.bus_servo import ServoManager, ServoState - - mock_backend = MagicMock() - mock_backend.get_moving.return_value = 0 - mock_backend.get_position.return_value = 90.0 - - servo_cfg = { - 'name': 'test', - 'id': 1, - 'range': [0, 180], - 'model': 'ST3215', - **extra_cfg, - } - - with patch.object(BusServoFactory, 'create', return_value=mock_backend): - mgr = ServoManager(backend='simulation', servos=[servo_cfg]) - - return mgr, mgr._servos['test'], mock_backend - - def test_init_creates_servo_state(self): - from modules.actuators.bus_servo.bus_servo import ServoState - mgr, sv, _ = self._make_manager() - self.assertIsInstance(sv, ServoState) - self.assertEqual(sv.identifier, 'test') - self.assertEqual(sv.range, [0, 180]) - +class TestBusServo(unittest.TestCase): def test_init_simulation_backend(self): + servo = Servo(name='test', id=1, range=[0, 180], model='ST3215', backend='simulation') + self.assertEqual(servo.identifier, 'test') + self.assertEqual(servo.index, 1) + self.assertEqual(servo.range, [0, 180]) + self.assertEqual(servo.model, 'ST3215') + # Should use simulation backend + self.assertIsInstance(servo.backend_servo, SimulationBusServo) + + def test_move_to_degrees(self): + servo = Servo(name='test', id=1, range=[0, 180], model='ST3215', backend='simulation') + # Should log the move and update position + servo.move(90, unit='degrees') + self.assertEqual(servo.backend_servo.position, 90) + + def test_factory_simulation(self): sim = BusServoFactory.create( - backend='simulation', model='ST3215', servo_id=1, port='sim', range=[0, 180] + backend='simulation', + model='ST3215', + servo_id=1, + port='sim', + range=[0, 180] ) self.assertIsInstance(sim, SimulationBusServo) + sim.move_to(45, unit='degrees') + self.assertEqual(sim.position, 45) - def test_do_move_calls_backend(self): - mgr, sv, backend = self._make_manager() - sv.pos = 90 - mgr._do_move(sv, 90) - backend.move_to.assert_called_once_with(90, unit='degrees') - self.assertEqual(sv.pos, 90) - - def test_do_move_out_of_range_logs_error(self): - mgr, sv, backend = self._make_manager() - mgr.log = MagicMock() - mgr._do_move(sv, 999) - backend.move_to.assert_not_called() - mgr.log.assert_called_once() - - def test_get_servo_position(self): - mgr, sv, backend = self._make_manager() - backend.get_position.return_value = 45.0 - pos = mgr.get_servo_position('test') - self.assertEqual(pos, 45.0) - backend.get_position.assert_called_once_with(unit='degrees') - - def test_detach_servo(self): - mgr, sv, backend = self._make_manager() - mgr.detach_servo('test') - backend.detach.assert_called_once() - - def test_attach_servo(self): - mgr, sv, backend = self._make_manager() - mgr.attach_servo('test') - backend.attach.assert_called_once() - - def test_exit_calls_detach_all_and_backend_exit(self): - mgr, sv, backend = self._make_manager() - mgr.exit() - backend.detach.assert_called() - backend.exit.assert_called() - - def test_group_move(self): - mgr, sv, _ = self._make_manager() - sv.move = MagicMock() - mgr.group_move({'test': 120}) - sv.move.assert_called_once_with(120) - - def test_move_to_pose(self): - mgr, sv, _ = self._make_manager() - mgr._poses = {'stand': {'test': 100}} - sv.move = MagicMock() - mgr.move_to_pose('stand') - sv.move.assert_called_once_with(100) - - def test_dict_like_getitem(self): - mgr, sv, _ = self._make_manager() - self.assertIs(mgr['test'], sv) - - def test_dict_like_contains(self): - mgr, _, _ = self._make_manager() - self.assertIn('test', mgr) - - def test_dict_like_iter(self): - mgr, _, _ = self._make_manager() - self.assertEqual(list(mgr), ['test']) - - def test_dict_like_items(self): - mgr, sv, _ = self._make_manager() - self.assertEqual(dict(mgr.items()), {'test': sv}) - - def test_simulation_backend_methods(self): + def test_simulation_methods(self): sim = SimulationBusServo(1, 'ST3215', 'sim', range=[0, 180]) sim.set_speed(10) self.assertEqual(sim.speed, 10) @@ -328,6 +228,5 @@ def test_simulation_backend_methods(self): sim.calibrate_to_center() self.assertEqual(sim.position, 90) - if __name__ == '__main__': unittest.main() diff --git a/src/modules/actuators/tests/bus_servo_test.py b/src/modules/actuators/tests/bus_servo_test.py index eeb9ce28..80f55376 100644 --- a/src/modules/actuators/tests/bus_servo_test.py +++ b/src/modules/actuators/tests/bus_servo_test.py @@ -5,255 +5,197 @@ from unittest.mock import MagicMock, patch, call -def _make_manager(model='ST', speed=300, acceleration=50, range=(0, 4095)): +def _make_servo(model='ST', speed=300, acceleration=50, range=(0, 4095)): """ - Build a ServoManager with a single servo and all hardware dependencies - mocked out. Returns (manager, servo_state, mock_backend). + Build a Servo instance with all hardware dependencies mocked out. + The messaging service is wired up so subscribe/publish work via simple + dict-based stubs (avoiding the real pub/sub library). """ from modules.actuators.bus_servo import bus_servo as servo_mod + # Create a mock backend servo (replaces WaveshareBusServo, etc.) mock_backend = MagicMock() mock_backend.get_moving.return_value = 0 mock_backend.get_position.return_value = 100 - servo_cfg = { - 'name': 'test_servo', - 'model': model, - 'id': 1, - 'range': list(range), - 'speed': speed, - 'acceleration': acceleration, - 'baudrate': 1000000, - 'port': '/dev/ttyAMA0', - 'start': None, - } - + # Patch BusServoFactory.create so no hardware SDK is imported with patch.object(servo_mod.BusServoFactory, 'create', return_value=mock_backend): - manager = servo_mod.ServoManager( - backend='simulation', - servos=[servo_cfg], - ) - - servo_state = manager._servos['test_servo'] - servo_state.pos = 100 # Set a known current position + kwargs = { + 'name': 'test_servo', + 'model': model, + 'id': 1, + 'range': list(range), + 'speed': speed, + 'acceleration': acceleration, + 'baudrate': 1000000, + 'port': '/dev/ttyAMA0', + 'calibrate_on_boot': False, + 'demonstrate_on_boot': False, + 'center_on_boot': False, + 'start': None, + 'poses': [], + } + sv = servo_mod.Servo(**kwargs) + + sv.pos = 100 # Set a known current position # Wire up a mock messaging service messaging_service = MagicMock() - manager._messaging_service = messaging_service + sv._messaging_service = messaging_service - return manager, servo_state, mock_backend + return sv, mock_backend -class TestServoManagerQueue(unittest.TestCase): +class TestBusServoQueue(unittest.TestCase): def setUp(self): - self.manager, self.servo_state, self.mock_backend = _make_manager() + self.sv, self.ph = _make_servo() # ------------------------------------------------------------------ # move (queues the request) # ------------------------------------------------------------------ def test_queue_move_adds_item(self): - self.servo_state.move(500) - self.assertEqual(len(self.servo_state._move_queue), 1) - item = self.servo_state._move_queue[0] + self.sv.move(500) + self.assertEqual(len(self.sv._move_queue), 1) + item = self.sv._move_queue[0] self.assertEqual(item['position'], 500) - self.assertEqual(item['speed'], self.servo_state.speed) - self.assertEqual(item['acceleration'], self.servo_state.acceleration) + self.assertEqual(item['speed'], self.sv.speed) + self.assertEqual(item['acceleration'], self.sv.acceleration) self.assertEqual(item['delay'], 0) self.assertAlmostEqual(item['timestamp'], time.time(), delta=1.0) def test_queue_move_speed_acceleration_override(self): - self.servo_state.move(200, speed=100, acceleration=10) - item = self.servo_state._move_queue[0] + self.sv.move(200, speed=100, acceleration=10) + item = self.sv._move_queue[0] self.assertEqual(item['speed'], 100) self.assertEqual(item['acceleration'], 10) def test_queue_move_delay_stored(self): - self.servo_state.move(300, delay=2.5) - item = self.servo_state._move_queue[0] + self.sv.move(300, delay=2.5) + item = self.sv._move_queue[0] self.assertEqual(item['delay'], 2.5) def test_queue_move_multiple_items(self): - self.servo_state.move(100) - self.servo_state.move(200) - self.servo_state.move(300) - self.assertEqual(len(self.servo_state._move_queue), 3) - positions = [item['position'] for item in self.servo_state._move_queue] + self.sv.move(100) + self.sv.move(200) + self.sv.move(300) + self.assertEqual(len(self.sv._move_queue), 3) + positions = [item['position'] for item in self.sv._move_queue] self.assertEqual(positions, [100, 200, 300]) # ------------------------------------------------------------------ - # _process_servo_queue + # _process_queue # ------------------------------------------------------------------ def test_process_queue_empty_does_nothing(self): - self.manager._do_move = MagicMock() - self.manager._process_servo_queue(self.servo_state) - self.manager._do_move.assert_not_called() + self.sv._do_move = MagicMock() + self.sv._process_queue() + self.sv._do_move.assert_not_called() def test_process_queue_calls_move_when_idle(self): - self.manager.is_servo_moving = MagicMock(return_value=False) - self.manager._do_move = MagicMock() - self.servo_state.move(500) - self.manager._process_servo_queue(self.servo_state) - self.manager._do_move.assert_called_once_with( - self.servo_state, 500, - self.servo_state.speed, self.servo_state.acceleration, - ) - self.assertEqual(len(self.servo_state._move_queue), 0) + self.sv.is_moving = MagicMock(return_value=False) + self.sv._do_move = MagicMock() + self.sv.move(500) + self.sv._process_queue() + self.sv._do_move.assert_called_once_with(500, self.sv.speed, self.sv.acceleration) + self.assertEqual(len(self.sv._move_queue), 0) def test_process_queue_does_not_move_while_moving(self): - self.manager.is_servo_moving = MagicMock(return_value=True) - self.manager._do_move = MagicMock() - self.servo_state.move(500) - self.manager._process_servo_queue(self.servo_state) - self.manager._do_move.assert_not_called() - self.assertEqual(len(self.servo_state._move_queue), 1) + self.sv.is_moving = MagicMock(return_value=True) + self.sv._do_move = MagicMock() + self.sv.move(500) + self.sv._process_queue() + self.sv._do_move.assert_not_called() + self.assertEqual(len(self.sv._move_queue), 1) def test_process_queue_respects_delay(self): - self.manager.is_servo_moving = MagicMock(return_value=False) - self.manager._do_move = MagicMock() - self.servo_state.move(500, delay=100) - self.manager._process_servo_queue(self.servo_state) - self.manager._do_move.assert_not_called() - self.assertEqual(len(self.servo_state._move_queue), 1) + self.sv.is_moving = MagicMock(return_value=False) + self.sv._do_move = MagicMock() + # Add item with a future delay + self.sv.move(500, delay=100) + self.sv._process_queue() + # Should NOT move yet because delay has not elapsed + self.sv._do_move.assert_not_called() + self.assertEqual(len(self.sv._move_queue), 1) def test_process_queue_executes_after_delay_elapsed(self): - self.manager.is_servo_moving = MagicMock(return_value=False) - self.manager._do_move = MagicMock() - self.servo_state.move(500, delay=0) - self.servo_state._move_queue[0]['timestamp'] = time.time() - 5 - self.manager._process_servo_queue(self.servo_state) - self.manager._do_move.assert_called_once() + self.sv.is_moving = MagicMock(return_value=False) + self.sv._do_move = MagicMock() + self.sv.move(500, delay=0) + # Backdate the timestamp to simulate delay already elapsed + self.sv._move_queue[0]['timestamp'] = time.time() - 5 + self.sv._process_queue() + self.sv._do_move.assert_called_once() def test_process_queue_processes_one_item_per_call(self): - self.manager.is_servo_moving = MagicMock(return_value=False) - self.manager._do_move = MagicMock() - self.servo_state.move(100) - self.servo_state.move(200) - self.manager._process_servo_queue(self.servo_state) - self.assertEqual(len(self.servo_state._move_queue), 1) - self.manager._do_move.assert_called_once_with( - self.servo_state, 100, - self.servo_state.speed, self.servo_state.acceleration, - ) + self.sv.is_moving = MagicMock(return_value=False) + self.sv._do_move = MagicMock() + self.sv.move(100) + self.sv.move(200) + self.sv._process_queue() + # Only first item should be processed + self.assertEqual(len(self.sv._move_queue), 1) + self.sv._do_move.assert_called_once_with(100, self.sv.speed, self.sv.acceleration) def test_process_queue_uses_per_item_speed_acceleration(self): - self.manager.is_servo_moving = MagicMock(return_value=False) - self.manager._do_move = MagicMock() - self.servo_state.move(400, speed=50, acceleration=5) - self.manager._process_servo_queue(self.servo_state) - self.manager._do_move.assert_called_once_with(self.servo_state, 400, 50, 5) + self.sv.is_moving = MagicMock(return_value=False) + self.sv._do_move = MagicMock() + self.sv.move(400, speed=50, acceleration=5) + self.sv._process_queue() + self.sv._do_move.assert_called_once_with(400, 50, 5) # ------------------------------------------------------------------ # _do_move – no blocking sleep, delegates to backend # ------------------------------------------------------------------ def test_move_does_not_block(self): - """_do_move() must not call time.sleep.""" + """_do_move() must not call time.sleep (blocking removed).""" with patch('time.sleep') as mock_sleep: - self.manager._do_move(self.servo_state, 500) + self.sv._do_move(500) mock_sleep.assert_not_called() def test_move_accepts_speed_acceleration_params(self): - """_do_move() should delegate the move to the backend.""" - self.manager._do_move(self.servo_state, 500, speed=10, acceleration=2) - self.mock_backend.move_to.assert_called_once_with(500, unit='degrees') - - def test_move_uses_backend_when_not_supplied(self): - self.manager._do_move(self.servo_state, 500) - self.mock_backend.move_to.assert_called_once_with(500, unit='degrees') + """_do_move() should delegate the move to the backend servo.""" + self.sv._do_move(500, speed=10, acceleration=2) + self.ph.move_to.assert_called_once_with(500, unit='degrees') - # ------------------------------------------------------------------ - # loop() - # ------------------------------------------------------------------ - def test_loop_processes_all_servos(self): - """loop() should call _process_servo_queue for every managed servo.""" - with patch.object(self.manager, '_process_servo_queue') as mock_pq: - self.manager.loop() - mock_pq.assert_called_once_with(self.servo_state) + def test_move_uses_instance_defaults_when_not_supplied(self): + self.sv._do_move(500) + self.ph.move_to.assert_called_once_with(500, unit='degrees') # ------------------------------------------------------------------ # setup_messaging subscriptions # ------------------------------------------------------------------ + def test_loop_calls_process_queue(self): + """loop() should call _process_queue to drain the move queue.""" + with patch.object(self.sv, '_process_queue') as mock_pq: + self.sv.loop() + mock_pq.assert_called_once() + + def test_setup_messaging_does_not_subscribe_to_system_loop(self): + """system/loop subscription replaced by direct loop() call — should not be in subscriptions.""" + messaging_service = MagicMock() + with patch.object(self.sv, 'get_position', return_value=100): + self.sv.messaging_service = messaging_service + + topics = [c.args[0] for c in messaging_service.subscribe.call_args_list] + self.assertNotIn('system/loop', topics) + def test_setup_messaging_subscribes_queue_topic(self): messaging_service = MagicMock() - with patch.object(self.manager, 'get_servo_position', return_value=100): - self.manager.messaging_service = messaging_service + with patch.object(self.sv, 'get_position', return_value=100): + self.sv.messaging_service = messaging_service topics = [c.args[0] for c in messaging_service.subscribe.call_args_list] self.assertIn('servo:test_servo:queue', topics) def test_setup_messaging_mvabs_uses_queue_move(self): messaging_service = MagicMock() - with patch.object(self.manager, 'get_servo_position', return_value=100): - self.manager.messaging_service = messaging_service + with patch.object(self.sv, 'get_position', return_value=100): + self.sv.messaging_service = messaging_service + # Find the callback registered for ':mvabs' subscriptions = {c.args[0]: c.args[1] for c in messaging_service.subscribe.call_args_list} self.assertIn('servo:test_servo:mvabs', subscriptions) - self.assertEqual(subscriptions['servo:test_servo:mvabs'], self.servo_state.move) - - # ------------------------------------------------------------------ - # Dict-like interface - # ------------------------------------------------------------------ - def test_getitem_returns_servo_state(self): - from modules.actuators.bus_servo.bus_servo import ServoState - self.assertIsInstance(self.manager['test_servo'], ServoState) - - def test_iter_yields_servo_names(self): - self.assertIn('test_servo', list(self.manager)) - - def test_contains_servo_name(self): - self.assertIn('test_servo', self.manager) - - def test_items_returns_name_state_pairs(self): - items = dict(self.manager.items()) - self.assertIn('test_servo', items) - - # ------------------------------------------------------------------ - # Group move / pose - # ------------------------------------------------------------------ - def test_group_move_queues_for_all_servos(self): - self.servo_state.move = MagicMock() - self.manager.group_move({'test_servo': 200}) - self.servo_state.move.assert_called_once_with(200) - - def test_move_to_pose_unknown_logs_warning(self): - self.manager.log = MagicMock() - self.manager.move_to_pose('nonexistent_pose') - self.manager.log.assert_called_once() - - def test_move_to_pose_known_queues_moves(self): - self.manager._poses = {'test_pose': {'test_servo': 150}} - self.servo_state.move = MagicMock() - self.manager.move_to_pose('test_pose') - self.servo_state.move.assert_called_once_with(150) - - # ------------------------------------------------------------------ - # ServoState delegates to manager - # ------------------------------------------------------------------ - def test_servo_state_detach_delegates(self): - self.manager.detach_servo = MagicMock() - self.servo_state.detach() - self.manager.detach_servo.assert_called_once_with('test_servo') - - def test_servo_state_attach_delegates(self): - self.manager.attach_servo = MagicMock() - self.servo_state.attach() - self.manager.attach_servo.assert_called_once_with('test_servo') - - def test_servo_state_get_position_delegates(self): - self.manager.get_servo_position = MagicMock(return_value=42.0) - pos = self.servo_state.get_position() - self.assertEqual(pos, 42.0) - - def test_servo_state_is_moving_delegates(self): - self.manager.is_servo_moving = MagicMock(return_value=True) - self.assertTrue(self.servo_state.is_moving()) - - def test_servo_state_move_relative_clamps_to_range(self): - self.servo_state.pos = 100 - self.servo_state.range = [0, 110] - self.servo_state.move = MagicMock() - self.servo_state.move_relative(50) # would reach 150, clamps to 110 - self.servo_state.move.assert_called_once_with(110) + self.assertEqual(subscriptions['servo:test_servo:mvabs'], self.sv.move) if __name__ == '__main__': diff --git a/src/modules/personality/personality.py b/src/modules/personality/personality.py index 916414fd..d7788426 100644 --- a/src/modules/personality/personality.py +++ b/src/modules/personality/personality.py @@ -171,8 +171,8 @@ def balance(self): if abs(pitch) < 2: return # No need to adjust for small angles # print(f"Angle to move: {pitch}") - self.servos['leg_l_hip'].move_relative(-pitch) - self.servos['leg_r_hip'].move_relative(pitch) + self.servos['leg_l_hip'].move_relative(pitch) + self.servos['leg_r_hip'].move_relative(-pitch) def handle_user_message(self, user_id=None, message=None): print(f"Received message from user {user_id}: {message}") From a58586f64e99f7c948ce26734819cb327b622d67 Mon Sep 17 00:00:00 2001 From: danic85 <6583012+danic85@users.noreply.github.com> Date: Mon, 20 Apr 2026 17:14:56 +0100 Subject: [PATCH 4/4] WIP --- environments/cody.yml | 10 ++++++---- src/modules/actuators/bus_servo/bus_servo.py | 14 ++++++-------- .../bus_servo/libraries/waveshare_backend.py | 1 + 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/environments/cody.yml b/environments/cody.yml index 59e45caa..bb48e519 100644 --- a/environments/cody.yml +++ b/environments/cody.yml @@ -41,9 +41,10 @@ bus_servo: - name: "leg_r_hip" model: 'ST3215' id: 3 - range: [0.0, 101.3] # Need to re-center + range: [0, 360] # Need to re-center + center_on_boot: true # calibrate_on_boot: true - start: 65 + # start: 65 - name: "leg_r_knee" model: 'ST3215' id: 2 @@ -99,9 +100,10 @@ bus_servo: - name: "leg_l_hip" model: 'ST3215' id: 8 - range: [144.35, 336] # Need to re-center + range: [0, 360] # Need to re-center + center_on_boot: true # calibrate_on_boot: true - start: 214.33 + # start: 214.33 - name: "leg_l_knee" model: 'ST3215' id: 9 diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py index 9e098c25..0a3f2eee 100644 --- a/src/modules/actuators/bus_servo/bus_servo.py +++ b/src/modules/actuators/bus_servo/bus_servo.py @@ -24,6 +24,7 @@ def __init__(self, **kwargs): self.calibrate_on_boot = kwargs.get('calibrate_on_boot', False) # Loop to show position for manual configuration self.demonstrate_on_boot = kwargs.get('demonstrate_on_boot', False) # Move to min and max to demonstrate range self.center_on_boot = kwargs.get('center_on_boot', False) # Move to center of range on boot + self.zero_on_boot = kwargs.get('zero_on_boot', False) # Set current position as zero on boot self.pos = None self.speed = kwargs.get('speed', 300) # 3073 self.acceleration = kwargs.get('acceleration', 50) @@ -57,14 +58,14 @@ def setup_messaging(self): self.subscribe('system/exit', self.exit) self.subscribe('servo/pose', self.move_to_pose) + if self.center_on_boot: + self.calibrate_to_center() + if self.calibrate_on_boot: self.calibrate_dynamic() # Log will show current position repeatedly to help with manual configuration self.pos = self.get_position() # Get initial position to avoid jumping from unknown position - if self.center_on_boot: - self.calibrate_to_center() - if self.demonstrate_on_boot: self.log(f"Demonstrating servo {self.identifier} movement, speed={self.speed}, acceleration={self.acceleration}") if self.range is not None: @@ -177,6 +178,7 @@ def is_moving(self): return False if abs(self.pos - pos) > 2: self.log(f"Warning: Servo {self.identifier} is not reporting as moving but position {pos} does not match target position {self.pos}", level='warning') + self.move(self.pos) # Attempt to correct by moving to current position return False def get_position(self): @@ -257,8 +259,4 @@ def calibrate_dynamic(self): if self.start is not None and (self.start < min_pos or self.start > max_pos): self.start = (min_pos + max_pos) // 2 - self.log(f"Start position {self.start} out of new range, setting to midpoint {self.start}") - - - - + self.log(f"Start position {self.start} out of new range, setting to midpoint {self.start}") \ No newline at end of file diff --git a/src/modules/actuators/bus_servo/libraries/waveshare_backend.py b/src/modules/actuators/bus_servo/libraries/waveshare_backend.py index 5fc3c431..a72097fc 100644 --- a/src/modules/actuators/bus_servo/libraries/waveshare_backend.py +++ b/src/modules/actuators/bus_servo/libraries/waveshare_backend.py @@ -181,5 +181,6 @@ def calibrate_to_center(self): return center_deg = (self.range[0] + self.range[1]) / 2 center_raw = degrees_to_raw(center_deg, self.min_deg, self.max_deg, self.min_raw, self.max_raw) + self.log(f"Calibrating servo {self.servo_id} to center position {center_deg} degrees (raw: {center_raw})") self.move_to_raw(center_raw) self.log(f"Moved servo {self.servo_id} to center position {center_deg} degrees (raw: {center_raw})")