73 lines
2.2 KiB
Python
73 lines
2.2 KiB
Python
import math
|
|
from .pca9685 import ServoController, map_range
|
|
from .dc_motor_controller import DcMotor, setup_gpio
|
|
|
|
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
|
|
|
|
|
|
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()
|
|
|
|
def set_movement(self, angle: float, force: float):
|
|
if force > 1:
|
|
force = 1.0
|
|
|
|
cos_angle = math.cos(angle)
|
|
sin_angle = math.sin(angle)
|
|
|
|
velocity_l = 0
|
|
velocity_r = 0
|
|
|
|
if cos_angle > 0 and sin_angle > 0:
|
|
if cos_angle > sin_angle:
|
|
velocity_r = cos_angle * force
|
|
velocity_l = sin_angle * force
|
|
elif cos_angle < sin_angle:
|
|
velocity_r = sin_angle * force
|
|
velocity_l = (1 - cos_angle) * force
|
|
elif cos_angle < 0 and sin_angle > 0:
|
|
if abs(cos_angle) < sin_angle:
|
|
velocity_r = (1 - cos_angle) * force
|
|
velocity_l = sin_angle * force
|
|
elif abs(cos_angle) > sin_angle:
|
|
velocity_r = sin_angle * force
|
|
velocity_l = cos_angle * force
|
|
else:
|
|
pass
|
|
self.motor_a.set(velocity_l)
|
|
self.motor_b.set(velocity_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)
|
|
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))
|