Skip to content
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
28 commits
Select commit Hold shift + click to select a range
4c8c7d9
WIP bus servo management
danic85 Jul 12, 2025
d18189c
Update config
danic85 Jul 12, 2025
60d524a
Added SC servo examples + compatibility
danic85 Jul 13, 2025
3e4dfc0
Configure initial positions
danic85 Jul 14, 2025
0be65a3
Allow other poses to be stored and accessed
danic85 Jul 14, 2025
91fac95
Added change_id script
danic85 Aug 3, 2025
69f2d7c
Calibrate servos on boot
danic85 Oct 4, 2025
9a57f8a
adjustments
danic85 Oct 4, 2025
0cf906e
Dynamic servo calibration + center on change_id
danic85 Oct 5, 2025
8d62b54
Controller support and WIP recording
danic85 Dec 13, 2025
f02d0db
Working with Pi
danic85 Dec 13, 2025
b052ed0
Adjust config
danic85 Dec 13, 2025
6841f63
Cleanup
danic85 Dec 14, 2025
c1ed384
Working input recorder
danic85 Dec 14, 2025
5e503e5
Support servo control (WIP - calibration needed)
danic85 Dec 14, 2025
008c369
Callibration + allow modules to be disabled on test environments
danic85 Dec 15, 2025
af0fdb2
Disable test
danic85 Dec 15, 2025
40040c8
Add documentation and tests
danic85 Dec 16, 2025
f645caa
Fix test
danic85 Dec 16, 2025
8a22781
Timeout controller connection after 30 seconds
danic85 Dec 16, 2025
d81c07c
Enable on robot
danic85 Dec 16, 2025
6a84d43
Fix for environment input to resolve pi issue
danic85 Dec 17, 2025
4803d58
Calibrate new leg position
danic85 Dec 17, 2025
dc4cb27
Improvements to gamepad controller modules. Temperature alert in disp…
danic85 Dec 18, 2025
d48ccd8
Fix tests
danic85 Dec 18, 2025
4a5a0ee
Fixes and improvements to loop and temperature management
danic85 Dec 21, 2025
5349956
Fix tests
danic85 Dec 21, 2025
cd4331b
Resolve PR comments
danic85 Dec 21, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Dynamic servo calibration + center on change_id
  • Loading branch information
danic85 committed Oct 5, 2025
commit 0cf906e48f23a58a1b0e8729f67d24f90bbc0a8f
15 changes: 13 additions & 2 deletions docs/BusServo.md
Original file line number Diff line number Diff line change
Expand Up @@ -16,16 +16,27 @@ If you have challenges understanding the current ID of the servo, run `modules/a

Note: if there is an issue with dependencies, you may need to run the script from within the `modules/actuators/bus_servo` directory.

Once this has been done for all servos, you can use the `Servo` class to control them by enabling it in the above yaml file.
Once the ID has been set for all servos, you can use the `Servo` class to control them by enabling it in the above yaml file.

### Centering
The servos range is between 0-4095 for ST servos and 0-1024 for SC servos. The position readout can wrap around (from 0 to 4095), so the servos should be set to the midpoint before mounting them in the robot otherwise it can result in the servo passing the wrong way through the range of values, which could damage the robot.

The `change_id.py` script will set the servo to the midpoint when changing the ID. The servo should then be mounted in the robot at roughly the midpoint position.


### Calibration
To calibrate the servo positions, set the flag `calibrate_on_boot` to `true` in the configuration file for each instance (servo). This will cause the servo to output it's current position in the debug log, which can then be copied into the start position, or range. Servos can be manually moved to any position to identify their range or certain poses.

Note: calibrate_on_boot is enabled by default to help with initial configuration.
Note: `calibrate_on_boot` is enabled by default to help with initial configuration.

Once the start position and range are set, move the flag to the next servo and repeat the process until all servos are calibrated.

Finally, set `calibrate_on_boot` to false and re-run the program to start using the servos with their configured positions.

### Demonstration

To demonstrate the servo movement on boot, set the flag `demonstrate_on_boot` to `true` in the configuration file for each instance (servo). This will cause the servo to move to its minimum and maximum positions once on initialization, allowing you to see the range of motion.

## Subscriptions

The `Servo` class subscribes to the following topics:
Expand Down
16 changes: 16 additions & 0 deletions modules/actuators/bus_servo/change_id.py
Original file line number Diff line number Diff line change
Expand Up @@ -127,5 +127,21 @@ def getch():

print(f"{SERVO_TYPE} servo ID changed successfully to {new_id}")
SERVO_ID = new_id

print(f"Press any key to center servo ID {SERVO_ID} (required for further calibration)...")
getch()
# Center servo
if SERVO_TYPE == "STS":
center_result, center_error = packetHandler.WritePosEx(SERVO_ID, 2048, 1000, 50)
else:
center_result, center_error = packetHandler.WritePosEx(SERVO_ID, 512, 1000, 50)
if center_result != COMM_SUCCESS or center_error != 0:
print("Failed to center servo: %s %s" % (
packetHandler.getTxRxResult(center_result),
packetHandler.getRxPacketError(center_error)
))
else:
print(f"Centered servo ID {SERVO_ID}")

# Close port
portHandler.closePort()
54 changes: 50 additions & 4 deletions modules/actuators/bus_servo/servo.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,8 @@

import sys
import os

Copilot AI Dec 21, 2025

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Import of 'os' is not used.

Copilot uses AI. Check for mistakes.
import select
import time

if os.name == 'nt':
import msvcrt
Expand Down Expand Up @@ -50,6 +52,7 @@ def __init__(self, **kwargs):
self.baudrate = kwargs.get('baudrate', 1000000)
self.port = kwargs.get('port', '/dev/ttyACM0') # Change as needed, find with `ls /dev/ttyACM*`
self.calibrate_on_boot = kwargs.get('calibrate_on_boot', True) # 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.pos = self.start
self.speed = 2400 # 3073
self.acceleration = 50
Expand Down Expand Up @@ -97,10 +100,20 @@ def setup_messaging(self):
self.subscribe('system/exit', self.exit)

if self.calibrate_on_boot:
self.calibrate() # Log will show current position repeatedly to help with manual configuration
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.demonstrate_on_boot:
self.log(f"Demonstrating servo {self.identifier} movement")
if self.range is not None:
self.move(self.range[0])
time.sleep(1)
self.move(self.range[1])
time.sleep(1)
else:
self.log(f"Range not set for servo {self.identifier}, cannot demonstrate movement", level='warning')

# Move to start position
# if self.get_pose_value('stand') is not None:
# self.start = self.get_pose_value('stand')
Comment on lines +124 to +125

Copilot AI Dec 21, 2025

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This comment appears to contain commented-out code.

Copilot uses AI. Check for mistakes.
Expand Down Expand Up @@ -148,7 +161,7 @@ def get_position(self):
# Read STServo present position
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))
# self.log("[ID:%03d] PresPos:%d PresSpd:%d" % (self.index, sts_present_position, sts_present_speed))
return sts_present_position
else:
return self.sc_get_position_speed('position')
Expand Down Expand Up @@ -234,12 +247,45 @@ def calibrate(self):
max = self.get_position()
self.log(f"Captured maximum position: {max}")
self.range = (min, max)
if self.start < min or self.start > max:
if self.start is not None and (self.start < min or self.start > max):
self.start = (min + max) // 2
self.log(f"Start position {self.start} out of new range, setting to midpoint {self.start}")
self.log(f"Updated range for {self.identifier}: {self.range}. Start position: {self.start}")


def calibrate_dynamic(self):
"""
Continuously log the current position to help with manual calibration.
Store min an max as they are found.
Complete on key press. and store in self.range
"""
self.log(f"Calibrating servo {self.identifier}. Move the servo to find min and max positions. Press any key to finish...")
min_pos = None
max_pos = None
try:
while True:
pos = self.get_position()
if min_pos is None or pos < min_pos:
min_pos = pos
if max_pos is None or pos > max_pos:
max_pos = pos
# Print on the same line, pad with spaces to clear previous content
print(f"\rCurrent position: {pos}, Min: {min_pos}, Max: {max_pos} ", end='', flush=True)
time.sleep(0.05)
if sys.stdin in select.select([sys.stdin], [], [], 0)[0]:
sys.stdin.read(1) # Consume the key so buffer is cleared
break
except KeyboardInterrupt:
Comment thread
danic85 marked this conversation as resolved.
Resolved
pass
print() # Move to next line after loop
if min_pos is not None and max_pos is not None:
self.range = (min_pos, max_pos)
self.log(f"Calibration complete for {self.identifier}. Range: {self.range}")
else:
self.log(f"No positions recorded during calibration for {self.identifier}.", level='warning')

if self.start is not None and (self.start < min_pos or self.start > max_pos):
self.start = (min_pos + max_pos) // 2
self.log(f"Start position {self.start} out of new range, setting to midpoint {self.start}")



Expand Down