feat: added functions to control servos

control/walle.py

@@ -1,7 +1,10 @@
 import math
+import collections
 from .pca9685 import ServoController, map_range
 from .dc_motor_controller import DcMotor, setup_gpio
 
+ServoMinMax = collections.namedtuple('ServoMinMax', ['minval', 'maxval'])
+
 try:
     from RPi import GPIO
 except ImportError:
@@ -21,6 +24,16 @@ SERVO_NECK_ROTATE = 11
 SERVO_NECK_TOP = 10
 SERVO_NECK_BOTTOM = 9
 
+SERVO_MIN_MAX = {
+    SERVO_ARM_L: ServoMinMax(50, 130),
+    SERVO_ARM_R: ServoMinMax(50, 130),
+    SERVO_EYE_L: ServoMinMax(40, 80),
+    SERVO_EYE_R: ServoMinMax(40, 80),
+    SERVO_NECK_ROTATE: ServoMinMax(60, 120),  # 60 - 90 - 120
+    SERVO_NECK_TOP: ServoMinMax(30, 180),
+    SERVO_NECK_BOTTOM: ServoMinMax(10, 180),
+}
+
 
 class WallE:
     def __init__(self):
@@ -30,6 +43,35 @@ class WallE:
         self.servo_controller = ServoController()
         self.servo_controller.setup()
 
+    def set_servo(self, channel, value):
+        try:
+            min_val, max_val = SERVO_MIN_MAX[channel]
+        except IndexError:
+            return None
+        value = min(max_val, max(min_val, value))
+        self.servo_controller.write(channel, value)
+
+    def set_arm_l(self, val):
+        self.set_servo(SERVO_ARM_L, val)
+
+    def set_arm_r(self, val):
+        self.set_servo(SERVO_ARM_R, val)
+
+    def set_eye_l(self, val):
+        self.set_servo(SERVO_EYE_L, val)
+
+    def set_eye_r(self, val):
+        self.set_servo(SERVO_EYE_R, val)
+
+    def set_neck_rotate(self, val):
+        self.set_servo(SERVO_NECK_ROTATE, val)
+
+    def set_neck_top(self, val):
+        self.set_servo(SERVO_NECK_TOP, val)
+
+    def set_neck_bottom(self, val):
+        self.set_servo(SERVO_NECK_BOTTOM, val)
+
     def set_movement(self, angle: float, force: float):
         if force > 1:
             force = 1.0
@@ -58,9 +100,5 @@ class WallE:
             distance = 1
         up_down = distance * math.sin(angle)
         left_right = distance * math.cos(angle)
-        min_arm_r = 60
+        self.set_servo(SERVO_ARM_L, map_range(up_down, -1.0, 1.0, 50, 130))
-        max_arm_r = 160  # 80
+        self.set_servo(SERVO_ARM_R, map_range(left_right, -1.0, 1.0, 130, 50))
-        min_arm_l = 20
-        max_arm_l = 100  # 80
-        self.servo_controller.write(SERVO_ARM_L, map_range(up_down, -1.0, 1.0, min_arm_l, max_arm_l))
-        self.servo_controller.write(SERVO_ARM_R, map_range(left_right, -1.0, 1.0, min_arm_r, max_arm_r))