128 lines
4.1 KiB
Python
128 lines
4.1 KiB
Python
import math
|
|
import collections
|
|
from .pca9685 import ServoController, map_range
|
|
from .dc_motor_controller import DcMotor, setup_gpio
|
|
|
|
ServoMinMax = collections.namedtuple('ServoMinMax', ['minval', 'maxval', 'restval', 'maxstep'])
|
|
|
|
try:
|
|
from RPi import GPIO
|
|
except ImportError:
|
|
from .mockGpio import GPIO
|
|
|
|
PIN_MOTOR_A_ENABLE = 6
|
|
PIN_MOTOR_A_REVERSE = 12
|
|
|
|
PIN_MOTOR_B_ENABLE = 0
|
|
PIN_MOTOR_B_REVERSE = 5
|
|
|
|
SERVO_ARM_L = 15
|
|
SERVO_ARM_R = 14
|
|
SERVO_EYE_L = 13
|
|
SERVO_EYE_R = 12
|
|
SERVO_NECK_ROTATE = 11
|
|
SERVO_NECK_TOP = 10
|
|
SERVO_NECK_BOTTOM = 9
|
|
|
|
SERVO_MIN_MAX = {
|
|
SERVO_ARM_L: ServoMinMax(50, 130, 50, 5),
|
|
SERVO_ARM_R: ServoMinMax(50, 130, 130, 5),
|
|
SERVO_EYE_L: ServoMinMax(40, 100, 40, 5),
|
|
SERVO_EYE_R: ServoMinMax(80, 120, 120, 5),
|
|
SERVO_NECK_ROTATE: ServoMinMax(60, 120, 90, 5), # 60 - 90 - 120
|
|
SERVO_NECK_TOP: ServoMinMax(30, 180, 30, 5),
|
|
SERVO_NECK_BOTTOM: ServoMinMax(10, 180, 10, 5),
|
|
}
|
|
|
|
|
|
class WallE:
|
|
def __init__(self):
|
|
setup_gpio()
|
|
self.motor_a = DcMotor(PIN_MOTOR_A_ENABLE, PIN_MOTOR_A_REVERSE)
|
|
self.motor_b = DcMotor(PIN_MOTOR_B_ENABLE, PIN_MOTOR_B_REVERSE)
|
|
self.servo_controller = ServoController()
|
|
self.servo_controller.setup()
|
|
self.servo_positions = {}
|
|
self.servo_targets = {}
|
|
|
|
def setup(self):
|
|
for channel, min_max in SERVO_MIN_MAX.items():
|
|
self.servo_controller.write(channel, min_max.restval)
|
|
self.servo_positions[channel] = min_max.restval
|
|
|
|
def tick(self):
|
|
for channel, target in self.servo_targets.items():
|
|
current_value = self.servo_positions[channel]
|
|
if target == current_value:
|
|
del self.servo_targets[channel]
|
|
continue
|
|
try:
|
|
servo_min_max = SERVO_MIN_MAX[channel]
|
|
except IndexError:
|
|
continue
|
|
delta = abs(current_value - target)
|
|
step_size = min(delta, servo_min_max.maxstep)
|
|
new_val = current_value + step_size if target > current_value else current_value - step_size
|
|
self.servo_controller.write(channel, new_val)
|
|
self.servo_positions[channel] = new_val
|
|
|
|
def set_servo(self, channel, value):
|
|
try:
|
|
servo_min_max = SERVO_MIN_MAX[channel]
|
|
except IndexError:
|
|
return None
|
|
value = min(servo_min_max.maxval, max(servo_min_max.minval, value))
|
|
self.servo_targets[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
|
|
|
|
x = math.cos(angle) * force
|
|
y = math.sin(angle) * force
|
|
|
|
if y >= 0:
|
|
n_mot_premix_l = 1.0 if x >= 0 else 1.0 + x
|
|
n_mot_premix_r = 1.0 - x if x >= 0 else 1.0
|
|
else:
|
|
n_mot_premix_l = 1.0 - x if x >= 0 else 1.0
|
|
n_mot_premix_r = 1.0 if x >= 0 else 1.0 + x
|
|
|
|
n_mot_premix_l = n_mot_premix_l * y / 1.0
|
|
n_mot_premix_r = n_mot_premix_r * y / 1.0
|
|
n_piv_speed = x
|
|
f_piv_scale = 0.0 if abs(y) > .4 else 1.0 - abs(y) / .4
|
|
mot_mix_l = (1.0 - f_piv_scale) * n_mot_premix_l + f_piv_scale * n_piv_speed
|
|
mot_mix_r = (1.0 - f_piv_scale) * n_mot_premix_r + f_piv_scale * (-n_piv_speed)
|
|
self.motor_a.set(mot_mix_l)
|
|
self.motor_b.set(mot_mix_r)
|
|
|
|
def set_eye_velocity(self, angle: float, distance: float):
|
|
if distance > 1:
|
|
distance = 1
|
|
up_down = distance * math.sin(angle)
|
|
left_right = distance * math.cos(angle)
|
|
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))
|