diff --git a/environments/cody.yml b/environments/cody.yml index 7c5f220b..bb48e519 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,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: diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py index b15d9a0c..0a3f2eee 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') @@ -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}") \ No newline at end of file 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/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})") diff --git a/src/modules/personality/personality.py b/src/modules/personality/personality.py index eb19afe6..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}") @@ -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