Feature/leg animations clean

.github/workflows/test.yml

@@ -21,11 +21,12 @@ jobs:
         run: |
           python -m pip install --upgrade pip
           if [ -f requirements.txt ]; then pip install -r requirements.txt; fi
+          if [ -f install.sh ]; then chmod +x install.sh && ./install.sh cody; fi
 
       - name: Run unittests
         run: |
-          PYTHONPATH=src python -m unittest discover -s src/tests -p 'test_*.py' -t src
-          PYTHONPATH=src python -m unittest discover -s src/modules -p 'test_*.py' -t src
+          PYTHONPATH=src myenv/bin/python3 -m unittest discover -s src/tests -p 'test_*.py' -t src
+          PYTHONPATH=src myenv/bin/python3 -m unittest discover -s src/modules -p 'test_*.py' -t src
 
     permissions:
       pull-requests: write

environments/cody.yml

@@ -20,47 +20,53 @@ bus_servo:
   enabled: true
   config:
     poses:
-      - legs_forward: {'leg_r_tilt': 2806, 'leg_r_hip': 1989, 'leg_r_knee': 1349, 'leg_r_ankle': 2754, 'neck_pan': 1698, 'neck_tilt': 118, 'leg_l_tilt': 1619, 'leg_l_hip': 1028, 'leg_l_knee': 2634, 'leg_l_ankle': 1567}
-      - sit: {'leg_r_tilt': 2806, 'leg_r_hip': 2063, 'leg_r_knee': 1840, 'leg_r_ankle': 3641, 'neck_pan': 1697, 'neck_tilt': 118, 'leg_l_tilt': 1621, 'leg_l_hip': 853, 'leg_l_knee': 2161, 'leg_l_ankle': 614}
-      - wave_1: {'leg_r_tilt': 2809, 'leg_r_hip': 2063, 'leg_r_knee': 1840, 'leg_r_ankle': 3641, 'neck_pan': 1697, 'neck_tilt': 118, 'leg_l_tilt': 1618, 'leg_l_hip': 855, 'leg_l_knee': 2614, 'leg_l_ankle': 1337}
-      - wave_2: {'leg_r_tilt': 2807, 'leg_r_hip': 2063, 'leg_r_knee': 1840, 'leg_r_ankle': 3641, 'neck_pan': 1697, 'neck_tilt': 118, 'leg_l_tilt': 1618, 'leg_l_hip': 855, 'leg_l_knee': 2614, 'leg_l_ankle': 874}      
-      - stand_low: {'leg_r_tilt': 2790, 'leg_r_hip': 422, 'leg_r_knee': 717, 'leg_r_ankle': 3174, 'neck_pan': 1697, 'neck_tilt': 119, 'leg_l_tilt': 1578, 'leg_l_hip': 2668, 'leg_l_knee': 3342, 'leg_l_ankle': 1107}      
-      - stand_high: {'leg_r_tilt': 2790, 'leg_r_hip': 692, 'leg_r_knee': 1322, 'leg_r_ankle': 2861, 'neck_pan': 1700, 'neck_tilt': 123, 'leg_l_tilt': 1578, 'leg_l_hip': 2510, 'leg_l_knee': 2712, 'leg_l_ankle': 1560}     
-      - sit_edge: {'leg_r_tilt': 2790, 'leg_r_hip': 1918, 'leg_r_knee': 2768, 'leg_r_ankle': 2854, 'neck_pan': 1704, 'neck_tilt': 124, 'leg_l_tilt': 1577, 'leg_l_hip': 1183, 'leg_l_knee': 1332, 'leg_l_ankle': 1562}    
-      - sit_edge_swing_l: {'leg_r_tilt': 2790, 'leg_r_hip': 1918, 'leg_r_knee': 2768, 'leg_r_ankle': 2854, 'neck_pan': 1704, 'neck_tilt': 124, 'leg_l_tilt': 1577, 'leg_l_hip': 1185, 'leg_l_knee': 1789, 'leg_l_ankle': 1988}    
-      - sit_edge_swing_r: {'leg_r_tilt': 2790, 'leg_r_hip': 1918, 'leg_r_knee': 2222, 'leg_r_ankle': 2496, 'neck_pan': 1704, 'neck_tilt': 124, 'leg_l_tilt': 1577, 'leg_l_hip': 1182, 'leg_l_knee': 1345, 'leg_l_ankle': 1511}    
+      - storage: {'leg_r_tilt': 2784, 'leg_r_hip': 1006, 'leg_r_knee': 772, 'leg_r_ankle': 2991, 'neck_pan': 1708, 'neck_tilt': 121, 'leg_l_tilt': 1601, 'leg_l_hip': 3108, 'leg_l_knee': 3258, 'leg_l_ankle': 1286}
+      - legs_forward: {'leg_r_tilt': 2784, 'leg_r_hip': 1005, 'leg_r_knee': 1998, 'leg_r_ankle': 2351, 'neck_pan': 1710, 'neck_tilt': 121, 'leg_l_tilt': 1606, 'leg_l_hip': 3099, 'leg_l_knee': 2107, 'leg_l_ankle': 1865}
+      - sit: {'leg_r_tilt': 2784, 'leg_r_hip': 866, 'leg_r_knee': 2042, 'leg_r_ankle': 3396, 'neck_pan': 1711, 'neck_tilt': 121, 'leg_l_tilt': 1597, 'leg_l_hip': 3229, 'leg_l_knee': 2095, 'leg_l_ankle': 698}
+      - wave_1: {'leg_r_tilt': 2785, 'leg_r_hip': 857, 'leg_r_knee': 2037, 'leg_r_ankle': 3396, 'neck_pan': 1710, 'neck_tilt': 121, 'leg_l_tilt': 1599, 'leg_l_hip': 3131, 'leg_l_knee': 2626, 'leg_l_ankle': 1371}
+      - wave_2: {'leg_r_tilt': 2785, 'leg_r_hip': 857, 'leg_r_knee': 2037, 'leg_r_ankle': 3396, 'neck_pan': 1710, 'neck_tilt': 121, 'leg_l_tilt': 1599, 'leg_l_hip': 3131, 'leg_l_knee': 2626, 'leg_l_ankle': 701}
+      # - stand_straight: {'leg_r_tilt': 2735, 'leg_r_hip': 1968, 'leg_r_knee': 2029, 'leg_r_ankle': 2353, 'neck_pan': 1711, 'neck_tilt': 121, 'leg_l_tilt': 1596, 'leg_l_hip': 2087, 'leg_l_knee': 2075, 'leg_l_ankle': 1799}
+      - stand_low: {'leg_r_tilt': 2698, 'leg_r_hip': 3051, 'leg_r_knee': 596, 'leg_r_ankle': 2751, 'neck_pan': 1708, 'neck_tilt': 120, 'leg_l_tilt': 1626, 'leg_l_hip': 1149, 'leg_l_knee': 3481, 'leg_l_ankle': 1306}
+      - stand_high: {'leg_r_tilt': 2687, 'leg_r_hip': 2547, 'leg_r_knee': 1080, 'leg_r_ankle': 2753, 'neck_pan': 1711, 'neck_tilt': 116, 'leg_l_tilt': 1595, 'leg_l_hip': 1536, 'leg_l_knee': 2976, 'leg_l_ankle': 1522}
+      - stand_dip_l: {'leg_r_tilt': 2693, 'leg_r_hip': 2539, 'leg_r_knee': 1081, 'leg_r_ankle': 2742, 'neck_pan': 1713, 'neck_tilt': 119, 'leg_l_tilt': 1623, 'leg_l_hip': 1386, 'leg_l_knee': 3139, 'leg_l_ankle': 1522} 
+      - stand_dip_r: {'leg_r_tilt': 2685, 'leg_r_hip': 2703, 'leg_r_knee': 971, 'leg_r_ankle': 2743, 'neck_pan': 1709, 'neck_tilt': 119, 'leg_l_tilt': 1657, 'leg_l_hip': 1463, 'leg_l_knee': 2992, 'leg_l_ankle': 1511}
+      - sit_edge: {'leg_r_tilt': 2784, 'leg_r_hip': 985, 'leg_r_knee': 2897, 'leg_r_ankle': 2816, 'neck_pan': 1711, 'neck_tilt': 121, 'leg_l_tilt': 1606, 'leg_l_hip': 3072, 'leg_l_knee': 1431, 'leg_l_ankle': 1161}
+      - sit_edge_swing_l: {'leg_r_tilt': 2784, 'leg_r_hip': 985, 'leg_r_knee': 2897, 'leg_r_ankle': 2816, 'neck_pan': 1709, 'neck_tilt': 121, 'leg_l_tilt': 1606, 'leg_l_hip': 3071, 'leg_l_knee': 1928, 'leg_l_ankle': 1154}   
+      - sit_edge_swing_r: {'leg_r_tilt': 2783, 'leg_r_hip': 986, 'leg_r_knee': 2028, 'leg_r_ankle': 2633, 'neck_pan': 1709, 'neck_tilt': 121, 'leg_l_tilt': 1606, 'leg_l_hip': 3072, 'leg_l_knee': 1326, 'leg_l_ankle': 1161}
+      - legs_straight_out: {'leg_r_tilt': 3696, 'leg_r_hip': 2066, 'leg_r_knee': 1904, 'leg_r_ankle': 2777, 'neck_pan': 1713, 'neck_tilt': 116, 'leg_l_tilt': 599, 'leg_l_hip': 2103, 'leg_l_knee': 2285, 'leg_l_ankle': 1140}
+      - crab: {'leg_r_tilt': 3695, 'leg_r_hip': 1046, 'leg_r_knee': 1451, 'leg_r_ankle': 2745, 'neck_pan': 1712, 'neck_tilt': 116, 'leg_l_tilt': 599, 'leg_l_hip': 2930, 'leg_l_knee': 2855, 'leg_l_ankle': 1230}
+      # - sit_safe:
+      # - sit_safe_toe_up_l
+      # - sit_safe_toe_up_r
+      # - sit_safe_toe_down_l
+      # - sit_safe_toe_down_r
   instances:
     - name: "leg_r_tilt"
       model: 'ST3215'
       id: 1
-      range: [2511, 3944] # 1433 total range (4095 = 360 degrees)
-      range_degrees: 125.9
-      start: 2810
-      # calibrate_on_boot: true
+      range: [2352, 3749]
+      # start: 2791
     - name: "leg_r_hip"
       model: 'ST3215'
       id: 3
-      range: [0, 2063]
-      range_degrees: 181.3
+      range: [615, 3423]
+      # center_on_boot: true
       # calibrate_on_boot: true
-      # start: 1688
+      # start: 977
     - name: "leg_r_knee"
       model: 'ST3215'
       id: 2
-      range: [717, 3297]
-      range_degrees: 226.2
-      # start: 1345
+      range: [626, 3384]
+      # start: 1999
     - name: "leg_r_ankle"
       model: 'ST3215'
       id: 6
-      range: [2459, 3872]
-      range_degrees: 123.7
-      # start: 2762
+      range: [1123, 3638]
+      # start: 2351
     - name: "neck_pan"
       model: 'ST3215'
       id: 12
-      range: [0, 3412] # 300 degrees = 3412, 360 degrees = 4095
-      range_degrees: 299.7
+      range: [1200, 2200] # Reduced range to prevent damaging wires
       start: 1706
       # calibrate_on_boot: true
       # demonstrate_on_boot: true
@@ -69,7 +75,6 @@ bus_servo:
     #   baudrate: 115200
     #   id: 4
     #   range: [0, 1023]
-    #   range_degrees: 300
     #   start: 511
       # calibrate_on_boot: true
       # demonstrate_on_boot: true
@@ -78,10 +83,9 @@ bus_servo:
       model: 'SC09'
       baudrate: 115200
       id: 11
-      range: [30, 220]
-      range_degrees: 55.6
+      range: [30, 200]
       start: 120
-      speed: 60 # max=0, 1 - 3000 tested
+      speed: 200 # max=0, 1 - 3000 tested
       # acceleration: 0 # max (no significant difference between this and 1 - 3000)
       # demonstrate_on_boot: true
     # - name: "neck_tilt2"
@@ -97,27 +101,24 @@ bus_servo:
       model: 'ST3215'
       id: 7
       range: [8, 1796] # 1788 total range (4095 = 360 degrees)
-      range_degrees: 156.9
-      start: 1617
-      # calibrate_on_boot: true
+      # start: 1607
     - name: "leg_l_hip"
       model: 'ST3215'
       id: 8
-      range: [853, 3098]
-      range_degrees: 197.3
-      # start: 2451 
+      range: [615, 3423]
+      # center_on_boot: true
+      # calibrate_on_boot: true
+      # start: 977
     - name: "leg_l_knee"
       model: 'ST3215'
       id: 9
-      range: [1332, 3404]
-      range_degrees: 181.1
-      # start: 2643
+      range: [684, 3471]
+      # start: 2106
     - name: "leg_l_ankle"
       model: 'ST3215'
       id: 10
-      range: [614, 2270]
-      range_degrees: 145.2
-      # start: 1566
+      range: [520, 3059]
+      # start: 1856
       # demonstrate_on_boot: true
 
 chatgpt:
@@ -351,6 +352,36 @@ personality:
   config:
     mode: 6 # Config.MODE_LIVE
     sleep_timeout: 60  # Time in seconds before entering sleep mode
+    balance_enabled: true
+    track_people: true
+    track_people_servos: true
+    animate_pose_enabled: true
+    check_being_carried_enabled: false
+    knee_pose_thresholds:
+        sitting_edge: 1700 # 1431
+        sitting: 2000 # 2095
+        standing: 2900 # 2976
+    poses:
+      - storage: {'leg_r_tilt': 2784, 'leg_r_hip': 1006, 'leg_r_knee': 772, 'leg_r_ankle': 2991, 'neck_pan': 1708, 'neck_tilt': 121, 'leg_l_tilt': 1601, 'leg_l_hip': 3108, 'leg_l_knee': 3258, 'leg_l_ankle': 1286}
+      - legs_forward: {'leg_r_tilt': 2784, 'leg_r_hip': 1005, 'leg_r_knee': 1998, 'leg_r_ankle': 2351, 'leg_l_tilt': 1606, 'leg_l_hip': 3099, 'leg_l_knee': 2107, 'leg_l_ankle': 1865}
+      - sit: {'leg_r_tilt': 2784, 'leg_r_hip': 866, 'leg_r_knee': 2042, 'leg_r_ankle': 3396, 'leg_l_tilt': 1597, 'leg_l_hip': 3229, 'leg_l_knee': 2095, 'leg_l_ankle': 698}
+      - wave_1: {'leg_r_tilt': 2785, 'leg_r_hip': 857, 'leg_r_knee': 2037, 'leg_r_ankle': 3396, 'leg_l_tilt': 1599, 'leg_l_hip': 3131, 'leg_l_knee': 2626, 'leg_l_ankle': 1371}
+      - wave_2: {'leg_r_tilt': 2785, 'leg_r_hip': 857, 'leg_r_knee': 2037, 'leg_r_ankle': 3396, 'leg_l_tilt': 1599, 'leg_l_hip': 3131, 'leg_l_knee': 2626, 'leg_l_ankle': 701}
+      # - stand_straight: {'leg_r_tilt': 2735, 'leg_r_hip': 1968, 'leg_r_knee': 2029, 'leg_r_ankle': 2353, 'leg_l_tilt': 1596, 'leg_l_hip': 2087, 'leg_l_knee': 2075, 'leg_l_ankle': 1799}
+      - stand_low: {'leg_r_tilt': 2698, 'leg_r_hip': 3051, 'leg_r_knee': 596, 'leg_r_ankle': 2751, 'leg_l_tilt': 1626, 'leg_l_hip': 1149, 'leg_l_knee': 3481, 'leg_l_ankle': 1306}
+      - stand_high: {'leg_r_tilt': 2687, 'leg_r_hip': 2547, 'leg_r_knee': 1080, 'leg_r_ankle': 2753, 'leg_l_tilt': 1595, 'leg_l_hip': 1536, 'leg_l_knee': 2976, 'leg_l_ankle': 1522}
+      - stand_dip_l: {'leg_r_tilt': 2693, 'leg_r_hip': 2539, 'leg_r_knee': 1081, 'leg_r_ankle': 2742, 'leg_l_tilt': 1623, 'leg_l_hip': 1386, 'leg_l_knee': 3139, 'leg_l_ankle': 1522} 
+      - stand_dip_r: {'leg_r_tilt': 2685, 'leg_r_hip': 2703, 'leg_r_knee': 971, 'leg_r_ankle': 2743, 'leg_l_tilt': 1657, 'leg_l_hip': 1463, 'leg_l_knee': 2992, 'leg_l_ankle': 1511}
+      - sit_edge: {'leg_r_tilt': 2784, 'leg_r_hip': 985, 'leg_r_knee': 2897, 'leg_r_ankle': 2816,  'leg_l_tilt': 1606, 'leg_l_hip': 3072, 'leg_l_knee': 1431, 'leg_l_ankle': 1161}
+      - sit_edge_swing_l: {'leg_r_tilt': 2784, 'leg_r_hip': 985, 'leg_r_knee': 2897, 'leg_r_ankle': 2816, 'leg_l_tilt': 1606, 'leg_l_hip': 3071, 'leg_l_knee': 1928, 'leg_l_ankle': 1154}   
+      - sit_edge_swing_r: {'leg_r_tilt': 2783, 'leg_r_hip': 986, 'leg_r_knee': 2028, 'leg_r_ankle': 2633, 'leg_l_tilt': 1606, 'leg_l_hip': 3072, 'leg_l_knee': 1326, 'leg_l_ankle': 1161}
+      - legs_straight_out: {'leg_r_tilt': 3696, 'leg_r_hip': 2066, 'leg_r_knee': 1904, 'leg_r_ankle': 2777, 'leg_l_tilt': 599, 'leg_l_hip': 2103, 'leg_l_knee': 2285, 'leg_l_ankle': 1140}
+      - crab: {'leg_r_tilt': 3695, 'leg_r_hip': 1046, 'leg_r_knee': 1451, 'leg_r_ankle': 2745, 'leg_l_tilt': 599, 'leg_l_hip': 2930, 'leg_l_knee': 2855, 'leg_l_ankle': 1230}
+      # - sit_safe:
+      # - sit_safe_toe_up_l
+      # - sit_safe_toe_up_r
+      # - sit_safe_toe_down_l
+      # - sit_safe_toe_down_r
 
 pitemperature:
   enabled: true

src/main.py

@@ -35,6 +35,7 @@ def main():
     # Inject messaging service and all module-to-module dependencies declared in the
     # environment YAML file (inject: / on_inject: blocks).
     loader.inject_dependencies(module_instances)
+    loader.modules_loaded(module_instances)  # Call on_load() for all modules after injection
 
     # Use the new SystemLoop class to run the main loop
     system_loop = SystemLoop(module_instances['MessagingService'].messaging_service, module_instances)

src/module_loader.py

@@ -204,6 +204,13 @@ def inject_dependencies(self, instances):
                     else:
                         print(f"[ModuleLoader] on_inject: {target_key}.{method_name} not found or not callable")
 
+    def modules_loaded(self, instances):
+        """Call on_load() for all modules after dependencies have been injected."""
+        for name, module in instances.items():
+            if hasattr(module, 'on_load') and callable(module.on_load):
+                print(f"[ModuleLoader] Calling on_load for {name}")
+                module.on_load()
+
     def _resolve_inject_source(self, source_spec, instances):
         """
         Resolve an inject source specification to a value.

src/modules/actuators/bus_servo/bus_servo.py

@@ -48,7 +48,7 @@ def __init__(self, **kwargs):
         self.model = kwargs.get('model', 'ST')
         self.index = kwargs.get('id')
         self.range = kwargs.get('range')
-        self.range_degrees = kwargs.get('range_degrees', None)  # Optional range in degrees for easier control
+        self.range_degrees = kwargs.get('range_degrees', self.calculate_range_degrees(self.range[0], self.range[1]))
         self.start = kwargs.get('start') # Default start position
         self.poses = kwargs.get('poses')  # Dictionary of poses
         self.baudrate = kwargs.get('baudrate', 1000000)
@@ -57,8 +57,8 @@ def __init__(self, **kwargs):
         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.speed = kwargs.get('speed', 0) # 3073
+        self.acceleration = kwargs.get('acceleration', 0)
         self._move_queue = collections.deque()
         # After loading YAML:
         poses_list = kwargs.get('poses', [])
@@ -108,7 +108,7 @@ def setup_messaging(self):
         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)
+        # self.subscribe('servo/pose', self.move_to_pose) # Disabled as this was causing issues with servos. Personality should call move directly.
 
         if self.calibrate_on_boot:
             self.calibrate_dynamic() # Log will show current position repeatedly to help with manual configuration
@@ -135,6 +135,10 @@ def setup_messaging(self):
 
     def move_to_pose(self, pose_name):
         # print(self.poses)
+        # if pose in list of poses
+        if pose_name not in self.poses:
+            self.log(f"Pose '{pose_name}' not found for servo {self.identifier}", level='warning')
+            return
         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)
@@ -168,13 +172,15 @@ def _sc_write(self, type, value, verbose=False):
             # self.log(f"[SCServo][{self.identifier}] Error context: load={load} (comm_result={load_comm_result}, error={load_error}), position={pos} (comm_result={pos_comm_result}, error={pos_error}), speed={speed} (comm_result={speed_comm_result}, error={speed_error})")
         return comm_result == COMM_SUCCESS and error == 0
 
-    def move_degrees(self, degrees):
+    def move_degrees(self, degrees, speed=None):
         if self.range_degrees is None:
             self.log(f"Servo {self.identifier} does not have range_degrees set, cannot move by degrees", level='error')
             return
+        # self.log(f"[move_degrees] Moving servo {self.identifier} by {degrees} degrees")
         # Convert degrees to position value based on range, adjusting RELATIVE to current position
         self.pos = self.get_position()  # Update current position before calculating new position
         if self.range is not None and self.pos is not None:
+            # self.log(f"Current position: {self.pos}, Range: {self.range}, Range degrees: {self.range_degrees}")
             # Calculate how many position units correspond to the degree change
             units_per_degree = (self.range[1] - self.range[0]) / self.range_degrees
             position_delta = degrees * units_per_degree
@@ -187,7 +193,9 @@ def move_degrees(self, degrees):
             if self.range_degrees > 0:
                 pc_move = round((degrees / self.range_degrees) * 100)
                 self.log(f"Moving servo {self.identifier} by {degrees} degrees (position {self.pos} -> {new_position} | {pc_move}% of range)")
-                self.move(new_position)
+                self.move(new_position, speed)
+            else:
+                self.log(f"Invalid range_degrees for servo {self.identifier}, cannot move by degrees {self.range_degrees}", level='error')
 
     def move(self, position, speed=None, acceleration=None, delay=0, **kwargs):
         """
@@ -277,8 +285,13 @@ def move_relative(self, delta):
     def is_moving(self):
         if self.get_moving() == 1:
             return True
-        elif abs(self.pos - self.get_position()) > 15:
-            print(f"Warning: Servo {self.identifier} is not reporting as moving but position {self.get_position()} does not match target position {self.pos}")
+        current_position = self.get_position()
+        if self.pos is not None and current_position is not None and abs(self.pos - current_position) > 15:
+            self.log(
+                f"Servo {self.identifier} is not reporting as moving but position {current_position} does not match target position {self.pos}",
+                level='warning'
+            )
+            return False
         return False
 
     def get_position(self):
@@ -416,7 +429,7 @@ def calibrate_dynamic(self):
                     max_pos = pos
                 # Print on the same line, pad with spaces to clear previous content
                 # (4095 = 360 degrees, so 1264 = 111 degrees)
-                range_degrees = (360/4095)*(max_pos - min_pos) if min_pos is not None and max_pos is not None else 'N/A'
+                range_degrees = self.calculate_range_degrees(max_pos, min_pos)
                 print(f"\rCurrent position: {pos}, Min: {min_pos}, Max: {max_pos} Range(deg): {range_degrees}", end='', flush=True)
                 time.sleep(0.05)
                 if sys.stdin in select.select([sys.stdin], [], [], 0)[0]:
@@ -436,6 +449,10 @@ def calibrate_dynamic(self):
             self.start = (min_pos + max_pos) // 2
             self.log(f"Start position {self.start} out of new range, setting to midpoint {self.start}")
 
+    def calculate_range_degrees(self, max_pos, min_pos):
+        max_val = SC_MAX if self.model.startswith('SC') else ST_MAX
+        return (360/max_val)*abs(max_pos - min_pos) if min_pos is not None and max_pos is not None else 0
+
     def calibrate_to_center(self):
         """
         Move the servo to the center of its range.
@@ -453,7 +470,4 @@ def calibrate_to_center(self):
             self.packetHandler.write1ByteTxRx(self.portHandler, self.index, ADDR_SCS_GOAL_ACC, self.acceleration)
             self.packetHandler.write2ByteTxRx(self.portHandler, self.index, ADDR_SCS_GOAL_SPEED, self.speed)
             self.packetHandler.write2ByteTxRx(self.portHandler, self.index, ADDR_SCS_GOAL_POSITION, self.pos)
-            self.log(f"Moved servo {self.identifier} to position {self.pos}")
-
-
-
+            self.log(f"Moved servo {self.identifier} to position {self.pos}")