KropBot

:::js
var robotApp = angular.module('robotApp', []);
robotApp.controller('RobotController', function ($scope, $http, $interval, socket) {

    var uuidv4 = function () {
        return 'xxxxxxxx-xxxx-4xxx-yxxx-xxxxxxxxxxxx'.replace(/[xy]/g, function(c) {
            var r = Math.random() * 16 | 0, v = c == 'x' ? r : (r & 0x3 | 0x8);
            return v.toString(16);
        });
    }

    $scope.directions = [4,5,6,7,8,1,2,3];
    $scope.uuid = uuidv4();
    $scope.data = {
        selected: null,
        direction: null,
        magnitude: 0,
        n_controllers: 0,
        total_counts: {}
    }
    $scope.min = window.Math.min;

    socket.emit('client_ready');

    $scope.set_direction = function(i) {
        $scope.data.selected = i;
        $scope.send_instruction()
    }

    $scope.send_instruction = function() {
        socket.emit('instruction', {
            user: $scope.uuid,
            direction: $scope.data.selected,
        })
    }

    // Instruction timeout is 3 second, ping to ensure we stay live
    setInterval($scope.send_instruction, 1500);

:::js
        // Receive updated signal via socket and apply data
        socket.on('updated_status', function (data) {
            $scope.data.direction = data.direction;
            $scope.data.magnitude = data.magnitude;
            $scope.data.n_controllers = data.n_controllers
            $scope.data.total_counts = data.total_counts;
          });

        // Receive updated signal via socket and apply data
        socket.on('updated_image', function (data) {
            blob = new Blob([data], {type: "image/jpeg"});
            $scope.data.imageUrl = window.URL.createObjectURL(blob);
          });

 });

:::js
robotApp.factory('socket', function ($rootScope) {
  var socket = io.connect();
  return {
    on: function (eventName, callback) {
      socket.on(eventName, function () {
        var args = arguments;
        $rootScope.$apply(function () {
          callback.apply(socket, args);
        });
      });
    },
    emit: function (eventName, data, callback) {
      socket.emit(eventName, data, function () {
        var args = arguments;
        $rootScope.$apply(function () {
          if (callback) {
            callback.apply(socket, args);
          }
        });
      })
    }
  };
});

import os
import time

from flask import Flask
from flask_socketio import SocketIO, join_room

app = Flask(__name__)
app.config.from_object(os.environ.get('APP_SETTINGS', 'config.Config'))
app.config['SQLALCHEMY_TRACK_MODIFICATIONS'] = False
app.secret_key = app.config['SECRET_KEY']
socketio = SocketIO(app)

# Use a secret WS endpoint for robot.
robot_ws_secret = app.config['ROBOT_WS_SECRET']

# Buffer incoming instructions and emit direct to the robot.
# on each image cycle.
instruction_buffer = {}
latest_robot_state = {}

INSTRUCTION_DURATION = 3


@app.route('/')
def index():
    """
    Return template for survey view, data (form) loaded by JSON
    :return: raw index.html response
    """
    return app.send_static_file('index.html')

def clear_expired_instructions():
    """
    Remove all expired instructions from the buffer.

    Instructions are expired if their age > INSTRUCTION_DURATION. This needs to be low
    enough that the robot stops performing a behaviour when a client leaves, but
    high enough that an active client's instructions are not cleared due to lag.
    """
    global instruction_buffer
    threshold = time.time() - INSTRUCTION_DURATION
    instruction_buffer = {k: v for k, v in instruction_buffer.items() if v['timestamp'] > threshold}

@socketio.on('client_ready')
def client_ready_join_room(message):
    """
    Receive the ready instruction from (browser) clients and assign them to the client room.
    """
    join_room('clients')

@socketio.on('instruction')
def user_instruction(message):
    """
    Receive and buffer direction instruction from client.
    :return:
    """

    # Perform validation on inputs, direction must be in range 1-9 or None. Anything else
    # is interpreted as None (=STOP) from that client.
    message['direction'] = message['direction'] if message['direction'] in range(1, 9) else None

    instruction_buffer[message['user']] = {
        'direction': message['direction'],
        'timestamp': int(time.time())
    }

@socketio.on('robot_update_' + robot_ws_secret)
def robot_update(message):
    """
    Receive the robot's current status message (dict) and store for future
    forwarding to clients. Respond with the current instruction buffer directions.
    :param message: dict of robot status
    :return: list of directions (all clients)
    """
    # Forward latest state to clients.
    socketio.emit('updated_status', message, json=True, room='clients')
    # Clear expired instructions and return the remainder to the robot.
    clear_expired_instructions()
    return [v['direction'] for v in instruction_buffer.values()]


@socketio.on('robot_image_' + robot_ws_secret)
def robot_image(data):
    """
    Receive latest image data and broadcast to clients
    :param data:
    """
    # Forward latest camera image
    socketio.emit('updated_image', data, room='clients')


if __name__ == '__main__':
    socketio.run(app)

import atexit
from collections import Counter
from concurrent import futures
from io import BytesIO
import math, cmath
import os
import time

from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor
from picamera import PiCamera
from socketIO_client import SocketIO

SPEED_MULTIPLIER = 200
UPDATES_PER_SECOND = 5
CAMERA_QUALITY = 10
CAMERA_FPS = 5

# A store of incoming instructions from clients, stored as list of client directions
instructions = []

# Use a secret WS endpoint for robot.
robot_ws_secret = os.getenv('ROBOT_WS_SECRET', '')`

# Conversion from numeric inputs to motor instructions + multipliers. The multipliers
# are adjusted for each direction, e.g. forward is full-speed, turn is half.
DIRECTIONS = {
    1: ((Adafruit_MotorHAT.FORWARD, 0.75), (Adafruit_MotorHAT.FORWARD, 0.5)),
    2: ((Adafruit_MotorHAT.FORWARD, 0.5), (Adafruit_MotorHAT.BACKWARD, 0.5)),
    3: ((Adafruit_MotorHAT.BACKWARD, 0.75), (Adafruit_MotorHAT.BACKWARD, 0.5)),
    4: ((Adafruit_MotorHAT.BACKWARD, 1), (Adafruit_MotorHAT.BACKWARD, 1)),
    5: ((Adafruit_MotorHAT.BACKWARD, 0.5), (Adafruit_MotorHAT.BACKWARD, 0.75)),
    6: ((Adafruit_MotorHAT.BACKWARD, 0.5), (Adafruit_MotorHAT.FORWARD, 0.5)),
    7: ((Adafruit_MotorHAT.FORWARD, 0.5), (Adafruit_MotorHAT.FORWARD, 0.75)),
    8: ((Adafruit_MotorHAT.FORWARD, 1.5), (Adafruit_MotorHAT.FORWARD, 1.5)),
}

# Initialize motors, and define left and right controllers.
motor_hat = Adafruit_MotorHAT(addr=0x6f)
left_motor = motor_hat.getMotor(1)
right_motor = motor_hat.getMotor(2)


def turnOffMotors():
    """
    Shutdown motors and unset handlers.
    Called on exit to ensure the robot is stopped.
    """
    left_motor.run(Adafruit_MotorHAT.RELEASE)
    right_motor.run(Adafruit_MotorHAT.RELEASE)

def average_radians(list_of_radians):
    """
    Return average of a list of angles, in radians, and it's amplitude.

    We calculate a set of vectors for each angle, using a fixed distance.
    Add up the sum of the x, y of the resulting vectors.
    Work back to an angle + get a magnitude.

    :param list_of_radians:
    :return:
    """
    vectors = [cmath.rect(1, angle) if angle is not None else cmath.rect(0, 0)
                # length 1 for each vector; or 0,0 for null (stopped)
                for angle in list_of_radians]

    vector_sum = sum(vectors)
    return cmath.phase(vector_sum), abs(vector_sum)


def to_radians(d):
    """
    Convert 7-degrees values to radians.
    :param d:
    :return: direction in radians
    """
    return d * math.pi / 4 if d is not None else None


def to_degree7(r):
    """
    Convert radians to 'degrees' with a 0-7 scale.
    :param r:
    :return: direction in 7-value degrees
    """
    return round(r * 4 / math.pi)


def map1to8(v):
    """
    Limit v to the range 1-8 or None, with 0 being converted to 8 (straight ahead).

    This is necessary because the back-calculation to degree7 will negative values
    yet the input to calculate_average_instruction must use 1-8 to weight forward
    instructions correctly.
    :param v:
    :return: v, in the range 1-8 or None
    """
    if v is None or v > 0:
        return v
    return v + 8  # if 0, return 8

def calculate_average_instruction():
    """
    Return a dictionary of counts for each direction option in the current
    instructions and the direction with the maximum count.

    Directions are stored in numeric range 0-7, we first convert these imaginary
    degrees to radians, then calculate the average radians by adding vectors.
    Once we have that value in radians we can convert back to our own scale
    which the robot understands. The amplitude value gives us a speed.

    0 = Forward
    7/1 = Forward left/right (slight)
    6/2 = Turn left right (stationary)
    5/3 = Backwards left/right (slight)
    4 = Backwards

    :return: dict total_counts, direction
    """

    # If instructions remaining, calculate the average.
    if instructions:
        directions_v, direction_rads = zip(*[(d, to_radians(d)) for d in instructions])
        total_counts = Counter([map1to8(v) for v in directions_v])

        rad, magnitude = average_radians(direction_rads)

        if magnitude < 0.05:
            magnitude = 0
            direction = None

        return {
            'total_counts': total_counts,
            'direction': map1to8(to_degree7(rad)),
            'magnitude': magnitude
        }

    else:
        return {
            'total_counts': {},
            'direction': None,
            'magnitude': 0
        }

def control_robot(control):
    """
    Takes current robot control instructions and apply to the motors.
    If direction is None, all-stop, otherwise calculates a speed
    for each motor using a combination of DIRECTIONS, magnitude
    and SPEED_MULTIPLIER, capped at 255.
    :param control:
    """
    if control['direction'] is None:
        # All stop.
        left_motor.setSpeed(0)
        right_motor.setSpeed(0)
        return

    direction = int(control['direction'])
    left, right = DIRECTIONS[direction]
    magnitude = control['magnitude']

    left_motor.run(left[0])
    left_speed = int(left[1] * magnitude * SPEED_MULTIPLIER)
    left_speed = min(left_speed, 255)
    left_motor.setSpeed(left_speed)

    right_motor.run(right[0])
    right_speed = int(right[1] * magnitude * SPEED_MULTIPLIER)
    right_speed = min(right_speed, 255)
    right_motor.setSpeed(right_speed)

def on_new_instruction(message):
    """
    Handler for incoming instructions from clients. Instructions are received, combined
    and expired on the server, so only active instructions (on per client) are received
    here.
    :param message: dict of all current instructions from all clients.
    :return:
    """
    instructions.extend(message)

def streaming_worker():
    """
    A self-container worker for streaming the Pi camera over websockets to the server
    as JPEG images. Initializes the camera, opens the websocket then enters a continuous
    capture loop, with each snap transmitted.
    :return:
    """
    camera = PiCamera()
    camera.resolution = (200, 300)
    camera.framerate = CAMERA_FPS

    with BytesIO() as stream, SocketIO('https://kropbot.herokuapp.com', 443) as socketIO:
        # capture_continuous is an endless iterator. Using video port + low quality for speed.
        for _ in camera.capture_continuous(stream, format='jpeg', use_video_port=True, quality=CAMERA_QUALITY):
            stream.truncate()
            stream.seek(0)
            data = stream.read()
            socketIO.emit('robot_image_' + robot_ws_secret, bytearray(data))
            stream.seek(0)

if __name__ == "__main__":
    # Register our function to disable motors when we shutdown.
    atexit.register(turnOffMotors)
    with futures.ProcessPoolExecutor() as executor:
        # Execute our camera streamer 'streaming_worker' in a separate process.
        # This runs continuously until exit.
        future = executor.submit(streaming_worker)

        with SocketIO('https://kropbot.herokuapp.com', 443) as socketIO:
            while True:
                current_time = time.time()
                lock_time = current_time + 1.0 / UPDATES_PER_SECOND
                # Calculate current average instruction based on inputs,
                # then perform the action.
                instruction = calculate_average_instruction()
                control_robot(instruction)
                instruction['n_controllers'] = len(instructions)
                # on_new_instruction is a callback to handle the server's response.
                socketIO.emit('robot_update_' + robot_ws_secret, instruction, on_new_instruction)
                # Empty all current instructions before accepting any new ones,
                # ensuring that if we lose contact with the server we stop.
                del instructions[:]
                socketIO.wait_for_callbacks(5)

                # Throttle the updates sent out to UPDATES_PER_SECOND (very roughly).
                time.sleep(max(0, lock_time - time.time()))

heroku create <app-name>
heroku heroku git:remote -a <app-name>

git push heroku remote

heroku config:set ROBOT_WS_SECRET =<your-ws-secret>

scp robot.py pi@raspberrypi.local:/home/pi

git clone https://github.com/adafruit/Adafruit-Motor-HAT-Python Library.git
cd Adafruit-Motor-HAT-Python-Library
sudo apt-get install python3-dev python3-picamera
sudo python3 setup.py install

sudo pip3 install git+https://github.com/wwqgtxx/socketIO-client.git

raspi-config

python3 robot.py

@reboot ROBOT_WS_SECRET=<your_ws_value> python3 /home/pi/robot.py