Skip to content

Commit 5e503e5

Browse files
committed
Support servo control (WIP - calibration needed)
1 parent c1ed384 commit 5e503e5

File tree

4 files changed

+28
-22
lines changed

4 files changed

+28
-22
lines changed

config/controller.yml

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -70,16 +70,16 @@ controller:
7070
- topic: ''
7171
args: {}
7272
ABS_RZ:
73-
- topic: 'servo:pan:mv'
73+
- topic: 'servo:neck_pan:mv'
7474
args: {}
7575
deadzone: 200
7676
modifier:
77-
scale: 0.01
77+
scale: 0.001
7878
ABS_RX:
79-
- topic: 'servo:tilt:mv'
79+
- topic: 'servo:neck_tilt:mv'
8080
args: {}
8181
modifier:
82-
scale: -0.01
82+
scale: -0.001
8383
ABS_X:
8484
- topic: 'eye/move'
8585
args: {axis: 'y'}
@@ -105,19 +105,19 @@ controller:
105105
ABS_X:
106106
- topic: 'servo:leg_l_ankle:mv'
107107
modifier:
108-
scale: 1.0
108+
scale: 0.01
109109
- topic: 'servo:leg_r_ankle:mv'
110110
modifier:
111-
scale: -1.0
111+
scale: -0.01
112112
- topic: 'servo:leg_l_knee:mv'
113113
modifier:
114-
scale: 1.0
114+
scale: 0.01
115115
- topic: 'servo:leg_r_knee:mv'
116116
modifier:
117-
scale: -1.0
117+
scale: -0.01
118118
- topic: 'servo:leg_l_hip:mv'
119119
modifier:
120-
scale: 1.0
120+
scale: 0.01
121121
- topic: 'servo:leg_r_hip:mv'
122122
modifier:
123-
scale: -1.0
123+
scale: -0.01

config/servo_bus.yml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
servos:
2-
enabled: false
2+
enabled: true
33
path: "modules.actuators.bus_servo.servo.Servo" # Include class name here
44
instances:
55
- name: "leg_r_tilt"

modules/actuators/bus_servo/servo.py

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -130,6 +130,7 @@ def move(self, position):
130130
Move the servo to an absolute position.
131131
:param position: Position to move to (0-100)
132132
"""
133+
# self.log(f"(MOVE) Moving servo {self.identifier} from {self.pos} to position {position} for range {self.range}")
133134
if position < self.range[0] or position > self.range[1]:
134135
self.log(f"Position {position} out of range ({self.range[0]}-{self.range[1]})", level='error')
135136
return
@@ -151,7 +152,8 @@ def move_relative(self, delta):
151152
Move the servo relative to its current position.
152153
:param delta: Change in position (can be negative)
153154
"""
154-
new_position = self.pos + delta
155+
# self.log(f"Moving servo {self.identifier} from {self.pos} by delta {delta}")
156+
new_position = round(self.pos + delta)
155157
if new_position < self.range[0] or new_position > self.range[1]:
156158
self.log(f"Position {new_position} out of range ({self.range[0]}-{self.range[1]})", level='error')
157159
return

modules/controller.py

Lines changed: 14 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -124,23 +124,22 @@ def _handle_axis_event(self, value, number, collect_topics_args=False):
124124
return topics_args
125125
return topic, args
126126

127+
# Calculate delta and debounce only once per event
128+
now = int(time.monotonic() * 1000)
129+
last_value = self.axis_last_value.get(axis, 0)
130+
delta = value - last_value
131+
last_time = self.axis_last_time.get(axis)
132+
127133
for mapping in button_map[axis]:
128134
deadzone = mapping.get('deadzone', self.global_deadzone)
129135
if deadzone > abs(value):
130136
continue
131137

132138
debounce = mapping.get('debounce', self.global_debounce)
133-
now = int(time.monotonic() * 1000)
134-
last_time = self.axis_last_time.get(axis)
135-
136139
if last_time is not None and (now - last_time) < debounce:
137140
continue
138141

139-
self.axis_last_time[axis] = now
140-
last_value = self.axis_last_value.get(axis, 0)
141-
delta = value - last_value
142-
self.axis_last_value[axis] = value
143-
142+
# Only update last_time and last_value if at least one mapping is published
144143
topic = mapping.get('topic')
145144
if not topic:
146145
self.log(f"Empty topic for axis {axis} mapping, skipping.", level='warning')
@@ -150,12 +149,17 @@ def _handle_axis_event(self, value, number, collect_topics_args=False):
150149
modifier = mapping.get('modifier')
151150
if modifier is not None:
152151
scale = modifier.get('scale', 1.0)
153-
args['delta'] = delta * scale
152+
args['delta'] = round(delta * scale)
154153

154+
self.log(f"Publishing to topic {topic} with args {args} (jsdev)")
155155
self.publish(topic, **args)
156-
self.log(f"Published to topic {topic} with args {args} (jsdev)")
157156
topics_args.append({'topic': topic, 'args': args})
158157

158+
# Only update axis_last_time and axis_last_value if any mapping was published
159+
if topics_args:
160+
self.axis_last_time[axis] = now
161+
self.axis_last_value[axis] = value
162+
159163
# If collect_topics_args is set, return the list, else preserve old behavior
160164
import inspect
161165
frame = inspect.currentframe().f_back

0 commit comments

Comments
 (0)