From b2b2cc089ee02d274c26fa4fb4f43963ace411ac Mon Sep 17 00:00:00 2001
From: danic85 <6583012+danic85@users.noreply.github.com>
Date: Mon, 20 Apr 2026 18:23:43 +0100
Subject: [PATCH 01/10] WIP animations

---
 environments/cody.yml                        | 68 +++++++++-----------
 src/modules/actuators/bus_servo/bus_servo.py |  6 +-
 src/modules/personality/personality.py       | 51 +++++++--------
 3 files changed, 57 insertions(+), 68 deletions(-)

diff --git a/environments/cody.yml b/environments/cody.yml
index cd2e9b17..58779a04 100644
--- a/environments/cody.yml
+++ b/environments/cody.yml
@@ -20,47 +20,46 @@ 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': 2728, 'leg_r_hip': 2910, 'leg_r_knee': 593, 'leg_r_ankle': 2922, 'neck_pan': 1712, 'neck_tilt': 116, 'leg_l_tilt': 1604, 'leg_l_hip': 1374, 'leg_l_knee': 3477, 'leg_l_ankle': 1136}
+      - 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}
+      - 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}
   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
       start: 1706
       # calibrate_on_boot: true
       # demonstrate_on_boot: true
@@ -69,7 +68,6 @@ bus_servo:
     #   baudrate: 115200
     #   id: 4
     #   range: [0, 1023]
-    #   range_degrees: 300
     #   start: 511
       # calibrate_on_boot: true
       # demonstrate_on_boot: true
@@ -79,7 +77,6 @@ bus_servo:
       baudrate: 115200
       id: 11
       range: [30, 220]
-      range_degrees: 55.6
       start: 120
       speed: 60 # max=0, 1 - 3000 tested
       # acceleration: 0 # max (no significant difference between this and 1 - 3000)
@@ -97,27 +94,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:
diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py
index fd53ab8f..a6518532 100644
--- a/src/modules/actuators/bus_servo/bus_servo.py
+++ b/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)
@@ -416,7 +416,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 +436,8 @@ 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):
+        return (360/ST_MAX)*(max_pos - min_pos) if min_pos is not None and max_pos is not None else 'N/A'
     def calibrate_to_center(self):
         """
         Move the servo to the center of its range.
diff --git a/src/modules/personality/personality.py b/src/modules/personality/personality.py
index 3ca15cdb..962dd94b 100644
--- a/src/modules/personality/personality.py
+++ b/src/modules/personality/personality.py
@@ -34,11 +34,11 @@ def __init__(self, **kwargs):
         self.imu = {} # Set in main.py
         self.vision = None # Set in main.py
         self.euler = None
-        self.balance_enabled = kwargs.get('balance_enabled', True)
+        self.balance_enabled = kwargs.get('balance_enabled', False)
         self.chicken_head_enabled = kwargs.get('chicken_head_enabled', False)
-        self.track_people = kwargs.get('track_people', True) # Uses vision data to track detected people with the eyes, and optionally neck servos.
+        self.track_people = kwargs.get('track_people', False) # Uses vision data to track detected people with the eyes, and optionally neck servos.
         self.track_people_servos = kwargs.get('track_people_servos', False) # Moves neck servos to track detected people.
-        self.one_leg_balance_enabled = kwargs.get('one_leg_balance_enabled', True)
+        self.one_leg_balance_enabled = kwargs.get('one_leg_balance_enabled', False)
         self.servos = {} # Set in main.py
         self.pose = None
 
@@ -215,34 +215,27 @@ def cycle_display(self):
             self.publish('display/body/text', text=f"{self.pose}", font_size=20)
 
     def loop_10(self):
-        # loop 3 times:
-        # for i in range(3):
-        #     self.servos['leg_l_ankle'].move(1000, speed=0)
-        #     self.servos['leg_l_ankle'].move(1500, speed=0) 
-        # self.servos['leg_l_ankle'].move_degrees(50)
         # self.scan_vision()
         # self.output_current_pose()
-        # self.publish('servo/pose', pose_name='wave_1') # For testing pose movement
-        # time.sleep(1)
-        # current_pose = self.estimate_current_pose()
-        # if current_pose not in self.servos['leg_r_tilt'].poses:
-        #     return
-        # if current_pose == 'sit':
-            # self.publish('servo/pose', pose_name='wave_1') # For testing pose movement
-            # self.publish('servo/pose', pose_name='wave_2') # For testing pose movement
-            # self.publish('servo/pose', pose_name='wave_1') # For testing pose movement
-            # self.publish('servo/pose', pose_name='wave_2') # For testing pose movement
-            # self.publish('servo/pose', pose_name='sit') # For testing pose movement
-            # self.pose = 'sit'
-        # elif current_pose == 'sit_edge':
-        #     self.pose = 'sit_edge_swing_l'
-        # elif current_pose == 'sit_edge_swing_l':
-        #     self.pose = 'sit_edge_swing_r'
-        # elif current_pose == 'sit_edge_swing_r':
-        #     self.pose = 'sit_edge'
-        # elif current_pose == 'legs_forward':
-        #     self.pose = 'sit'
-        # self.publish('servo/pose', pose_name=self.pose) # For testing pose movement
+        current_pose = self.estimate_current_pose()
+        if current_pose not in self.servos['leg_r_tilt'].poses:
+            self.publish('servo/pose', pose_name='legs_forward') # Start in a default pose
+            return
+        if current_pose == 'sit':
+            self.publish('servo/pose', pose_name='wave_1') # For testing pose movement
+            self.publish('servo/pose', pose_name='wave_2') # For testing pose movement
+            self.publish('servo/pose', pose_name='wave_1') # For testing pose movement
+            self.publish('servo/pose', pose_name='wave_2') # For testing pose movement
+            self.pose = 'sit'
+        elif current_pose == 'sit_edge':
+            self.pose = 'sit_edge_swing_l'
+        elif current_pose == 'sit_edge_swing_l':
+            self.pose = 'sit_edge_swing_r'
+        elif current_pose == 'sit_edge_swing_r':
+            self.pose = 'sit_edge'
+        elif current_pose == 'legs_forward':
+            self.pose = 'sit'
+        self.publish('servo/pose', pose_name=self.pose) # For testing pose movement
         pass
     
     def loop(self):

From 836fa44fc3049946ef7cc603d0c0d32ec0b00f4d Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Mon, 20 Apr 2026 17:31:33 +0000
Subject: [PATCH 02/10] Fix bus servo moving-state mismatch handling

Agent-Logs-Url: https://github.com/makerforgetech/modular-biped/sessions/4eafc430-617c-4c05-a2bd-ea0bc585fcc3

Co-authored-by: danic85 <6583012+danic85@users.noreply.github.com>
---
 src/modules/actuators/bus_servo/bus_servo.py   | 10 +++++++---
 .../bus_servo/tests/test_bus_servo.py          | 18 ++++++++++++++++++
 2 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py
index a6518532..1d2bdc42 100644
--- a/src/modules/actuators/bus_servo/bus_servo.py
+++ b/src/modules/actuators/bus_servo/bus_servo.py
@@ -277,8 +277,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 True
         return False
         
     def get_position(self):
@@ -458,4 +463,3 @@ def calibrate_to_center(self):
             self.log(f"Moved servo {self.identifier} to position {self.pos}")
 
 
-
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 9e90d3dc..2705b04b 100644
--- a/src/modules/actuators/bus_servo/tests/test_bus_servo.py
+++ b/src/modules/actuators/bus_servo/tests/test_bus_servo.py
@@ -24,5 +24,23 @@ def test_init_defaults(self):
         self.assertEqual(servo.baudrate, 1000000)
         self.assertEqual(servo.port, '/dev/ttyAMA0')
 
+    def test_is_moving_true_when_mismatch_to_target(self):
+        servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')
+        servo.pos = 3396
+        servo.get_moving = MagicMock(return_value=0)
+        servo.get_position = MagicMock(return_value=3047)
+        servo.log = MagicMock()
+
+        self.assertTrue(servo.is_moving())
+        servo.log.assert_called_once()
+
+    def test_is_moving_false_when_within_tolerance(self):
+        servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')
+        servo.pos = 3396
+        servo.get_moving = MagicMock(return_value=0)
+        servo.get_position = MagicMock(return_value=3390)
+
+        self.assertFalse(servo.is_moving())
+
 if __name__ == '__main__':
     unittest.main()

From b33eda1671a91af1cd896cc229b2143c78efdc5f Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Mon, 20 Apr 2026 17:32:49 +0000
Subject: [PATCH 03/10] Strengthen bus_servo warning assertion in tests

Agent-Logs-Url: https://github.com/makerforgetech/modular-biped/sessions/4eafc430-617c-4c05-a2bd-ea0bc585fcc3

Co-authored-by: danic85 <6583012+danic85@users.noreply.github.com>
---
 src/modules/actuators/bus_servo/tests/test_bus_servo.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

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 2705b04b..2ad49369 100644
--- a/src/modules/actuators/bus_servo/tests/test_bus_servo.py
+++ b/src/modules/actuators/bus_servo/tests/test_bus_servo.py
@@ -32,7 +32,10 @@ def test_is_moving_true_when_mismatch_to_target(self):
         servo.log = MagicMock()
 
         self.assertTrue(servo.is_moving())
-        servo.log.assert_called_once()
+        servo.log.assert_called_once_with(
+            "Servo test is not reporting as moving but position 3047 does not match target position 3396",
+            level='warning'
+        )
 
     def test_is_moving_false_when_within_tolerance(self):
         servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')

From b0749b97cd7ae0de90dc74614341175ccc2f89c5 Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Mon, 20 Apr 2026 17:33:46 +0000
Subject: [PATCH 04/10] Assert no warning when servo is within tolerance

Agent-Logs-Url: https://github.com/makerforgetech/modular-biped/sessions/4eafc430-617c-4c05-a2bd-ea0bc585fcc3

Co-authored-by: danic85 <6583012+danic85@users.noreply.github.com>
---
 src/modules/actuators/bus_servo/tests/test_bus_servo.py | 2 ++
 1 file changed, 2 insertions(+)

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 2ad49369..045ca773 100644
--- a/src/modules/actuators/bus_servo/tests/test_bus_servo.py
+++ b/src/modules/actuators/bus_servo/tests/test_bus_servo.py
@@ -42,8 +42,10 @@ def test_is_moving_false_when_within_tolerance(self):
         servo.pos = 3396
         servo.get_moving = MagicMock(return_value=0)
         servo.get_position = MagicMock(return_value=3390)
+        servo.log = MagicMock()
 
         self.assertFalse(servo.is_moving())
+        servo.log.assert_not_called()
 
 if __name__ == '__main__':
     unittest.main()

From 5d4c8f39b901e7ecff5cddb4454df8a6664668a7 Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Mon, 20 Apr 2026 19:59:49 +0000
Subject: [PATCH 05/10] Share bus port handler across servos and serialize SDK
 access

Agent-Logs-Url: https://github.com/makerforgetech/modular-biped/sessions/ce792005-8d16-4964-9853-5beedd8c2a79

Co-authored-by: danic85 <6583012+danic85@users.noreply.github.com>
---
 src/modules/actuators/bus_servo/bus_servo.py  | 88 ++++++++++++-------
 .../bus_servo/tests/test_bus_servo.py         | 23 +++++
 2 files changed, 81 insertions(+), 30 deletions(-)

diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py
index 1d2bdc42..6666583d 100644
--- a/src/modules/actuators/bus_servo/bus_servo.py
+++ b/src/modules/actuators/bus_servo/bus_servo.py
@@ -4,6 +4,7 @@
 import sys
 import select
 import time
+from threading import Lock
 
 # if os.name == 'nt':
 #     import msvcrt
@@ -40,6 +41,10 @@
 SC_MAX = 1024
 
 class Servo(BaseModule):
+    _shared_port_handlers = {}
+    _shared_port_locks = {}
+    _shared_port_refcounts = {}
+
     def __init__(self, **kwargs):
         """
         Servo class
@@ -65,10 +70,23 @@ def __init__(self, **kwargs):
         # Convert to dict:
         self.poses = {list(pose.keys())[0]: list(pose.values())[0] for pose in poses_list}
 
-        # Initialize PortHandler instance
-        # Set the port path
-        # Get methods and members of PortHandlerLinux or PortHandlerWindows
-        self.portHandler = PortHandler(self.port)
+        self._port_key = (self.port, self.baudrate)
+        if self._port_key not in self._shared_port_handlers:
+            port_handler = PortHandler(self.port)
+            # Open port
+            if not port_handler.openPort():
+                raise Exception("Failed to open the port")
+
+            # Set port baudrate
+            if not port_handler.setBaudRate(self.baudrate):
+                raise Exception("Failed to change the baudrate")
+            self._shared_port_handlers[self._port_key] = port_handler
+            self._shared_port_locks[self._port_key] = Lock()
+            self._shared_port_refcounts[self._port_key] = 0
+
+        self.portHandler = self._shared_port_handlers[self._port_key]
+        self._port_lock = self._shared_port_locks[self._port_key]
+        self._shared_port_refcounts[self._port_key] += 1
         
         # if model starts with ST or SC, use the appropriate packet handler
         if self.model.startswith('ST'):
@@ -77,31 +95,32 @@ def __init__(self, **kwargs):
             self.packetHandler = PacketHandler(1) # 1 = protocol_end in examples
         else:
             raise ValueError(f"Unknown servo model: {self.model}. Supported models are ST and SC.")
-
-        # Open port
-        if not self.portHandler.openPort():
-            raise Exception("Failed to open the port")
-
-        # Set port baudrate
-        if not self.portHandler.setBaudRate(self.baudrate):
-            raise Exception("Failed to change the baudrate")
         
     def detach(self):
         # Detach servo
         if self.model.startswith('ST'):
             # Disable torque for STServo
-            sts_comm_result, sts_error = self.packetHandler.write1ByteTxRx(self.index, ADDR_TORQUE_ENABLE, 0)
+            with self._port_lock:
+                sts_comm_result, sts_error = self.packetHandler.write1ByteTxRx(self.index, ADDR_TORQUE_ENABLE, 0)
             if not self.handle_errors(sts_comm_result, sts_error):
                 self.log(f"ST Servo {self.identifier} disabled")
         else:
             # Disable torque for SCServo
-            scs_comm_result, scs_error = self.packetHandler.write1ByteTxRx(self.portHandler, self.index, ADDR_TORQUE_ENABLE, 0)
+            with self._port_lock:
+                scs_comm_result, scs_error = self.packetHandler.write1ByteTxRx(self.portHandler, self.index, ADDR_TORQUE_ENABLE, 0)
             if not self.handle_errors(scs_comm_result, scs_error):
                 self.log(f"SC Servo {self.identifier} disabled")
     
     def exit(self):
         self.detach()
-        self.portHandler.closePort()
+        if self._port_key in self._shared_port_refcounts:
+            self._shared_port_refcounts[self._port_key] -= 1
+            if self._shared_port_refcounts[self._port_key] <= 0:
+                with self._port_lock:
+                    self.portHandler.closePort()
+                del self._shared_port_refcounts[self._port_key]
+                del self._shared_port_locks[self._port_key]
+                del self._shared_port_handlers[self._port_key]
 
     def setup_messaging(self):
         self.subscribe('servo:' + self.identifier + ':mvabs', self.move)
@@ -145,13 +164,15 @@ def move_to_pose(self, pose_name):
             self.log(f"Pose '{pose_name}' not found for servo {self.identifier}", level='warning')
             
     def _sc_write(self, type, value, verbose=False):
-        comm_result, error = self.packetHandler.write2ByteTxRx(self.portHandler, self.index, type, value)
+        with self._port_lock:
+            comm_result, error = self.packetHandler.write2ByteTxRx(self.portHandler, self.index, type, value)
         if hasattr(self.packetHandler, 'getTxRxResult') and verbose:
             self.log(f"[SCServo Result] {self.packetHandler.getTxRxResult(comm_result)}")
         if hasattr(self.packetHandler, 'getRxPacketError') and error != 0:
             self.log(f"[SCServo Error] {self.identifier} {self.packetHandler.getRxPacketError(error)}")
             # Attempting to clear error by toggling torque off and on
-            comm_result, error = self.packetHandler.write1ByteTxRx(self.portHandler, self.index, ADDR_TORQUE_ENABLE, 0)
+            with self._port_lock:
+                comm_result, error = self.packetHandler.write1ByteTxRx(self.portHandler, self.index, ADDR_TORQUE_ENABLE, 0)
             time.sleep(0.1)
             # comm_result, error = self.packetHandler.write1ByteTxRx(self.portHandler, self.index, ADDR_TORQUE_ENABLE, )
             # self._sc_write(type, value)
@@ -244,7 +265,8 @@ def _do_move(self, position, speed=None, acceleration=None):
         
         # Write STServo goal position
         if self.model.startswith('ST'):
-            sts_comm_result, sts_error = self.packetHandler.WritePosEx(self.index, position, speed, acceleration)
+            with self._port_lock:
+                sts_comm_result, sts_error = self.packetHandler.WritePosEx(self.index, position, speed, acceleration)
             if not self.handle_errors(sts_comm_result, sts_error):
                 self.log(f"Moved ST servo {self.identifier} from {self.pos} to position {position}")
                 self.pos = position  # Update current position
@@ -292,7 +314,8 @@ def get_position(self):
         """
         if self.model.startswith('ST'):
             # Read STServo present position
-            sts_present_position, sts_present_speed, sts_comm_result, sts_error = self.packetHandler.ReadPosSpeed(self.index)
+            with self._port_lock:
+                sts_present_position, sts_present_speed, sts_comm_result, sts_error = self.packetHandler.ReadPosSpeed(self.index)
             if not self.handle_errors(sts_comm_result, sts_error):
                 # self.log("[ID:%03d] PresPos:%d PresSpd:%d" % (self.index, sts_present_position, sts_present_speed))
                 return sts_present_position
@@ -305,7 +328,8 @@ def get_speed(self):
         """
         if self.model.startswith('ST'):
             # Read STServo present position
-            sts_present_position, sts_present_speed, sts_comm_result, sts_error = self.packetHandler.ReadPosSpeed(self.index)
+            with self._port_lock:
+                sts_present_position, sts_present_speed, sts_comm_result, sts_error = self.packetHandler.ReadPosSpeed(self.index)
             if not self.handle_errors(sts_comm_result, sts_error):
                 return sts_present_speed
         else:
@@ -317,7 +341,8 @@ def get_moving(self):
         """
         if self.model.startswith('ST'):
             # Read STServo moving status
-            moving, sts_comm_result, sts_error = self.packetHandler.ReadMoving(self.index)
+            with self._port_lock:
+                moving, sts_comm_result, sts_error = self.packetHandler.ReadMoving(self.index)
             if not self.handle_errors(sts_comm_result, sts_error):
                 return moving
         else:
@@ -328,7 +353,8 @@ def get_moving(self):
         
     def sc_get_position_speed(self, pos_or_speed):
         # Read SCServo present position
-        scs_present_position_speed, scs_comm_result, scs_error = self.packetHandler.read4ByteTxRx(self.portHandler, self.index, ADDR_SCS_PRESENT_POSITION)
+        with self._port_lock:
+            scs_present_position_speed, scs_comm_result, scs_error = self.packetHandler.read4ByteTxRx(self.portHandler, self.index, ADDR_SCS_PRESENT_POSITION)
         if not self.handle_errors(scs_comm_result, scs_error):
             scs_present_position = SCS_LOWORD(scs_present_position_speed)
             scs_present_speed = SCS_HIWORD(scs_present_position_speed)
@@ -344,14 +370,16 @@ def enable_continuous(self):
         """
         if self.model.startswith('SC'):
             raise ValueError("Continuous mode is not supported for SCServo models.")
-        sts_comm_result, sts_error = self.packetHandler.WheelMode(self.index)
+        with self._port_lock:
+            sts_comm_result, sts_error = self.packetHandler.WheelMode(self.index)
         if not self.handle_errors(sts_comm_result, sts_error):
             self.log(f"Servo {self.name} set to wheel mode")
             
     def turn_wheel(self, speed):
         if self.model.startswith('SC'):
             raise ValueError("Continuous mode is not supported for SCServo models.")
-        sts_comm_result, sts_error = self.packetHandler.WriteSpec(self.index, speed, self.acceleration)
+        with self._port_lock:
+            sts_comm_result, sts_error = self.packetHandler.WriteSpec(self.index, speed, self.acceleration)
         if not self.handle_errors(sts_comm_result, sts_error):
             self.log(f"Servo {self.name} turned at speed {speed}")
             
@@ -451,15 +479,15 @@ def calibrate_to_center(self):
         
         if self.model.startswith('ST'):
             self.pos = (self.range[0] + self.range[1]) // 2  # Update current position to center
-            sts_comm_result, sts_error = self.packetHandler.WritePosEx(self.index, self.pos, self.speed, self.acceleration)
+            with self._port_lock:
+                sts_comm_result, sts_error = self.packetHandler.WritePosEx(self.index, self.pos, self.speed, self.acceleration)
             if not self.handle_errors(sts_comm_result, sts_error):
                 self.log(f"Moved servo {self.identifier} to position {self.pos}")
                 
         elif self.model.startswith('SC'):
             self.pos = (self.range[0] + self.range[1]) // 2  # Update current position to center
-            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)
+            with self._port_lock:
+                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}")
-
-
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 045ca773..884527f6 100644
--- a/src/modules/actuators/bus_servo/tests/test_bus_servo.py
+++ b/src/modules/actuators/bus_servo/tests/test_bus_servo.py
@@ -11,6 +11,12 @@
 from modules.actuators.bus_servo.bus_servo import Servo
 
 class TestBusServo(unittest.TestCase):
+    def setUp(self):
+        Servo._shared_port_handlers = {}
+        Servo._shared_port_locks = {}
+        Servo._shared_port_refcounts = {}
+        mock_st_sdk.reset_mock()
+        mock_sc_sdk.reset_mock()
 
     def test_init(self):
         servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')
@@ -47,5 +53,22 @@ def test_is_moving_false_when_within_tolerance(self):
         self.assertFalse(servo.is_moving())
         servo.log.assert_not_called()
 
+    def test_shared_port_handler_per_port_and_baudrate(self):
+        Servo(name='test1', id=1, range=[0, 4095], model='ST3215')
+        Servo(name='test2', id=2, range=[0, 4095], model='ST3215')
+        mock_st_sdk.PortHandler.assert_called_once_with('/dev/ttyAMA0')
+
+    def test_exit_closes_shared_port_only_once(self):
+        servo1 = Servo(name='test1', id=1, range=[0, 4095], model='ST3215')
+        servo2 = Servo(name='test2', id=2, range=[0, 4095], model='ST3215')
+        port_handler = servo1.portHandler
+        servo1.detach = MagicMock()
+        servo2.detach = MagicMock()
+
+        servo1.exit()
+        port_handler.closePort.assert_not_called()
+        servo2.exit()
+        port_handler.closePort.assert_called_once()
+
 if __name__ == '__main__':
     unittest.main()

From 586c158534bc562590b527eac83781fd34db4655 Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Mon, 20 Apr 2026 20:03:10 +0000
Subject: [PATCH 06/10] Harden shared port refcount close and SC center error
 handling

Agent-Logs-Url: https://github.com/makerforgetech/modular-biped/sessions/ce792005-8d16-4964-9853-5beedd8c2a79

Co-authored-by: danic85 <6583012+danic85@users.noreply.github.com>
---
 src/modules/actuators/bus_servo/bus_servo.py | 23 ++++++++++++++------
 1 file changed, 16 insertions(+), 7 deletions(-)

diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py
index 6666583d..03d5ae92 100644
--- a/src/modules/actuators/bus_servo/bus_servo.py
+++ b/src/modules/actuators/bus_servo/bus_servo.py
@@ -114,9 +114,11 @@ def detach(self):
     def exit(self):
         self.detach()
         if self._port_key in self._shared_port_refcounts:
-            self._shared_port_refcounts[self._port_key] -= 1
-            if self._shared_port_refcounts[self._port_key] <= 0:
-                with self._port_lock:
+            with self._port_lock:
+                self._shared_port_refcounts[self._port_key] -= 1
+                if self._shared_port_refcounts[self._port_key] > 0:
+                    return
+                else:
                     self.portHandler.closePort()
                 del self._shared_port_refcounts[self._port_key]
                 del self._shared_port_locks[self._port_key]
@@ -487,7 +489,14 @@ def calibrate_to_center(self):
         elif self.model.startswith('SC'):
             self.pos = (self.range[0] + self.range[1]) // 2  # Update current position to center
             with self._port_lock:
-                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}")
+                acc_result, acc_error = self.packetHandler.write1ByteTxRx(self.portHandler, self.index, ADDR_SCS_GOAL_ACC, self.acceleration)
+                speed_result, speed_error = self.packetHandler.write2ByteTxRx(self.portHandler, self.index, ADDR_SCS_GOAL_SPEED, self.speed)
+                pos_result, pos_error = self.packetHandler.write2ByteTxRx(self.portHandler, self.index, ADDR_SCS_GOAL_POSITION, self.pos)
+            if any([
+                self.handle_errors(acc_result, acc_error),
+                self.handle_errors(speed_result, speed_error),
+                self.handle_errors(pos_result, pos_error),
+            ]):
+                self.log(f"Failed to move servo {self.identifier} to center position {self.pos}", level='error')
+            else:
+                self.log(f"Moved servo {self.identifier} to position {self.pos}")

From 0a600b5b78256b44f9087ba3ed0014c9b0d5d597 Mon Sep 17 00:00:00 2001
From: danic85 <6583012+danic85@users.noreply.github.com>
Date: Mon, 20 Apr 2026 21:29:29 +0100
Subject: [PATCH 07/10] WIP

---
 environments/cody.yml                        |  4 ++-
 src/modules/actuators/bus_servo/bus_servo.py |  4 +--
 src/modules/personality/personality.py       | 30 +++++++++++---------
 3 files changed, 22 insertions(+), 16 deletions(-)

diff --git a/environments/cody.yml b/environments/cody.yml
index 58779a04..7053c02f 100644
--- a/environments/cody.yml
+++ b/environments/cody.yml
@@ -26,8 +26,10 @@ bus_servo:
       - 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': 2728, 'leg_r_hip': 2910, 'leg_r_knee': 593, 'leg_r_ankle': 2922, 'neck_pan': 1712, 'neck_tilt': 116, 'leg_l_tilt': 1604, 'leg_l_hip': 1374, 'leg_l_knee': 3477, 'leg_l_ankle': 1136}
+      - 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}
diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py
index 1d2bdc42..940b17d2 100644
--- a/src/modules/actuators/bus_servo/bus_servo.py
+++ b/src/modules/actuators/bus_servo/bus_servo.py
@@ -57,7 +57,7 @@ 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.speed = kwargs.get('speed', 0) # 3073
         self.acceleration = kwargs.get('acceleration', 50)
         self._move_queue = collections.deque()
         # After loading YAML:
@@ -283,7 +283,7 @@ def is_moving(self):
                 f"Servo {self.identifier} is not reporting as moving but position {current_position} does not match target position {self.pos}",
                 level='warning'
             )
-            return True
+            return False
         return False
         
     def get_position(self):
diff --git a/src/modules/personality/personality.py b/src/modules/personality/personality.py
index 962dd94b..6b80bfca 100644
--- a/src/modules/personality/personality.py
+++ b/src/modules/personality/personality.py
@@ -219,22 +219,26 @@ def loop_10(self):
         # self.output_current_pose()
         current_pose = self.estimate_current_pose()
         if current_pose not in self.servos['leg_r_tilt'].poses:
-            self.publish('servo/pose', pose_name='legs_forward') # Start in a default pose
+            self.publish('servo/pose', pose_name='stand_high') # Start in a default pose
             return
-        if current_pose == 'sit':
-            self.publish('servo/pose', pose_name='wave_1') # For testing pose movement
-            self.publish('servo/pose', pose_name='wave_2') # For testing pose movement
-            self.publish('servo/pose', pose_name='wave_1') # For testing pose movement
-            self.publish('servo/pose', pose_name='wave_2') # For testing pose movement
+        if current_pose == 'stand_low':
+            self.pose = 'stand_high'
+        elif current_pose == 'stand_high' or current_pose == 'stand_dip_l' or current_pose == 'stand_dip_r':
+            for _ in range(2):
+                self.publish('servo/pose', pose_name='stand_dip_l') # For testing pose movement
+                self.publish('servo/pose', pose_name='stand_dip_r') # For testing pose movement
+            self.pose = 'stand_high'
+        elif current_pose == 'sit':
+            for _ in range(3):
+                self.publish('servo/pose', pose_name='wave_1') # For testing pose movement
+                self.publish('servo/pose', pose_name='wave_2') # For testing pose movement
             self.pose = 'sit'
-        elif current_pose == 'sit_edge':
-            self.pose = 'sit_edge_swing_l'
-        elif current_pose == 'sit_edge_swing_l':
-            self.pose = 'sit_edge_swing_r'
-        elif current_pose == 'sit_edge_swing_r':
+        elif current_pose == 'sit_edge' or current_pose == 'sit_edge_swing_l' or current_pose == 'sit_edge_swing_r':
+            # random number between 1 and 4
+            for _ in range(4):
+                self.publish('servo/pose', pose_name='sit_edge_swing_l') # For testing pose movement
+                self.publish('servo/pose', pose_name='sit_edge_swing_r') # For testing pose movement
             self.pose = 'sit_edge'
-        elif current_pose == 'legs_forward':
-            self.pose = 'sit'
         self.publish('servo/pose', pose_name=self.pose) # For testing pose movement
         pass
     

From 2facb7239b8d1fbc35dfde23885af9e8ad931f5e Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Mon, 20 Apr 2026 20:36:29 +0000
Subject: [PATCH 08/10] Revert accidental WIP regressions blocking bus servo
 operation

Agent-Logs-Url: https://github.com/makerforgetech/modular-biped/sessions/607e681f-29e0-49d9-98c9-db6a4e60d6e0

Co-authored-by: danic85 <6583012+danic85@users.noreply.github.com>
---
 environments/cody.yml                        |  4 +--
 src/modules/actuators/bus_servo/bus_servo.py |  4 +--
 src/modules/personality/personality.py       | 30 +++++++++-----------
 3 files changed, 16 insertions(+), 22 deletions(-)

diff --git a/environments/cody.yml b/environments/cody.yml
index 7053c02f..58779a04 100644
--- a/environments/cody.yml
+++ b/environments/cody.yml
@@ -26,10 +26,8 @@ bus_servo:
       - 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_low: {'leg_r_tilt': 2728, 'leg_r_hip': 2910, 'leg_r_knee': 593, 'leg_r_ankle': 2922, 'neck_pan': 1712, 'neck_tilt': 116, 'leg_l_tilt': 1604, 'leg_l_hip': 1374, 'leg_l_knee': 3477, 'leg_l_ankle': 1136}
       - 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}
diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py
index a420fcf4..03d5ae92 100644
--- a/src/modules/actuators/bus_servo/bus_servo.py
+++ b/src/modules/actuators/bus_servo/bus_servo.py
@@ -62,7 +62,7 @@ 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', 0) # 3073
+        self.speed = kwargs.get('speed', 300) # 3073
         self.acceleration = kwargs.get('acceleration', 50)
         self._move_queue = collections.deque()
         # After loading YAML:
@@ -307,7 +307,7 @@ def is_moving(self):
                 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 True
         return False
         
     def get_position(self):
diff --git a/src/modules/personality/personality.py b/src/modules/personality/personality.py
index 6b80bfca..962dd94b 100644
--- a/src/modules/personality/personality.py
+++ b/src/modules/personality/personality.py
@@ -219,26 +219,22 @@ def loop_10(self):
         # self.output_current_pose()
         current_pose = self.estimate_current_pose()
         if current_pose not in self.servos['leg_r_tilt'].poses:
-            self.publish('servo/pose', pose_name='stand_high') # Start in a default pose
+            self.publish('servo/pose', pose_name='legs_forward') # Start in a default pose
             return
-        if current_pose == 'stand_low':
-            self.pose = 'stand_high'
-        elif current_pose == 'stand_high' or current_pose == 'stand_dip_l' or current_pose == 'stand_dip_r':
-            for _ in range(2):
-                self.publish('servo/pose', pose_name='stand_dip_l') # For testing pose movement
-                self.publish('servo/pose', pose_name='stand_dip_r') # For testing pose movement
-            self.pose = 'stand_high'
-        elif current_pose == 'sit':
-            for _ in range(3):
-                self.publish('servo/pose', pose_name='wave_1') # For testing pose movement
-                self.publish('servo/pose', pose_name='wave_2') # For testing pose movement
+        if current_pose == 'sit':
+            self.publish('servo/pose', pose_name='wave_1') # For testing pose movement
+            self.publish('servo/pose', pose_name='wave_2') # For testing pose movement
+            self.publish('servo/pose', pose_name='wave_1') # For testing pose movement
+            self.publish('servo/pose', pose_name='wave_2') # For testing pose movement
             self.pose = 'sit'
-        elif current_pose == 'sit_edge' or current_pose == 'sit_edge_swing_l' or current_pose == 'sit_edge_swing_r':
-            # random number between 1 and 4
-            for _ in range(4):
-                self.publish('servo/pose', pose_name='sit_edge_swing_l') # For testing pose movement
-                self.publish('servo/pose', pose_name='sit_edge_swing_r') # For testing pose movement
+        elif current_pose == 'sit_edge':
+            self.pose = 'sit_edge_swing_l'
+        elif current_pose == 'sit_edge_swing_l':
+            self.pose = 'sit_edge_swing_r'
+        elif current_pose == 'sit_edge_swing_r':
             self.pose = 'sit_edge'
+        elif current_pose == 'legs_forward':
+            self.pose = 'sit'
         self.publish('servo/pose', pose_name=self.pose) # For testing pose movement
         pass
     

From 58148a4e68a8003a583c11642caf31552c961dce Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Mon, 20 Apr 2026 21:02:18 +0000
Subject: [PATCH 09/10] Fix bus servo comm timeouts while preserving deliberate
 speed and mismatch behavior

Agent-Logs-Url: https://github.com/makerforgetech/modular-biped/sessions/f2f37f5f-4b6d-4f4d-a1eb-415829fea05b

Co-authored-by: danic85 <6583012+danic85@users.noreply.github.com>
---
 src/modules/actuators/bus_servo/bus_servo.py  | 69 ++++++++++++-------
 .../bus_servo/tests/test_bus_servo.py         | 11 ++-
 2 files changed, 50 insertions(+), 30 deletions(-)

diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py
index 03d5ae92..43ea5624 100644
--- a/src/modules/actuators/bus_servo/bus_servo.py
+++ b/src/modules/actuators/bus_servo/bus_servo.py
@@ -25,7 +25,7 @@
 
 # sys.path.append("..")
 # Uses STServo and SCServo SDK library
-from modules.actuators.bus_servo.STservo_sdk import PortHandler, sts, COMM_SUCCESS
+from modules.actuators.bus_servo.STservo_sdk import PortHandler, sts, COMM_SUCCESS, COMM_PORT_BUSY, COMM_RX_TIMEOUT
 from modules.actuators.bus_servo.SCservo_sdk import PacketHandler, SCS_LOWORD, SCS_HIWORD, SCS_TOHOST
 
 from modules.base_module import BaseModule
@@ -62,7 +62,7 @@ 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.speed = kwargs.get('speed', 0) # 3073
         self.acceleration = kwargs.get('acceleration', 50)
         self._move_queue = collections.deque()
         # After loading YAML:
@@ -166,8 +166,11 @@ def move_to_pose(self, pose_name):
             self.log(f"Pose '{pose_name}' not found for servo {self.identifier}", level='warning')
             
     def _sc_write(self, type, value, verbose=False):
-        with self._port_lock:
-            comm_result, error = self.packetHandler.write2ByteTxRx(self.portHandler, self.index, type, value)
+        comm_result, error = self._execute_with_retries(
+            lambda: self._locked_call(
+                lambda: self.packetHandler.write2ByteTxRx(self.portHandler, self.index, type, value)
+            )
+        )
         if hasattr(self.packetHandler, 'getTxRxResult') and verbose:
             self.log(f"[SCServo Result] {self.packetHandler.getTxRxResult(comm_result)}")
         if hasattr(self.packetHandler, 'getRxPacketError') and error != 0:
@@ -190,6 +193,24 @@ def _sc_write(self, type, value, verbose=False):
             # speed, speed_comm_result, speed_error = self.packetHandler.read2ByteTxRx(self.portHandler, self.index, ADDR_SCS_PRESENT_SPEED)
             # 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 _locked_call(self, operation):
+        with self._port_lock:
+            return operation()
+
+    def _execute_with_retries(self, operation, retries=2, retry_delay=0.002):
+        result = operation()
+        if not isinstance(result, tuple) or len(result) < 2:
+            return result
+
+        comm_result = result[-2]
+        retries_remaining = retries
+        while comm_result in (COMM_PORT_BUSY, COMM_RX_TIMEOUT) and retries_remaining > 0:
+            time.sleep(retry_delay)
+            result = operation()
+            comm_result = result[-2]
+            retries_remaining -= 1
+        return result
     
     def move_degrees(self, degrees):
         if self.range_degrees is None:
@@ -267,8 +288,11 @@ def _do_move(self, position, speed=None, acceleration=None):
         
         # Write STServo goal position
         if self.model.startswith('ST'):
-            with self._port_lock:
-                sts_comm_result, sts_error = self.packetHandler.WritePosEx(self.index, position, speed, acceleration)
+            sts_comm_result, sts_error = self._execute_with_retries(
+                lambda: self._locked_call(
+                    lambda: self.packetHandler.WritePosEx(self.index, position, speed, acceleration)
+                )
+            )
             if not self.handle_errors(sts_comm_result, sts_error):
                 self.log(f"Moved ST servo {self.identifier} from {self.pos} to position {position}")
                 self.pos = position  # Update current position
@@ -299,16 +323,7 @@ def move_relative(self, delta):
         
         
     def is_moving(self):
-        if self.get_moving() == 1:
-            return True
-        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 True
-        return False
+        return self.get_moving() == 1
         
     def get_position(self):
         """
@@ -316,8 +331,9 @@ def get_position(self):
         """
         if self.model.startswith('ST'):
             # Read STServo present position
-            with self._port_lock:
-                sts_present_position, sts_present_speed, sts_comm_result, sts_error = self.packetHandler.ReadPosSpeed(self.index)
+            sts_present_position, sts_present_speed, sts_comm_result, sts_error = self._execute_with_retries(
+                lambda: self._locked_call(lambda: self.packetHandler.ReadPosSpeed(self.index))
+            )
             if not self.handle_errors(sts_comm_result, sts_error):
                 # self.log("[ID:%03d] PresPos:%d PresSpd:%d" % (self.index, sts_present_position, sts_present_speed))
                 return sts_present_position
@@ -330,8 +346,9 @@ def get_speed(self):
         """
         if self.model.startswith('ST'):
             # Read STServo present position
-            with self._port_lock:
-                sts_present_position, sts_present_speed, sts_comm_result, sts_error = self.packetHandler.ReadPosSpeed(self.index)
+            sts_present_position, sts_present_speed, sts_comm_result, sts_error = self._execute_with_retries(
+                lambda: self._locked_call(lambda: self.packetHandler.ReadPosSpeed(self.index))
+            )
             if not self.handle_errors(sts_comm_result, sts_error):
                 return sts_present_speed
         else:
@@ -343,8 +360,9 @@ def get_moving(self):
         """
         if self.model.startswith('ST'):
             # Read STServo moving status
-            with self._port_lock:
-                moving, sts_comm_result, sts_error = self.packetHandler.ReadMoving(self.index)
+            moving, sts_comm_result, sts_error = self._execute_with_retries(
+                lambda: self._locked_call(lambda: self.packetHandler.ReadMoving(self.index))
+            )
             if not self.handle_errors(sts_comm_result, sts_error):
                 return moving
         else:
@@ -355,8 +373,11 @@ def get_moving(self):
         
     def sc_get_position_speed(self, pos_or_speed):
         # Read SCServo present position
-        with self._port_lock:
-            scs_present_position_speed, scs_comm_result, scs_error = self.packetHandler.read4ByteTxRx(self.portHandler, self.index, ADDR_SCS_PRESENT_POSITION)
+        scs_present_position_speed, scs_comm_result, scs_error = self._execute_with_retries(
+            lambda: self._locked_call(
+                lambda: self.packetHandler.read4ByteTxRx(self.portHandler, self.index, ADDR_SCS_PRESENT_POSITION)
+            )
+        )
         if not self.handle_errors(scs_comm_result, scs_error):
             scs_present_position = SCS_LOWORD(scs_present_position_speed)
             scs_present_speed = SCS_HIWORD(scs_present_position_speed)
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 884527f6..ca132c96 100644
--- a/src/modules/actuators/bus_servo/tests/test_bus_servo.py
+++ b/src/modules/actuators/bus_servo/tests/test_bus_servo.py
@@ -29,19 +29,18 @@ def test_init_defaults(self):
         servo = Servo(name='test', id=1, range=[0, 1023])
         self.assertEqual(servo.baudrate, 1000000)
         self.assertEqual(servo.port, '/dev/ttyAMA0')
+        self.assertEqual(servo.speed, 0)
 
-    def test_is_moving_true_when_mismatch_to_target(self):
+    def test_is_moving_false_when_mismatch_to_target(self):
         servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')
         servo.pos = 3396
         servo.get_moving = MagicMock(return_value=0)
         servo.get_position = MagicMock(return_value=3047)
         servo.log = MagicMock()
 
-        self.assertTrue(servo.is_moving())
-        servo.log.assert_called_once_with(
-            "Servo test is not reporting as moving but position 3047 does not match target position 3396",
-            level='warning'
-        )
+        self.assertFalse(servo.is_moving())
+        servo.get_position.assert_not_called()
+        servo.log.assert_not_called()
 
     def test_is_moving_false_when_within_tolerance(self):
         servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')

From a33efa18f5ff67cc2c03fd9eaec1e87d1a96619a Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Mon, 20 Apr 2026 21:07:28 +0000
Subject: [PATCH 10/10] Stabilize bus servo comms with retry logic while
 keeping deliberate speed and moving semantics

Agent-Logs-Url: https://github.com/makerforgetech/modular-biped/sessions/f2f37f5f-4b6d-4f4d-a1eb-415829fea05b

Co-authored-by: danic85 <6583012+danic85@users.noreply.github.com>
---
 src/modules/actuators/bus_servo/bus_servo.py  | 12 +++--
 .../bus_servo/tests/test_bus_servo.py         | 48 ++++++++++++++++++-
 2 files changed, 55 insertions(+), 5 deletions(-)

diff --git a/src/modules/actuators/bus_servo/bus_servo.py b/src/modules/actuators/bus_servo/bus_servo.py
index 43ea5624..10c630b5 100644
--- a/src/modules/actuators/bus_servo/bus_servo.py
+++ b/src/modules/actuators/bus_servo/bus_servo.py
@@ -198,18 +198,22 @@ def _locked_call(self, operation):
         with self._port_lock:
             return operation()
 
-    def _execute_with_retries(self, operation, retries=2, retry_delay=0.002):
+    def _execute_with_retries(self, operation, max_attempts=3, retry_delay=0.002):
+        # ST/SC SDK methods used in this module return tuples with
+        # communication result and packet error in the final two positions.
         result = operation()
         if not isinstance(result, tuple) or len(result) < 2:
             return result
 
         comm_result = result[-2]
-        retries_remaining = retries
-        while comm_result in (COMM_PORT_BUSY, COMM_RX_TIMEOUT) and retries_remaining > 0:
+        attempts = 1
+        while comm_result in (COMM_PORT_BUSY, COMM_RX_TIMEOUT) and attempts < max_attempts:
             time.sleep(retry_delay)
             result = operation()
+            if not isinstance(result, tuple) or len(result) < 2:
+                return result
             comm_result = result[-2]
-            retries_remaining -= 1
+            attempts += 1
         return result
     
     def move_degrees(self, degrees):
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 ca132c96..3c0d5b62 100644
--- a/src/modules/actuators/bus_servo/tests/test_bus_servo.py
+++ b/src/modules/actuators/bus_servo/tests/test_bus_servo.py
@@ -9,6 +9,7 @@
 sys.modules['modules.actuators.bus_servo.SCservo_sdk'] = mock_sc_sdk
 
 from modules.actuators.bus_servo.bus_servo import Servo
+from modules.actuators.bus_servo import bus_servo as bus_servo_module
 
 class TestBusServo(unittest.TestCase):
     def setUp(self):
@@ -31,7 +32,7 @@ def test_init_defaults(self):
         self.assertEqual(servo.port, '/dev/ttyAMA0')
         self.assertEqual(servo.speed, 0)
 
-    def test_is_moving_false_when_mismatch_to_target(self):
+    def test_is_moving_returns_false_when_get_moving_returns_zero(self):
         servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')
         servo.pos = 3396
         servo.get_moving = MagicMock(return_value=0)
@@ -42,6 +43,51 @@ def test_is_moving_false_when_mismatch_to_target(self):
         servo.get_position.assert_not_called()
         servo.log.assert_not_called()
 
+    def test_is_moving_returns_true_when_get_moving_returns_one(self):
+        servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')
+        servo.get_moving = MagicMock(return_value=1)
+        self.assertTrue(servo.is_moving())
+
+    def test_execute_with_retries_retries_port_busy_then_succeeds(self):
+        servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')
+        operation = MagicMock(side_effect=[(-1, 0), (0, 0)])
+        with patch.object(bus_servo_module, 'COMM_PORT_BUSY', -1), \
+             patch.object(bus_servo_module, 'COMM_RX_TIMEOUT', -6), \
+             patch('modules.actuators.bus_servo.bus_servo.time.sleep') as mock_sleep:
+            result = servo._execute_with_retries(operation, max_attempts=3, retry_delay=0)
+        self.assertEqual(result, (0, 0))
+        self.assertEqual(operation.call_count, 2)
+        mock_sleep.assert_called_once_with(0)
+
+    def test_execute_with_retries_returns_immediately_on_success(self):
+        servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')
+        operation = MagicMock(return_value=(0, 0))
+        with patch.object(bus_servo_module, 'COMM_PORT_BUSY', -1), \
+             patch.object(bus_servo_module, 'COMM_RX_TIMEOUT', -6), \
+             patch('modules.actuators.bus_servo.bus_servo.time.sleep') as mock_sleep:
+            result = servo._execute_with_retries(operation, max_attempts=3, retry_delay=0)
+        self.assertEqual(result, (0, 0))
+        self.assertEqual(operation.call_count, 1)
+        mock_sleep.assert_not_called()
+
+    def test_execute_with_retries_returns_failure_after_max_attempts(self):
+        servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')
+        operation = MagicMock(return_value=(-1, 0))
+        with patch.object(bus_servo_module, 'COMM_PORT_BUSY', -1), patch.object(bus_servo_module, 'COMM_RX_TIMEOUT', -6):
+            result = servo._execute_with_retries(operation, max_attempts=3, retry_delay=0)
+        self.assertEqual(result, (-1, 0))
+        self.assertEqual(operation.call_count, 3)
+
+    def test_execute_with_retries_applies_retry_delay(self):
+        servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')
+        operation = MagicMock(side_effect=[(-1, 0), (0, 0)])
+        with patch.object(bus_servo_module, 'COMM_PORT_BUSY', -1), \
+             patch.object(bus_servo_module, 'COMM_RX_TIMEOUT', -6), \
+             patch('modules.actuators.bus_servo.bus_servo.time.sleep') as mock_sleep:
+            result = servo._execute_with_retries(operation, max_attempts=3, retry_delay=0.01)
+        self.assertEqual(result, (0, 0))
+        mock_sleep.assert_called_once_with(0.01)
+
     def test_is_moving_false_when_within_tolerance(self):
         servo = Servo(name='test', id=1, range=[0, 4095], model='ST3215')
         servo.pos = 3396