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
Callibration + allow modules to be disabled on test environments
  • Loading branch information
danic85 committed Dec 15, 2025
commit 008c369061f9a647ae0a88dc4e224ead91a4f39a
65 changes: 43 additions & 22 deletions config/controller.yml
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,9 @@ controller:
enabled: true
path: 'modules.controller.Controller'
config:
debug: false
device: '/dev/input/js0' # Add this
deadzone: 500
deadzone: 4000 # thumbsticks deadzone (seems to be needed for xbox controllers)
button_names: # Add this
0: 'BTN_A'
1: 'BTN_B'
Expand All @@ -26,6 +27,8 @@ controller:
5: 'ABS_RY'
6: 'ABS_HAT0X'
7: 'ABS_HAT0Y'
axis_max: 32767.0
axis_min: -32767.0
modifier_buttons:
- BTN_TL
- BTN_TR
Expand All @@ -49,10 +52,10 @@ controller:
BTN_MODE: # Xbox Logo
- topic: ''
args: {}
ABS_HAT0X: # DPAD X (-1, 0 or 1)
ABS_HAT0X: # DPAD X
- topic: ''
args: {}
ABS_HAT0Y: # DPAD Y (-1, 0 or 1)
ABS_HAT0Y: # DPAD Y
- topic: ''
args: {}
BTN_A:
Expand All @@ -69,33 +72,51 @@ controller:
BTN_B:
- topic: ''
args: {}
ABS_RZ:
ABS_RX:
- topic: 'servo:neck_pan:mv'
args: {}
deadzone: 200
modifier:
scale: 0.001
ABS_RX:
scale: 0.1 # max speed per event
max_delta: 200 # max position change per event
ABS_RZ:
- topic: 'servo:neck_tilt:mv'
args: {}
modifier:
scale: -0.001
scale: -0.2
max_delta: 200
ABS_RY: # Right trigger
- topic: 'eye/blink'
args: {}
start: -32767
ABS_Z: # Left trigger
- topic: ''
args: {}
start: -32767
ABS_X:
- topic: 'eye/move'
args: {axis: 'y'}
modifier:
scale: -0.03
scale: -1.0
max_delta: 50
ABS_Y:
- topic: 'eye/move'
args: {axis: 'x'}
modifier:
scale: -0.03
ABS_Z: # Left trigger
- topic: ''
args: {}
ABS_RY: # Right trigger
- topic: ''
args: {}
scale: -1.0
max_delta: 50
BTN_TL:
BTN_X:
- topic: 'servo:neck_pan:mv'
args: {delta: 50}
BTN_Y:
- topic: 'servo:neck_tilt:mv'
args: {delta: 50}
BTN_B:
- topic: 'servo:neck_pan:mv'
args: {delta: -50}
BTN_A:
- topic: 'servo:neck_tilt:mv'
args: {delta: -50}
BTN_TR:
BTN_A:
- topic: 'tts'
Expand All @@ -105,19 +126,19 @@ controller:
ABS_X:
- topic: 'servo:leg_l_ankle:mv'
modifier:
scale: 0.01
scale: 0.1
- topic: 'servo:leg_r_ankle:mv'
modifier:
scale: -0.01
scale: -0.1
- topic: 'servo:leg_l_knee:mv'
modifier:
scale: 0.01
scale: 0.1
- topic: 'servo:leg_r_knee:mv'
modifier:
scale: -0.01
scale: -0.1
- topic: 'servo:leg_l_hip:mv'
modifier:
scale: 0.01
scale: 0.1
- topic: 'servo:leg_r_hip:mv'
modifier:
scale: -0.01
scale: -0.1
1 change: 1 addition & 0 deletions config/gpio_laser.yml
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
motion:
enabled: true
environment: robot
path: 'modules.gpio.laser.Laser'
config:
pin: 6
Expand Down
3 changes: 2 additions & 1 deletion config/gpio_motion.yml
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
gpio_motion:
enabled: false
enabled: true
environment: robot
path: 'modules.gpio.motion.Motion'
config:
pin: 9
Expand Down
2 changes: 1 addition & 1 deletion config/personality.yml
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
personality:
enabled: true
enabled: false
path: modules.personality.Personality
config:
mode: 6 # Config.MODE_LIVE
13 changes: 9 additions & 4 deletions config/servo_bus.yml
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
servos:
enabled: true
environment: robot
path: "modules.actuators.bus_servo.servo.Servo" # Include class name here
instances:
- name: "leg_r_tilt"
Expand Down Expand Up @@ -38,15 +39,19 @@ servos:
- stand: 3721
- sit: 3721
- centre: 2048
- name: "neck_pan"
- name: "neck_tilt"
model: 'SC09'
baudrate: 115200
id: 4
range: [96, 716]
range: [160, 638]
start: 511
- name: "neck_tilt"
- name: "neck_pan"
# calibrate_on_boot: true
# demonstrate_on_boot: true
model: 'SC09'
baudrate: 115200
id: 5
range: [171, 955]
range: [0, 1022]
start: 511
- name: "leg_l_tilt"
model: 'ST3215'
Expand Down
1 change: 1 addition & 0 deletions config/tft_display.yml
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
tft_display:
enabled: true
environment: robot
path: modules.display.tft_display_eye.TFTDisplayEye
config:
bus: 0 # Use `ls /dev/spi*` to identify. (/dev/spidev0.0) is bus 0, device 0
Expand Down
9 changes: 6 additions & 3 deletions main.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,13 @@ def main():
# Throw exception to safely exit script when terminated
signal.signal(signal.SIGTERM, Config.exit)

# Dynamically load and initialize modules
loader = ModuleLoader(config_folder="config")
# Get environment argument (default to 'robot')
env = sys.argv[1] if len(sys.argv) > 1 else 'robot'

# Dynamically load and initialize modules, passing env
loader = ModuleLoader(config_folder="config", environment=env)
module_instances = loader.load_modules()

# Set messaging service for all modules
messaging_service = module_instances['MessagingService'].messaging_service
loader.set_messaging_service(module_instances, messaging_service)
Expand Down
17 changes: 14 additions & 3 deletions module_loader.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
from pubsub import pub

class ModuleLoader:
def __init__(self, config_folder='config'):
def __init__(self, config_folder='config', environment=None):
"""
ModuleLoader class
:param config_folder: folder containing the module configuration files
Expand All @@ -27,6 +27,7 @@ def __init__(self, config_folder='config'):
translator_inst = modules['Translator']
"""
self.config_folder = config_folder
self.environment = environment or 'robot'
self.modules = self.load_yaml_files()

def load_yaml_files(self):
Expand All @@ -38,8 +39,18 @@ def load_yaml_files(self):
try:
config = yaml.safe_load(stream)
for module_name, module_config in config.items():
if module_config.get('enabled', False):
loaded_modules.append(module_config)
if not module_config.get('enabled', False):
continue
# If 'environment' is specified, filter by self.environment
env_field = module_config.get('environment')
if env_field is not None:
if isinstance(env_field, str):
if env_field != self.environment:
continue
elif isinstance(env_field, list):
if self.environment not in env_field:
continue
loaded_modules.append(module_config)
except yaml.YAMLError as e:
print(f"Error loading {file_path}: {e}")
return loaded_modules
Expand Down
41 changes: 35 additions & 6 deletions modules/actuators/bus_servo/servo.py
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,7 @@ def setup_messaging(self):
if self.demonstrate_on_boot:
self.log(f"Demonstrating servo {self.identifier} movement")
if self.range is not None:
time.sleep(1)
self.move(self.range[0])
time.sleep(1)
self.move(self.range[1])
Expand All @@ -125,6 +126,26 @@ def setup_messaging(self):
# 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.
self.move(self.start)

def _sc_write(self, type, value, verbose=False):
comm_result, error = self.packetHandler.write2ByteTxRx(self.portHandler, self.index, type, value)
self.log(f"[SCServo][{self.identifier}] Set speed: comm_result={comm_result}, error={error}")
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.packetHandler.getRxPacketError(error)}")
# Typical address for Present Load is 54 (1 byte), but check your servo's manual
ADDR_SCS_PRESENT_LOAD = 54
ADDR_SCS_PRESENT_POSITION = 56
ADDR_SCS_PRESENT_SPEED = 58
# Read present load
load, load_comm_result, load_error = self.packetHandler.read1ByteTxRx(self.portHandler, self.index, ADDR_SCS_PRESENT_LOAD)
# Read present position (2 bytes)
pos, pos_comm_result, pos_error = self.packetHandler.read2ByteTxRx(self.portHandler, self.index, ADDR_SCS_PRESENT_POSITION)
# Read present speed (2 bytes)
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 move(self, position):
"""
Move the servo to an absolute position.
Expand All @@ -142,10 +163,15 @@ def move(self, position):
self.log(f"Moved ST servo {self.identifier} from {self.pos} to position {position}")
self.pos = position # Update current position
elif self.model.startswith('SC'):
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, position)
self.log(f"Moved SC servo {self.identifier} from {self.pos} to position {position}")
if (
#self._sc_write(ADDR_TORQUE_ENABLE, 1) and
#self._sc_write(ADDR_SCS_GOAL_ACC, self.acceleration) and
#self._sc_write(ADDR_SCS_GOAL_SPEED, self.speed) and
Comment thread
danic85 marked this conversation as resolved.
Resolved
self._sc_write(ADDR_SCS_GOAL_POSITION, position)):
self.log(f"Moved SC servo {self.identifier} from {self.pos} to position {position} in range {self.range}")
self.pos = position # Update current position
else:
self.log(f"Failed to move SC servo {self.identifier} to position {position}", level='error')

def move_relative(self, delta):
"""
Expand All @@ -155,8 +181,8 @@ def move_relative(self, delta):
# self.log(f"Moving servo {self.identifier} from {self.pos} by delta {delta}")
new_position = round(self.pos + delta)
if new_position < self.range[0] or new_position > self.range[1]:
self.log(f"Position {new_position} out of range ({self.range[0]}-{self.range[1]})", level='error')
return
self.log(f"Position {new_position} out of range ({self.range[0]}-{self.range[1]}). Adjusting", level='warning')
new_position = self.range[0] if new_position < self.range[0] else self.range[1]

# Move to new position
self.move(new_position)
Expand Down Expand Up @@ -272,6 +298,9 @@ def calibrate_dynamic(self):
try:
while True:
pos = self.get_position()
if pos is None:
self.log(f"Failed to get position for servo {self.identifier}", level='warning')
continue
if min_pos is None or pos < min_pos:
min_pos = pos
if max_pos is None or pos > max_pos:
Expand Down
Loading
Loading