Support servo control (WIP - calibration needed)

Author: danic85

config/controller.yml

@@ -70,16 +70,16 @@ controller:
           - topic: ''
             args: {}
         ABS_RZ:
-          - topic: 'servo:pan:mv'
+          - topic: 'servo:neck_pan:mv'
             args: {}
             deadzone: 200
             modifier:
-              scale: 0.01
+              scale: 0.001
         ABS_RX:
-          - topic: 'servo:tilt:mv'
+          - topic: 'servo:neck_tilt:mv'
             args: {}
             modifier:
-              scale: -0.01
+              scale: -0.001
         ABS_X:
           - topic: 'eye/move'
             args: {axis: 'y'}
@@ -105,19 +105,19 @@ controller:
         ABS_X:
           - topic: 'servo:leg_l_ankle:mv'
             modifier:
-              scale: 1.0
+              scale: 0.01
           - topic: 'servo:leg_r_ankle:mv'
             modifier:
-              scale: -1.0
+              scale: -0.01
           - topic: 'servo:leg_l_knee:mv'
             modifier:
-              scale: 1.0
+              scale: 0.01
           - topic: 'servo:leg_r_knee:mv'
             modifier:
-              scale: -1.0
+              scale: -0.01
           - topic: 'servo:leg_l_hip:mv'
             modifier:
-              scale: 1.0
+              scale: 0.01
           - topic: 'servo:leg_r_hip:mv'
             modifier:
-              scale: -1.0
+              scale: -0.01

config/servo_bus.yml

@@ -1,5 +1,5 @@
 servos:
-  enabled: false
+  enabled: true
   path: "modules.actuators.bus_servo.servo.Servo" # Include class name here
   instances:
     - name: "leg_r_tilt"

modules/actuators/bus_servo/servo.py

@@ -130,6 +130,7 @@ def move(self, position):
         Move the servo to an absolute position.
         :param position: Position to move to (0-100)
         """
+        # self.log(f"(MOVE) Moving servo {self.identifier} from {self.pos} to position {position} for range {self.range}")
         if position < self.range[0] or position > self.range[1]:
             self.log(f"Position {position} out of range ({self.range[0]}-{self.range[1]})", level='error')
             return
@@ -151,7 +152,8 @@ def move_relative(self, delta):
         Move the servo relative to its current position.
         :param delta: Change in position (can be negative)
         """
-        new_position = self.pos + 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

modules/controller.py

@@ -124,23 +124,22 @@ def _handle_axis_event(self, value, number, collect_topics_args=False):
                 return topics_args
             return topic, args
 
+        # Calculate delta and debounce only once per event
+        now = int(time.monotonic() * 1000)
+        last_value = self.axis_last_value.get(axis, 0)
+        delta = value - last_value
+        last_time = self.axis_last_time.get(axis)
+
         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
-
+            # Only update last_time and last_value if at least one mapping is published
             topic = mapping.get('topic')
             if not topic:
                 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):
             modifier = mapping.get('modifier')
             if modifier is not None:
                 scale = modifier.get('scale', 1.0)
-                args['delta'] = delta * scale
+                args['delta'] = round(delta * scale)
 
+            self.log(f"Publishing to topic {topic} with args {args} (jsdev)")
             self.publish(topic, **args)
-            self.log(f"Published to topic {topic} with args {args} (jsdev)")
             topics_args.append({'topic': topic, 'args': args})
 
+        # Only update axis_last_time and axis_last_value if any mapping was published
+        if topics_args:
+            self.axis_last_time[axis] = now
+            self.axis_last_value[axis] = value
+
         # If collect_topics_args is set, return the list, else preserve old behavior
         import inspect
         frame = inspect.currentframe().f_back