feat: added some features

control/walle.py

@@ -1,12 +1,11 @@
 import math
-from typing import Optional
-import atexit
+from .pca9685 import ServoController, map_range
+from .dc_motor_controller import DcMotor, setup_gpio
 
-if True:  # NOTE: false on pi
-    from RPiSim.GPIO import GPIO
-else:
+try:
     from RPi import GPIO
-
+except ImportError:
+    from .mockGpio import GPIO
 
 PIN_MOTOR_A_ENABLE = 6
 PIN_MOTOR_A_REVERSE = 12
@@ -14,97 +13,55 @@ PIN_MOTOR_A_REVERSE = 12
 PIN_MOTOR_B_ENABLE = 0
 PIN_MOTOR_B_REVERSE = 5
 
-PWM_FREQUENCY = 10_000  # 10 KHz
-
-if not hasattr(GPIO, 'PWM'):
-    class PWM:
-        def __init__(self, pin, freq):
-            self.pin = pin
-            self.freq = freq
-            self.dc: Optional[float] = None
-            self.name = None
-
-        def start(self, dc: float):
-            self.dc = dc
-
-        def ChangeDutyCycle(self, dc):
-            self.dc = dc
-            print(f'[{self.pin}@{self.freq}Hz]{" " + self.name if self.name else ""} => {self.dc}')
-
-        def stop(self):
-            print("stop pwm")
-
-    GPIO.PWM = PWM
-
-
-class Motor:
-    def __init__(self, pin_enable, pin_reverse):
-        self.pin_enable = pin_enable
-        self.pin_reverse = pin_reverse
-
-        GPIO.setup(pin_enable, GPIO.OUT)
-        GPIO.setup(pin_reverse, GPIO.OUT)
-        GPIO.output(pin_reverse, GPIO.LOW)
-
-        self.pwm_enable = GPIO.PWM(pin_enable, PWM_FREQUENCY)
-        self.pwm_enable.start(0.0)
-        self.is_reverse = False
-
-    def set(self, val):
-        """
-        Set the speed of the motor
-
-        Args:
-            val: Value between 1.0 and -1.0
-        """
-        self._set_reverse(val)
-        absval = abs(val)
-        if absval > 1:
-            print(f"clipping {val} to 1")
-            absval = 1
-
-        self.pwm_enable.ChangeDutyCycle(absval)
-
-    def _set_reverse(self, val):
-        if val >= 0 and self.is_reverse:
-            GPIO.output(self.pin_reverse, GPIO.LOW)
-            self.is_reverse = False
-        elif val < 0 and not self.is_reverse:
-            GPIO.output(self.pin_reverse, GPIO.HIGH)
-            self.is_reverse = True
-
-    def stop(self):
-        self.pwm_enable.stop()
-        GPIO.output(self.pin_reverse, GPIO.LOW)
+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):
-        GPIO.setmode(GPIO.BCM)
-        self.motor_a = Motor(PIN_MOTOR_A_ENABLE, PIN_MOTOR_A_REVERSE)
-        self.motor_b = Motor(PIN_MOTOR_B_ENABLE, PIN_MOTOR_B_REVERSE)
-
-        atexit.register(self.cleanup)
-
-    def cleanup(self):
-        print("cleaning up")
-        self.motor_a.stop()
-        self.motor_b.stop()
-        GPIO.cleanup()
+        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, distance: float):
-        angle_rad = math.radians(angle)
-        x = math.cos(angle_rad)
-        y = math.sin(angle_rad)
-        velocity_l = distance * (abs(x - 1.0) / 2.0)
-        velocity_r = distance * ((x + 1.0) / 2.0)
+        if distance > 0:
+            distance = distance/50.0  # map 0 - 50 to 0 - 1
 
-        if y > 0:
-            self.motor_a.set(velocity_l)
-            self.motor_b.set(velocity_r)
+        angle_rad = math.radians(angle)
+        cos_angle = math.cos(angle_rad)
+        sin_angle = math.sin(angle_rad)
+
+        velocity_l = 0
+        velocity_r = 0
+
+        if cos_angle > 0 and sin_angle > 0:
+            if cos_angle > sin_angle:
+                velocity_r = cos_angle * distance
+                velocity_l = sin_angle * distance
+            elif cos_angle < sin_angle:
+                velocity_r = sin_angle * distance
+                velocity_l = cos_angle * distance
+        elif cos_angle < 0 and sin_angle > 0:
+            if cos_angle < sin_angle:
+                velocity_r = 1 - sin_angle * distance
+                velocity_l = sin_angle * distance
+            elif cos_angle > abs(sin_angle):
+                velocity_r = sin_angle * distance
+                velocity_l = cos_angle * distance
         else:
-            self.motor_a.set(-velocity_l)
-            self.motor_b.set(-velocity_r)
+            pass
+        self.motor_a.set(velocity_l)
+        self.motor_b.set(velocity_r)
 
     def set_eye_velocity(self, up_down: float, left_right: float):
-        pass
+        minval = 50
+        maxval = 130
+        self.servo_controller.write(SERVO_ARM_L, map_range(up_down, 0.0, 1.0, minval, maxval))
+        self.servo_controller.write(SERVO_ARM_R, map_range(left_right, 0.0, 1.0, minval, maxval))