feat: initial commit

control/camera.py

@@ -0,0 +1,34 @@
+from io import BytesIO
+import cv2
+from PIL import Image
+
+
+class Camera:
+    capture: cv2.VideoCapture
+
+    def __init__(self):
+        self.capture = cv2.VideoCapture(0)
+
+    def generate_images(self):
+        try:
+            while True:
+                return_code, image = self.capture.read()
+                if not return_code:
+                    continue
+                image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+                jpg = Image.fromarray(image_rgb)
+                byte_stream = BytesIO()
+                jpg.save(byte_stream, 'JPEG')
+                yield bytes(byte_stream.getbuffer())
+        finally:
+            self.capture.release()
+
+    def mjpeg_stream(self, boundary: bytes):
+        for frame in self.generate_images():
+            if not frame:
+                break
+            yield b''.join([
+                b'--', boundary, b'\r\n',
+                b'Content-Type: image/jpeg\r\n\r\n',
+                frame,
+                b'\r\n'])

control/walle.py

@@ -0,0 +1,110 @@
+import math
+from typing import Optional
+import atexit
+
+if True:  # NOTE: false on pi
+    from RPiSim.GPIO import GPIO
+else:
+    from RPi import GPIO
+
+
+PIN_MOTOR_A_ENABLE = 6
+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)
+
+
+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()
+
+    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 y > 0:
+            self.motor_a.set(velocity_l)
+            self.motor_b.set(velocity_r)
+        else:
+            self.motor_a.set(-velocity_l)
+            self.motor_b.set(-velocity_r)
+
+    def set_eye_velocity(self, up_down: float, left_right: float):
+        pass