feat: added some features

app.py

@@ -1,8 +1,15 @@
-from control.camera import Camera
-from control.walle import WallE
+import base64
+
+# import eventlet
+# eventlet.monkey_patch()
+
 from flask import Flask, jsonify, send_file, Response
 from flask_socketio import SocketIO
 
+from control.camera import Camera
+from control.walle import WallE
+
+DATA_URL_PREFIX = 'data:image/jpeg;base64,'
 # DEBUG = True
 
 boundary = 'lkajflkasdjlkfaj'
@@ -11,9 +18,21 @@ app = Flask(__name__, static_url_path='', static_folder='client/dist')
 app.config.from_object(__name__)
 sio = SocketIO(app)
 
+# camera = Camera()
 walle = WallE()
 
 
+def camera_thread():
+    while True:
+        for image in camera.generate_images():
+            if not image:
+                break
+            image_url = DATA_URL_PREFIX + base64.b64encode(image).decode('ascii')
+            sio.emit('camera_image', {
+                'image': image_url,
+            }, broadcast=True)
+
+
 @app.route('/')
 def index():
     return send_file('client/dist/index.html')
@@ -34,4 +53,5 @@ def message(directions):
 
 
 if __name__ == '__main__':
+    # eventlet.spawn(camera_thread)
     sio.run(app, host='0.0.0.0')

client/src/main.js

@@ -4,7 +4,7 @@ import App from './App.vue';
 
 Vue.use(new VueSocketIO({
   debug: true,
-  connection: 'http://192.168.0.100:5000',
+  connection: `${window.location.protocol}//${window.location.host}`,
   options: {
     transport: 'websocket',
   },

control/dc_motor_controller.py

@@ -0,0 +1,71 @@
+import atexit
+
+try:
+    from RPi import GPIO
+except ImportError:
+    from .mockGpio import GPIO
+
+
+PWM_FREQUENCY = 10_000  # 10 KHz
+MOTORS = []
+
+
+def setup_gpio():
+    GPIO.setmode(GPIO.BCM)
+
+    atexit.register(cleanup_gpio)
+
+
+def cleanup_gpio():
+    print("cleaning up")
+    for motor in MOTORS:
+        # noinspection PyProtectedMember
+        motor._stop()
+    GPIO.cleanup()
+
+
+class DcMotor:
+    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
+
+        MOTORS.append(self)
+
+    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 * 100)
+
+    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)
+
+    def stop(self):
+        self._stop()
+        MOTORS.remove(self)

control/mockGpio.py

@@ -0,0 +1,42 @@
+from typing import Optional
+
+
+class GPIO:
+    BCM = 1
+    IN = 1
+    OUT = 2
+    LOW = 0
+    HIGH = 1
+
+    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")
+
+    @staticmethod
+    def setup(pin, mode):
+        print(f"setup {pin} -> {mode}")
+
+    @staticmethod
+    def output(pin, value):
+        print(f"output {pin} -> {value}")
+
+    @staticmethod
+    def setmode(mode):
+        print(f"setmode {mode}")
+
+    @staticmethod
+    def cleanup():
+        print("cleanup")

control/pca9685/__init__.py

@@ -0,0 +1,4 @@
+from .pca9685 import PWM, map_range
+from .servocontroller import ServoController
+
+__all__ = ['PWM', 'ServoController', 'map_range']

control/pca9685/pca9685.py

@@ -0,0 +1,151 @@
+#!/usr/bin/python
+'''
+**********************************************************************
+* Filename    : PCA9685.py
+* Description : A driver module for PCA9685
+* Author      : Cavon
+* Brand       : SunFounder
+* E-mail      : service@sunfounder.com
+* Website     : www.sunfounder.com
+* Version     : v2.0.0
+**********************************************************************
+'''
+
+import smbus
+import time
+import math
+
+
+def map_range(x, in_min, in_max, out_min, out_max):
+    """To map the value from arange to another"""
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
+
+
+class PWM:
+    """A PWM control class for PCA9685."""
+    _MODE1 = 0x00
+    _MODE2 = 0x01
+    _SUBADR1 = 0x02
+    _SUBADR2 = 0x03
+    _SUBADR3 = 0x04
+    _PRESCALE = 0xFE
+    _LED0_ON_L = 0x06
+    _LED0_ON_H = 0x07
+    _LED0_OFF_L = 0x08
+    _LED0_OFF_H = 0x09
+    _ALL_LED_ON_L = 0xFA
+    _ALL_LED_ON_H = 0xFB
+    _ALL_LED_OFF_L = 0xFC
+    _ALL_LED_OFF_H = 0xFD
+
+    _RESTART = 0x80
+    _SLEEP = 0x10
+    _ALLCALL = 0x01
+    _INVRT = 0x10
+    _OUTDRV = 0x04
+
+    _DEBUG = False
+    _DEBUG_INFO = 'DEBUG "PCA9685.py":'
+
+    def __init__(self, bus_number=None, address=0x40):
+        self.address = address
+        if bus_number is None:
+            self.bus_number = 1
+        else:
+            self.bus_number = bus_number
+        self.bus = smbus.SMBus(self.bus_number)
+        self._frequency = 60
+
+    def _debug_(self, message):
+        if self._DEBUG:
+            print(self._DEBUG_INFO, message)
+
+    def setup(self):
+        '''Init the class with bus_number and address'''
+        self._debug_('Reseting PCA9685 MODE1 (without SLEEP) and MODE2')
+        self.write_all_value(0, 0)
+        self._write_byte_data(self._MODE2, self._OUTDRV)
+        self._write_byte_data(self._MODE1, self._ALLCALL)
+        time.sleep(0.005)
+
+        mode1 = self._read_byte_data(self._MODE1)
+        mode1 = mode1 & ~self._SLEEP
+        self._write_byte_data(self._MODE1, mode1)
+        time.sleep(0.005)
+        self._frequency = 60
+
+    def _write_byte_data(self, reg, value):
+        '''Write data to I2C with self.address'''
+        self._debug_('Writing value %2X to %2X' % (value, reg))
+        try:
+            self.bus.write_byte_data(self.address, reg, value)
+        except Exception as i:
+            print(i)
+
+    def _read_byte_data(self, reg):
+        '''Read data from I2C with self.address'''
+        self._debug_('Reading value from %2X' % reg)
+        try:
+            results = self.bus.read_byte_data(self.address, reg)
+            return results
+        except Exception as i:
+            print(i)
+
+    @property
+    def frequency(self):
+        return self._frequency
+
+    @frequency.setter
+    def frequency(self, freq):
+        """Set PWM frequency"""
+        self._debug_('Set frequency to %d' % freq)
+        self._frequency = freq
+        prescale_value = 25000000.0
+        prescale_value /= 4096.0
+        prescale_value /= float(freq)
+        prescale_value -= 1.0
+        self._debug_('Setting PWM frequency to %d Hz' % freq)
+        self._debug_('Estimated pre-scale: %d' % prescale_value)
+        prescale = math.floor(prescale_value + 0.5)
+        self._debug_('Final pre-scale: %d' % prescale)
+
+        old_mode = self._read_byte_data(self._MODE1);
+        new_mode = (old_mode & 0x7F) | 0x10
+        self._write_byte_data(self._MODE1, new_mode)
+        self._write_byte_data(self._PRESCALE, int(math.floor(prescale)))
+        self._write_byte_data(self._MODE1, old_mode)
+        time.sleep(0.005)
+        self._write_byte_data(self._MODE1, old_mode | 0x80)
+
+    def write(self, channel, on, off):
+        """Set on and off value on specific channel"""
+        self._debug_('Set channel "%d" to value "%d"' % (channel, off))
+        self._write_byte_data(self._LED0_ON_L + 4 * channel, on & 0xFF)
+        self._write_byte_data(self._LED0_ON_H + 4 * channel, on >> 8)
+        self._write_byte_data(self._LED0_OFF_L + 4 * channel, off & 0xFF)
+        self._write_byte_data(self._LED0_OFF_H + 4 * channel, off >> 8)
+
+    def write_all_value(self, on, off):
+        """Set on and off value on all channel"""
+        self._debug_('Set all channel to value "%d"' % (off))
+        self._write_byte_data(self._ALL_LED_ON_L, on & 0xFF)
+        self._write_byte_data(self._ALL_LED_ON_H, on >> 8)
+        self._write_byte_data(self._ALL_LED_OFF_L, off & 0xFF)
+        self._write_byte_data(self._ALL_LED_OFF_H, off >> 8)
+
+    @property
+    def debug(self):
+        return self._DEBUG
+
+    @debug.setter
+    def debug(self, debug):
+        """Set if debug information shows"""
+        if debug in (True, False):
+            self._DEBUG = debug
+        else:
+            raise ValueError('debug must be "True" (Set debug on) or "False" (Set debug off), not "{0}"'.format(debug))
+
+        if self._DEBUG:
+            print(self._DEBUG_INFO, "Set debug on")
+        else:
+            print(self._DEBUG_INFO, "Set debug off")

control/pca9685/servocontroller.py

@@ -0,0 +1,87 @@
+"""
+**********************************************************************
+* Filename    : Servo.py
+* Description : Driver module for servo, with PCA9685
+* Author      : Cavon
+* Brand       : SunFounder
+* E-mail      : service@sunfounder.com
+* Website     : www.sunfounder.com
+* Update      : Cavon    2016-09-13    New release
+*               Cavon    2016-09-21    Change channel from 1 to all
+**********************************************************************
+"""
+
+from .pca9685 import PWM, map_range
+
+
+class ServoController:
+    """Servo driver class"""
+    _MIN_PULSE_WIDTH = 600
+    _MAX_PULSE_WIDTH = 2400
+    _DEFAULT_PULSE_WIDTH = 1500
+    _FREQUENCY = 60
+
+    _DEBUG = False
+    _DEBUG_INFO = 'DEBUG "Servo.py":'
+
+    def __init__(self, lock=True, bus_number=None, address=0x40):
+        """ Init a servo on specific channel, this offset """
+        self._debug_("Debug on")
+        self.lock = lock
+
+        self.pwm = PWM(bus_number=bus_number, address=address)
+        self.frequency = self._FREQUENCY
+
+    def _debug_(self, message):
+        if self._DEBUG:
+            print(self._DEBUG_INFO, message)
+
+    def setup(self):
+        self.pwm.setup()
+
+    def _angle_to_analog(self, angle):
+        ''' Calculate 12-bit analog value from giving angle '''
+        pulse_wide = map_range(angle, 0, 180, self._MIN_PULSE_WIDTH, self._MAX_PULSE_WIDTH)
+        analog_value = int(float(pulse_wide) / 1000000 * self.frequency * 4096)
+        self._debug_('Angle %d equals Analog_value %d' % (angle, analog_value))
+        return analog_value
+
+    @property
+    def frequency(self):
+        return self._frequency
+
+    @frequency.setter
+    def frequency(self, value):
+        self._frequency = value
+        self.pwm.frequency = value
+
+    def write(self, channel, angle):
+        ''' Turn the servo with giving angle. '''
+        if self.lock:
+            if angle > 180:
+                angle = 180
+            if angle < 0:
+                angle = 0
+        else:
+            if angle < 0 or angle > 180:
+                raise ValueError("Servo \"{0}\" turn angle \"{1}\" is not in (0, 180).".format(channel, angle))
+        val = self._angle_to_analog(angle)
+        self.pwm.write(channel, 0, val)
+        self._debug_('Turn angle = %d' % angle)
+
+    @property
+    def debug(self):
+        return self._DEBUG
+
+    @debug.setter
+    def debug(self, debug):
+        ''' Set if debug information shows '''
+        if debug in (True, False):
+            self._DEBUG = debug
+        else:
+            raise ValueError('debug must be "True" (Set debug on) or "False" (Set debug off), not "{0}"'.format(debug))
+
+        if self._DEBUG:
+            print(self._DEBUG_INFO, "Set debug on")
+        else:
+            print(self._DEBUG_INFO, "Set debug off")

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:
+        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:
+            pass
         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
+        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))