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
Cleanup
  • Loading branch information
danic85 committed Dec 14, 2025
commit 6841f636a3b5db82248a56dfa9272e4c43639bda
23 changes: 23 additions & 0 deletions config/controller.yml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,30 @@ controller:
enabled: true
path: 'modules.controller.Controller'
config:
device: '/dev/input/js0' # Add this
deadzone: 500
button_names: # Add this
0: 'BTN_A'
1: 'BTN_B'
2: 'BTN_X'
3: 'BTN_Y'
4: 'BTN_TL'
5: 'BTN_TR'
6: 'BTN_SELECT'
7: 'BTN_START'
8: 'BTN_MODE'
9: 'BTN_THUMBL'
10: 'BTN_THUMBR'
11: 'KEY_RECORD' # Xbox Series controller
axis_names: # Add this
0: 'ABS_X'
1: 'ABS_Y'
2: 'ABS_Z'
3: 'ABS_RZ'
4: 'ABS_RX'
5: 'ABS_RY'
6: 'ABS_HAT0X'
7: 'ABS_HAT0Y'
modifier_buttons:
- BTN_TL
- BTN_TR
Expand Down
208 changes: 113 additions & 95 deletions modules/controller.py
Original file line number Diff line number Diff line change
@@ -1,137 +1,155 @@
import struct
import threading
import time

from modules.base_module import BaseModule
import yaml

import struct

class Controller(BaseModule):
"""Handles gamepad/joystick input and maps events to pub/sub topics."""

# Joystick event constants
JS_EVENT_BUTTON = 0x01
JS_EVENT_AXIS = 0x02
JS_EVENT_INIT = 0x80
JS_EVENT_FORMAT = "IhBB"
JS_EVENT_SIZE = struct.calcsize(JS_EVENT_FORMAT)

def __init__(self, **kwargs):
self.running = False
self.button_action_map = kwargs.get('button_action_map', {})
self.thread = threading.Thread(target=self.listen, daemon=True)
self.global_deadzone = kwargs.get('deadzone', 0.0)
self.global_debounce = kwargs.get('debounce', 800) # Minimum change threshold
self.modifier_buttons = set(kwargs.get('modifier_buttons', []))
self.modifier_buttons = set(kwargs.get('modifier_buttons', [])) # Allow alternate mappings when these buttons are held
self.pressed_buttons = set()
self.axis_last_value = {} # Track last value for each axis
self.axis_last_time = {} # Track last event time (ms) for each axis
self.device = kwargs.get('device', "/dev/input/js0")
self.device = kwargs.get('device', "/dev/input/js0") # Required to specify the gamepad device
if 'button_names' not in kwargs:
raise ValueError("button_names must be provided in configuration")
if 'axis_names' not in kwargs:
raise ValueError("axis_names must be provided in configuration")
self.button_names = kwargs['button_names']
self.axis_names = kwargs['axis_names']

def _get_active_mapping(self):
"""Get the current button map based on pressed modifier buttons."""
active_mods = sorted(self.pressed_buttons & self.modifier_buttons)
mapping_key = '+'.join(active_mods) if active_mods else 'default'
return self.button_action_map.get(mapping_key, self.button_action_map.get('default', {}))

def setup_messaging(self):
self.running = True
self.thread.start()

def listen(self):
"""Poll the joystick device for input events."""
self.log("Controller listening for input...")
JS_EVENT_FORMAT = "IhBB"
JS_EVENT_SIZE = struct.calcsize(JS_EVENT_FORMAT)
while self.running:
try:
with open(self.device, "rb") as jsdev:
while self.running:
evbuf = jsdev.read(JS_EVENT_SIZE)
evbuf = jsdev.read(self.JS_EVENT_SIZE)
if not evbuf:
continue
time_ms, value, type_, number = struct.unpack(JS_EVENT_FORMAT, evbuf)
# print(f"time={time} value={value} type={type_} number={number}")
self.handle_js_event(time_ms, value, type_, number)
time_ms, value, type_, number = struct.unpack(self.JS_EVENT_FORMAT, evbuf)
self._handle_js_event(time_ms, value, type_, number)
except FileNotFoundError:
self.log(f"No gamepad found at {self.device}. Waiting for connection...", level='warning')
time.sleep(1)
except Exception as e:
self.log(f"Error reading {self.device}: {e}", level='error')
self.log(f"Error: {e}", level='error')
time.sleep(1)

def handle_js_event(self, time_ms, value, type_, number):
# type_: 0x01 = button, 0x02 = axis, 0x80 = init
JS_EVENT_BUTTON = 1
JS_EVENT_AXIS = 0x02
JS_EVENT_INIT = 0x80

def _handle_js_event(self, time_ms, value, type_, number):
"""Route joystick events to appropriate handlers."""
is_init = (type_ & self.JS_EVENT_INIT) != 0
event_type = type_ & ~self.JS_EVENT_INIT

topic = None
args = {}

is_init = (type_ & JS_EVENT_INIT) != 0
event_type = type_ & ~JS_EVENT_INIT

# Map button/axis numbers to names as needed for your controller
button_names = {
0: 'BTN_A', 1: 'BTN_B', 2: 'BTN_X', 3: 'BTN_Y',
4: 'BTN_TL', 5: 'BTN_TR', 6: 'BTN_SELECT', 7: 'BTN_START',
8: 'BTN_MODE', 9: 'BTN_THUMBL', 10: 'BTN_THUMBR'
}
axis_names = {
0: 'ABS_X', 1: 'ABS_Y', 2: 'ABS_Z', 3: 'ABS_RZ',
4: 'ABS_RX', 5: 'ABS_RY', 6: 'ABS_HAT0X', 7: 'ABS_HAT0Y'
}

# self.log(f"JS Event: time={time_ms} value={value} type={event_type} number={number} (init={is_init})")

if event_type == JS_EVENT_BUTTON:
# self.log(f"JS Button Event: number={number} value={value} (init={is_init})")
button = button_names.get(number, f'BTN_{number}')
# value: 1=pressed, 0=released
if value == 1:
self.pressed_buttons.add(button)
elif value == 0 and button in self.pressed_buttons:
self.pressed_buttons.remove(button)

active_mods = sorted([b for b in self.pressed_buttons if b in self.modifier_buttons])
mapping_key = '+'.join(active_mods) if active_mods else 'default'
button_map = self.button_action_map.get(mapping_key, self.button_action_map.get('default', {}))
if button in button_map and value == 1:
actions = button_map[button]
for mapping in actions:
topic = mapping.get('topic')
if not topic:
self.log(f"Empty topic for button {button} mapping, skipping.", level='warning')
continue # Skip empty topic
args = mapping.get('args', {})
self.publish(topic, **args)
self.log(f"Published to topic {topic} with args {args} (jsdev)")

elif event_type == JS_EVENT_AXIS:
axis = axis_names.get(number, f'AXIS_{number}')
# self.log(f"Axis event: {axis} value={value} (jsdev)")
# value: -32767..32767 for sticks, -1/0/1 for hats
active_mods = sorted([b for b in self.pressed_buttons if b in self.modifier_buttons])
mapping_key = '+'.join(active_mods) if active_mods else 'default'
button_map = self.button_action_map.get(mapping_key, self.button_action_map.get('default', {}))

if axis in button_map:
actions = button_map[axis]
for mapping in actions:
deadzone = mapping.get('deadzone', self.global_deadzone)
if deadzone > abs(value):
continue # Skip if within deadzone

debounce = mapping.get('debounce', self.global_debounce) # debounce in ms
now = int(time.time() * 1000) # current time in ms
last_time = self.axis_last_time.get(axis, None)

# Only allow if enough time has passed
if last_time is not None and (now - last_time) < debounce:
continue # Skip if within debounce interval
self.axis_last_time[axis] = now
last_value = self.axis_last_value.get(axis, 0)
delta = value - last_value
self.axis_last_value[axis] = value

topic = mapping.get('topic')
if not topic:
self.log(f"Empty topic for axis {axis} mapping, skipping.", level='warning')
continue # Skip empty topic
args = dict(mapping.get('args', {})) # Copy to avoid mutation
modifier = mapping.get('modifier', None)
if modifier is not None:
scale = modifier.get('scale', 1.0)
args['delta'] = delta * scale
# self.log(f"Axis {axis} moved to {args.get('delta', value)} (jsdev)")
self.publish(topic, **args)
self.log(f"Published to topic {topic} with args {args} (jsdev)")
if event_type == self.JS_EVENT_BUTTON:
topic, args = self._handle_button_event(value, number)
elif event_type == self.JS_EVENT_AXIS:
topic, args = self._handle_axis_event(value, number)

self.publish('controller/event', event=(time_ms, value, type_, number, topic, args))

def _handle_button_event(self, value, number):
"""Handle button press/release events."""
button = self.button_names.get(number, f'BTN_{number}')
topic = None
args = {}

if value == 1:
self.pressed_buttons.add(button)
elif value == 0:
self.pressed_buttons.discard(button)

button_map = self._get_active_mapping()
if button in button_map and value == 1:
for mapping in button_map[button]:
topic = mapping.get('topic')
if not topic:
self.log(f"Empty topic for button {button} mapping, skipping.", level='warning')
continue
args = mapping.get('args', {})
self.publish(topic, **args)
self.log(f"Published to topic {topic} with args {args} (jsdev)")

return topic, args

def _handle_axis_event(self, value, number):
"""Handle axis movement events."""
axis = self.axis_names.get(number, f'AXIS_{number}')
topic = None
args = {}

button_map = self._get_active_mapping()
if axis not in button_map:
return topic, args

for mapping in button_map[axis]:
deadzone = mapping.get('deadzone', self.global_deadzone)
if deadzone > abs(value):
continue

debounce = mapping.get('debounce', self.global_debounce)
now = int(time.monotonic() * 1000)
last_time = self.axis_last_time.get(axis)

if last_time is not None and (now - last_time) < debounce:
continue

self.axis_last_time[axis] = now
last_value = self.axis_last_value.get(axis, 0)
delta = value - last_value
self.axis_last_value[axis] = value

topic = mapping.get('topic')
if not topic:
self.log(f"Empty topic for axis {axis} mapping, skipping.", level='warning')
continue

args = dict(mapping.get('args', {}))
modifier = mapping.get('modifier')
if modifier is not None:
scale = modifier.get('scale', 1.0)
args['delta'] = delta * scale

self.publish(topic, **args)
self.log(f"Published to topic {topic} with args {args} (jsdev)")

return topic, args

# Keep public method for backwards compatibility with tests
def handle_js_event(self, time_ms, value, type_, number):
"""Public wrapper for _handle_js_event (for testing compatibility)."""
return self._handle_js_event(time_ms, value, type_, number)

def stop(self):
"""Stop the controller listener thread."""
self.running = False
self.thread.join()
36 changes: 36 additions & 0 deletions tests/test_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,15 @@ def setUp(self):

def test_dynamic_remapping(self):
config = {
'button_names': {
0: 'BTN_A', 1: 'BTN_B', 2: 'BTN_X', 3: 'BTN_Y',
4: 'BTN_TL', 5: 'BTN_TR', 6: 'BTN_SELECT', 7: 'BTN_START',
8: 'BTN_MODE', 9: 'BTN_THUMBL', 10: 'BTN_THUMBR'
},
'axis_names': {
0: 'ABS_X', 1: 'ABS_Y', 2: 'ABS_Z', 3: 'ABS_RZ',
4: 'ABS_RX', 5: 'ABS_RY', 6: 'ABS_HAT0X', 7: 'ABS_HAT0Y'
},
'button_action_map': {
'default': {
'BTN_A': [ {'topic': 'tts', 'args': {'msg': 'Default'}} ]
Expand All @@ -42,6 +51,15 @@ def test_dynamic_remapping(self):

def test_modifier_mapping(self):
config = {
'button_names': {
0: 'BTN_A', 1: 'BTN_B', 2: 'BTN_X', 3: 'BTN_Y',
4: 'BTN_TL', 5: 'BTN_TR', 6: 'BTN_SELECT', 7: 'BTN_START',
8: 'BTN_MODE', 9: 'BTN_THUMBL', 10: 'BTN_THUMBR'
},
'axis_names': {
0: 'ABS_X', 1: 'ABS_Y', 2: 'ABS_Z', 3: 'ABS_RZ',
4: 'ABS_RX', 5: 'ABS_RY', 6: 'ABS_HAT0X', 7: 'ABS_HAT0Y'
},
'button_action_map': {
'default': {
'BTN_A': [ {'topic': 'tts', 'args': {'msg': 'Default'}} ]
Expand Down Expand Up @@ -70,6 +88,15 @@ def test_controller_event_published(self):

def test_delta_handled_correctly(self):
config = {
'button_names': {
0: 'BTN_A', 1: 'BTN_B', 2: 'BTN_X', 3: 'BTN_Y',
4: 'BTN_TL', 5: 'BTN_TR', 6: 'BTN_SELECT', 7: 'BTN_START',
8: 'BTN_MODE', 9: 'BTN_THUMBL', 10: 'BTN_THUMBR'
},
'axis_names': {
0: 'ABS_X', 1: 'ABS_Y', 2: 'ABS_Z', 3: 'ABS_RZ',
4: 'ABS_RX', 5: 'ABS_RY', 6: 'ABS_HAT0X', 7: 'ABS_HAT0Y'
},
'button_action_map': {
'default': {
'ABS_X': [ {'topic': 'eye/move', 'args': {'axis': 'x'}, 'deadzone': 10, 'modifier': {'scale': 1.0}} ],
Expand All @@ -87,6 +114,15 @@ def test_delta_handled_correctly(self):

def test_deadzone_handled(self):
config = {
'button_names': {
0: 'BTN_A', 1: 'BTN_B', 2: 'BTN_X', 3: 'BTN_Y',
4: 'BTN_TL', 5: 'BTN_TR', 6: 'BTN_SELECT', 7: 'BTN_START',
8: 'BTN_MODE', 9: 'BTN_THUMBL', 10: 'BTN_THUMBR'
},
'axis_names': {
0: 'ABS_X', 1: 'ABS_Y', 2: 'ABS_Z', 3: 'ABS_RZ',
4: 'ABS_RX', 5: 'ABS_RY', 6: 'ABS_HAT0X', 7: 'ABS_HAT0Y'
},
'button_action_map': {
'default': {
'ABS_X': [ {'topic': 'eye/move', 'args': {'axis': 'x'}, 'deadzone': 10, 'modifier': {'scale': 1.0}} ],
Expand Down