Introduce ServoManager for centralised bus servo control

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,29 +30,33 @@ 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'
       id: 1
-      range: [220.9, 346.6] # converted from raw (4095 = 360 degrees)
-      start: 246.9
+      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, 181.3]
+      range: [0, 360] # Need to re-center
+      center_on_boot: true
       # calibrate_on_boot: true
-      # start: 148.4
+      # start: 65
     - name: "leg_r_knee"
       model: 'ST3215'
       id: 2
-      range: [62.9, 295.7]
-      # start: 118.2
+      range: [52.13, 296.7]
+      # calibrate_on_boot: true
+      # start: 55.3
     - name: "leg_r_ankle"
       model: 'ST3215'
       id: 6
-      range: [216.1, 340.5]
-      # start: 243.0
+      range: [111.74, 323.16]
+      # calibrate_on_boot: true
+      # start: 256.35
     - name: "neck_pan"
       model: 'ST3215'
       id: 12
@@ -89,24 +94,28 @@ bus_servo:
     - name: "leg_l_tilt"
       model: 'ST3215'
       id: 7
-      range: [0.7, 157.8] # converted from raw (4095 = 360 degrees)
-      start: 142.1
+      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: [74.9, 279.2]
-      # start: 215.4
+      range: [0, 360] # Need to re-center
+      center_on_boot: true
+      # calibrate_on_boot: true
+      # start: 214.33
     - name: "leg_l_knee"
       model: 'ST3215'
       id: 9
-      range: [117.1, 299.3]
-      # start: 232.2
+      range: [75.78, 299.3]
+      # calibrate_on_boot: true
+      # start: 299
     - name: "leg_l_ankle"
       model: 'ST3215'
       id: 10
-      range: [53.9, 199.3]
-      # start: 137.6
+      range: [47.47, 262.15]
+      # calibrate_on_boot: true
+      # start: 113.58
       # demonstrate_on_boot: true
 
 chatgpt:

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')
@@ -23,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)
@@ -56,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:
@@ -162,10 +164,21 @@ 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')
+            self.move(self.pos)  # Attempt to correct by moving to current position
         return False
 
     def get_position(self):
@@ -218,7 +231,7 @@ def calibrate_dynamic(self):
         max_pos = None
         try:
             while True:
-                pos = self.get_position()
+                pos = round(self.get_position(), 2)
                 if pos is None:
                     self.log(f"Failed to get position for servo {self.identifier}", level='warning')
                     continue
@@ -246,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}")

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

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

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}")
@@ -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