diff --git a/control/walle.py b/control/walle.py index aa86dc9..e0c6b72 100644 --- a/control/walle.py +++ b/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 - max_arm_r = 160 # 80 - 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)) + self.set_servo(SERVO_ARM_L, map_range(up_down, -1.0, 1.0, 50, 130)) + self.set_servo(SERVO_ARM_R, map_range(left_right, -1.0, 1.0, 130, 50))