jadonk/README.md

## README.md

      
    Raw
  

              README.md
            
          
    Moving this to https://github.com/jadonk/BlueDonkey
Switched to Buster now, so all this needs updating.
Some useful links:

https://github.com/spmallick/learnopencv
https://www.linkedin.com/learning/opencv-for-python-developers?trk=profileNonSelf_d_flagship3_profile_view_base_learningFeedm015%3Aa001_601786_learning
https://gist.github.com/wroscoe/d7005216ab36cd4fa6ae0a5b428cbc1f#file-manage-py-L263
http://docs.micropython.org/
http://docs.openmv.io/library/omv.image.html
https://github.com/openmv/openmv/blob/master/src/omv/img/stats.c
https://openmv.io/blogs/news/linear-regression-line-following
https://github.com/flatironinstitute/CaImAn/blob/master/caiman/base/rois.py
https://github.com/jokla/CarND-LaneLines-P1/blob/master/P1.ipynb
https://medium.com/@esmat.anis/robust-extrapolation-of-lines-in-video-using-linear-hough-transform-edd39d642ddf

The version of python-opencv for Debian Stretch is only for Python2 (https://packages.debian.org/stretch/python-opencv). You need to use Buster to get a build for Python3 (https://packages.debian.org/buster/python-opencv).
The pip repository for Python3 doesn't seem to have an armv7 build, so you need to use a version built for Raspberry Pi Zero or build it yourself.
The instructions below grab the Debian Stretch OpenCV library and the opencv-python pre-built for R-Pi0, assuming a Debian Stretch BeagleBone Blue image.
libjasper isn't in Debian Stretch, so it is built from source.
wget https://www.piwheels.org/simple/opencv-python/opencv_python-3.4.0.12-cp35-cp35m-linux_armv7l.whl#sha256=ff66665ddaa44d9a3a5271f4169ae865cdc3de897771dcc898053e8041fd2296
git clone https://github.com/mdadams/jasper
sudo bash -C <<EOF
apt-get update
DEBIAN_FRONTEND=noninteractive apt-get install -y cmake libjpeg62-turbo libtiff5 libpng16-16 libavcodec57 libavformat57 libswscale4 libv4l-0 libxvidcore4 libx264-148 libgtk2.0-bin libatlas3-base libwebp6 libopencv-dev libgstreamer1.0-0 libqtgui4 libqt4-test roboticscape python3 python3-pip python3-wheel xterm xauth libsdl1.2-dev
python3 -m wheel install opencv_python-3.4.0.12-cp35-cp35m-linux_armv7l.whl
#pip3 install rcpy
#pip3 install pygame
EOF
cd jasper
cmake . -DALLOW_IN_SOURCE_BUILD=1
make
sudo make install
sudo ln -s /usr/local/lib/libjasper.so.4 /usr/lib/libjasper.so.1
cd /opt/source/rcpy
git pull
sudo python3 setup.py install
Update kernel and boot scripts

If you don't have a recent image, update some stuff first:
sudo bash -C <<EOF
cd /opt/scripts/tools
git pull
./update_kernel.sh --lts-4_14
cd developers
yes | ./update_bootloader.sh
EOF
Reboot
Update distro

sudo bash -C <<EOF
apt-mark hold c9-core-installer
DEBIAN_FRONTEND=noninteractive apt-get -yq upgrade
EOF
Buster


https://debian.beagleboard.org/images/bone-debian-buster-iot-armhf-2018-06-17-4gb.img.xz

Add python3-pygame from sid.
sudo apt-get update
sudo apt-get install -y python3-pip python3-wheel python3-opencv libopencv-dev

  
## camera.py
#!/usr/bin/env python3
import os, sys
#if not os.geteuid() == 0:
#    sys.exit("\nPlease run as root.\n")
if sys.version_info < (3,0):
    sys.exit("\nPlease run under python3.\n")

import cv2, time
#capture = cv2.VideoCapture(0)
#capture.set(cv2.CAP_PROP_FPS, 10)
#capture.set(cv2.CAP_PROP_FRAME_WIDTH, 160)
#capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 120)
#capture.set(cv2.CAP_PROP_AUTO_EXPOSURE, 0.25)
#capture.set(cv2.CAP_PROP_EXPOSURE, 0.003)

import numpy

def capture_and_process_frame():
    #ret, frame = capture.read()
    frame = cv2.imread('camera01.jpg')
    ret = True

    if ret:
        cv2.imwrite('/run/bluedonkey/camera.jpg', frame)
        lower_yellow = numpy.array([0,160,160])
        upper_yellow = numpy.array([220,255,255])
        mask = cv2.inRange(frame, lower_yellow, upper_yellow)
        res = cv2.bitwise_and(frame, frame, mask=mask)
        cv2.rectangle(res, (0,0), (159,119), (0,0,0), 2)
        #cv2.imwrite('/run/bluedonkey/filtered.jpg', res)
        gray = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
        kernel = numpy.ones((3,3), numpy.uint8)
        #erosion = cv2.erode(gray, kernel, iterations = 1)
        dilation = cv2.dilate(gray, kernel, iterations = 1)
        ret, thresh = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
        #cv2.imwrite('/run/bluedonkey/filtered.jpg', thresh)
        im2, contours, hierarchy = cv2.findContours(thresh, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
        #cnt = contours[1]
        cv2.drawContours(res, contours, -1, (255,0,0), 3)
        params = cv2.SimpleBlobDetector_Params()
        params.filterByArea = True;
        params.minArea = 10;
        params.maxArea = 1000000;
        params.filterByColor = True;
        params.blobColor = 255;
        detector = cv2.SimpleBlobDetector_create(params)
        keypoints = detector.detect(thresh)
        res = cv2.drawKeypoints(res, keypoints, numpy.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
        #[vx,vy,x,y] = cv2.fitLine(cnt, cv2.DIST_L2, 0, 0.01, 0.01)
        #lefty = int((-x*vy/vx) + y)
        #righty = int(((160-x)*vy/vx)+y)
        #res = cv2.line(res, (159,righty), (0,lefty), (0,255,0), 2)
        #edges = cv2.Canny(thresh, 5, 15, apertureSize = 3)
        #res = numpy.bitwise_or(res, edges[:,:,numpy.newaxis])
        #lines = cv2.HoughLines(edges, 1, numpy.pi/180, 100)
        #for rho,theta in lines[0]:
        #    a = np.cos(theta)
        #    b = np.sin(theta)
        #    x0 = a*rho
        #    y0 = b*rho
        #    x1 = int(x0 + 160*(-b))
        #    y1 = int(y0 + 120*(a))
        #    x2 = int(x0 - 160*(-b))
        #    y2 = int(y0 - 120*(a))
        #    cv2.line(img,(x1,y1),(x2,y2),(0,0,255),2)
        cv2.imwrite('/run/bluedonkey/filtered.jpg', res)

    time.sleep(0.2)

#while(capture.isOpened()):
#   capture_and_process_frame()
capture_and_process_frame()
#capture.release()

## camera01.jpg

      
    Raw
  

              camera01.jpg
            
          
## camera02.jpg

      
    Raw
  

              camera02.jpg
            
          
## camera03.png

      
    Raw
  

              camera03.png
            
          
## images.html
<html>
<head>
<script src="https://ajax.googleapis.com/ajax/libs/jquery/3.3.1/jquery.min.js"></script>
<script type="text/javascript">
var timeoutPeriod = 100;
var cameraURI = '/bluedonkey/camera.png';
var filteredURI = '/bluedonkey/filtered.png';

function timedRefresh() {
    $('#camera').attr("src", cameraURI + '?d=' + Date.now());
    $('#filtered').attr("src", filteredURI + '?d=' + Date.now());
    setTimeout(timedRefresh,timeoutPeriod);
}
</script>

<title>Live images</title>

</head>

<body onload="timedRefresh();">

<img id="camera" width="160" height="120" />
<img id="filtered" width="160" height="120" />

</body>
</html>

## lane_detector.py
#!/usr/bin/env python3
import os, sys
#if not os.geteuid() == 0:
#    sys.exit("\nPlease run as root.\n")
if sys.version_info < (3,0):
    sys.exit("\nPlease run under python3.\n")

import cv2, time
#capture = cv2.VideoCapture(0)
#capture.set(cv2.CAP_PROP_FPS, 10)
#capture.set(cv2.CAP_PROP_FRAME_WIDTH, 160)
#capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 120)
#capture.set(cv2.CAP_PROP_AUTO_EXPOSURE, 0.25)
#capture.set(cv2.CAP_PROP_EXPOSURE, 0.003)

import numpy

def capture_and_process_frame():
    #ret, frame = capture.read()
    frame = cv2.imread('camera01.jpg')
    ret = True

    if ret:
        cv2.imwrite('/run/bluedonkey/camera.jpg', frame)
        lower_yellow = numpy.array([0,160,160])
        upper_yellow = numpy.array([255,255,255])
        mask = cv2.inRange(frame, lower_yellow, upper_yellow)
        res = cv2.bitwise_and(frame, frame, mask=mask)
        #cv2.rectangle(res, (0,0), (159,119), (0,0,0), 2)
        #cv2.imwrite('/run/bluedonkey/filtered.jpg', res)
        gray = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
        if True:
            blur = cv2.GaussianBlur(gray, (7, 7), 0)
            #edges = cv2.Canny(blur, 50, 150)
            dilation = cv2.dilate(blur, cv2.getStructuringElement(cv2.MORPH_DILATE, (5, 5)))
            erosion = cv2.erode(dilation, cv2.getStructuringElement(cv2.MORPH_ERODE, (3, 3)))
            #merge = gray + erosion
            lines = cv2.HoughLinesP(erosion, 2, numpy.pi/180, 9, numpy.array([]), minLineLength=30, maxLineGap=30)
            line_img = numpy.zeros((res.shape[0], res.shape[1], 3), dtype=numpy.uint8)
            for line in lines:
                for x1,y1,x2,y2 in line:
                    angle = numpy.arctan2(y2 - y1, x2 - x1) * 180. / numpy.pi
                    if ( (abs(angle) > 20.) and (abs(angle) < 90.)):
                        cv2.line(line_img, (x1, y1), (x2, y2), (0,0,255), 1)
            res = cv2.addWeighted(res, 0.8, line_img, 1, 0)
            gray_lines = cv2.cvtColor(line_img, cv2.COLOR_BGR2GRAY)
            pixelpoints = cv2.findNonZero(gray_lines)
        else:
            pixelpoints = cv2.findNonZero(gray)
        [vx,vy,x,y] = cv2.fitLine(pixelpoints, 4, 0, 0.01, 0.01)
        lefty = int((-x*vy/vx) + y)
        righty = int(((160-x)*vy/vx)+y)
        res = cv2.line(res, (159,righty), (0,lefty), (0,255,0), 2)
        cv2.imwrite('/run/bluedonkey/filtered.jpg', res)

    time.sleep(0.2)

#while(capture.isOpened()):
#   capture_and_process_frame()
capture_and_process_frame()
#capture.release()

## line_follower.py
#!/usr/bin/env python3
import os, sys
if not os.geteuid() == 0:
    sys.exit("\nPlease run as root.\n")
if sys.version_info < (3,0):
    sys.exit("\nPlease run under python3.\n")

import cv2, rcpy, datetime, time, numpy, pygame, threading, math

# Enable steering servo
from rcpy.servo import servo1
rcpy.servo.enable()
servo1.set(0)
servo1clk = rcpy.clock.Clock(servo1, 0.02)
servo1clk.start()

# Enable throttle
from rcpy.servo import servo3
rcpy.servo.enable()
servo3.set(0)
servo3clk = rcpy.clock.Clock(servo3, 0.02)
servo3clk.start()
time.sleep(1)
print("Arming throttle")
servo3.set(-0.1)
time.sleep(3)
servo3.set(0)

# This file was originally part of the OpenMV project.
# Copyright (c) 2013-2017 Ibrahim Abdelkader <iabdalkader@openmv.io> & Kwabena W. Agyeman <kwagyeman@openmv.io>
# This work is licensed under the MIT license, see the file LICENSE for details.

###########
# Settings
###########

IMG_DIR = "/var/lib/cloud9/mnt"
#IMG_DIR = "/run/bluedonkey"

COLOR_LINE_FOLLOWING = True # False to use grayscale thresholds, true to use color thresholds.
COLOR_THRESHOLDS = [( 85, 100,  -40,  -10,    0,  127)] # Yellow Line.
GRAYSCALE_THRESHOLDS = [(240, 255)] # White Line.
COLOR_HIGH_LIGHT_THRESHOLDS = [(80, 100, -10, 10, -10, 10)]
# https://pythonprogramming.net/color-filter-python-opencv-tutorial/
#COLOR_HIGH_LIGHT_THRESHOLDS_MAX = numpy.array([255,255,255])
#COLOR_HIGH_LIGHT_THRESHOLDS_MIN = numpy.array([230,230,230])
COLOR_HIGH_LIGHT_THRESHOLDS_MAX = numpy.array([255,190,60])
COLOR_HIGH_LIGHT_THRESHOLDS_MIN = numpy.array([165,40,0])
#FRAME_EXPOSURE = 0.000001
FRAME_EXPOSURE = 0
GRAYSCALE_HIGH_LIGHT_THRESHOLDS = [(250, 255)]
BINARY_VIEW = False # Helps debugging but costs FPS if on.
#BINARY_VIEW = True # Helps debugging but costs FPS if on.
DO_NOTHING = False # Just capture frames...
#FRAME_SIZE = sensor.QQVGA # Frame size.
FRAME_WIDTH = 160
FRAME_HEIGHT = 120
FRAME_REGION = 0.75 # Percentage of the image from the bottom (0 - 1.0).
FRAME_WIDE = 1.0 # Percentage of the frame width.
ROI_VERTICES = numpy.array([[0,119], [0,90], [70,20], [90,20], [159,90], [159,119]], dtype=numpy.int32)
ROI_MASK = numpy.zeros((120, 160), numpy.uint8)
cv2.fillConvexPoly(ROI_MASK, ROI_VERTICES, 255)

AREA_THRESHOLD = 0 # Raise to filter out false detections.
PIXELS_THRESHOLD = 40 # Raise to filter out false detections.
MAG_THRESHOLD = 4 # Raise to filter out false detections.
MIXING_RATE = 0.9 # Percentage of a new line detection to mix into current steering.

# Tweak these values for your robocar.
THROTTLE_CUT_OFF_ANGLE = 3.0 # Maximum angular distance from 90 before we cut speed [0.0-90.0).
THROTTLE_CUT_OFF_RATE = 0.8 # How much to cut our speed boost (below) once the above is passed (0.0-1.0].
THROTTLE_GAIN = 0.0 # e.g. how much to speed up on a straight away
THROTTLE_OFFSET = 75.0 # e.g. default speed (0 to 100)
THROTTLE_P_GAIN = 1.0
THROTTLE_I_GAIN = 0.0
THROTTLE_I_MIN = -0.0
THROTTLE_I_MAX = 0.0
THROTTLE_D_GAIN = 0.0

# Tweak these values for your robocar.
STEERING_OFFSET = 90 # Change this if you need to fix an imbalance in your car (0 to 180).
STEERING_P_GAIN = -30.0 # Make this smaller as you increase your speed and vice versa.
STEERING_I_GAIN = 0.0
STEERING_I_MIN = -0.0
STEERING_I_MAX = 0.0
STEERING_D_GAIN = -7 # Make this larger as you increase your speed and vice versa.

# Tweak these values for your robocar.
#THROTTLE_SERVO_MIN_US = 1500
#THROTTLE_SERVO_MAX_US = 2000
THROTTLE_SERVO_MIN_US = 0
THROTTLE_SERVO_MAX_US = 0.1

# Tweak these values for your robocar.
#STEERING_SERVO_MIN_US = 700
#STEERING_SERVO_MAX_US = 2300
STEERING_SERVO_MIN_US = -1.5
STEERING_SERVO_MAX_US = 1.5

###########
# Setup
###########

FRAME_REGION = max(min(FRAME_REGION, 1.0), 0.0)
FRAME_WIDE = max(min(FRAME_WIDE, 1.0), 0.0)
MIXING_RATE = max(min(MIXING_RATE, 1.0), 0.0)

THROTTLE_CUT_OFF_ANGLE = max(min(THROTTLE_CUT_OFF_ANGLE, 89.99), 0)
THROTTLE_CUT_OFF_RATE = max(min(THROTTLE_CUT_OFF_RATE, 1.0), 0.01)

THROTTLE_OFFSET = max(min(THROTTLE_OFFSET, 100), 0)
STEERING_OFFSET = max(min(STEERING_OFFSET, 180), 0)

# Handle if these were reversed...
tmp = max(THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US)
THROTTLE_SERVO_MIN_US = min(THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US)
THROTTLE_SERVO_MAX_US = tmp

# Handle if these were reversed...
tmp = max(STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US)
STEERING_SERVO_MIN_US = min(STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US)
STEERING_SERVO_MAX_US = tmp

# This function maps the output of the linear regression function to a driving vector for steering
# the robocar. See https://openmv.io/blogs/news/linear-regression-line-following for more info.

old_cx_normal = None
def figure_out_my_steering(line, img):
    global old_cx_normal
    [vx,vy,x,y] = line
    height, width, layers = img.shape
    cx_middle = x - (width / 2)
    cx_normal = cx_middle / (width / 2)

    if old_cx_normal != None:
        old_cx_normal = (cx_normal * MIXING_RATE) + (old_cx_normal * (1.0 - MIXING_RATE))
    else:
        old_cx_normal = cx_normal
    return old_cx_normal

# Solve: THROTTLE_CUT_OFF_RATE = pow(sin(90 +/- THROTTLE_CUT_OFF_ANGLE), x) for x...
#        -> sin(90 +/- THROTTLE_CUT_OFF_ANGLE) = cos(THROTTLE_CUT_OFF_ANGLE)
t_power = math.log(THROTTLE_CUT_OFF_RATE) / math.log(math.cos(math.radians(THROTTLE_CUT_OFF_ANGLE)))

def figure_out_my_throttle(steering): # steering -> [0:180]

    # pow(sin()) of the steering angle is only non-zero when driving straight... e.g. steering ~= 90
    t_result = math.pow(math.sin(math.radians(max(min(steering, 179.99), 0.0))), t_power)

    return (t_result * THROTTLE_GAIN) + THROTTLE_OFFSET

#
# Servo Control Code
#

# throttle [0:100] (101 values) -> [THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US]
# steering [0:180] (181 values) -> [STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US]
def set_servos(throttle, steering):
    throttle = THROTTLE_SERVO_MIN_US + ((throttle/100) * (THROTTLE_SERVO_MAX_US - THROTTLE_SERVO_MIN_US))
    steering = STEERING_SERVO_MIN_US + ((steering/180) * (STEERING_SERVO_MAX_US - STEERING_SERVO_MIN_US))
    #print(steering, end="")
    servo3.set(throttle)
    servo1.set(steering)

#
# Camera Control Code
#

capture = cv2.VideoCapture(0)
capture.set(cv2.CAP_PROP_FPS, 30)
capture.set(cv2.CAP_PROP_FRAME_WIDTH, FRAME_WIDTH)
capture.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT)
if FRAME_EXPOSURE > 0:
    capture.set(cv2.CAP_PROP_AUTO_EXPOSURE, 0.25)
    capture.set(cv2.CAP_PROP_EXPOSURE, FRAME_EXPOSURE)
frame = numpy.zeros((120, 160, 3), dtype=numpy.uint8)
frame_in = numpy.zeros((120, 160, 3), dtype=numpy.uint8)

class cameraThread(threading.Thread):
    def run(self):
        global frame, frame_in
        while True:
            ret, frametmp = capture.read()
            if ret:
                frame_in = frametmp
            k = cv2.waitKey(5) & 0xFF
            if k == ord('q'):
                break

thread = cameraThread()
thread.start()

clock = pygame.time.Clock()

###########
# Loop
###########

old_time = datetime.datetime.now()

throttle_old_result = None
throttle_i_output = 0
throttle_output = THROTTLE_OFFSET

steering_old_result = None
steering_i_output = 0
steering_output = STEERING_OFFSET

frame_cnt = 0

while True:
    clock.tick()
    #ret, frame = capture.read()
    frame = frame_in
    color_mask = cv2.inRange(frame, COLOR_HIGH_LIGHT_THRESHOLDS_MIN, COLOR_HIGH_LIGHT_THRESHOLDS_MAX)
    res = cv2.bitwise_and(frame, frame, mask=color_mask)
    res = cv2.bitwise_and(res, res, mask=ROI_MASK)
    gray = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
    pixelpoints = cv2.findNonZero(gray)
    if pixelpoints is not None:
        vx = 0
        vy = 1
        x = int(pixelpoints[:,:,0].mean())
        y = 50
        line = [vx,vy,x,y]
    else:
        line = False

    print_string = ""
    if line:
        new_time = datetime.datetime.now()
        delta_time = (new_time - old_time).microseconds / 1000
        old_time = new_time

        #
        # Figure out steering and do steering PID
        #

        steering_new_result = figure_out_my_steering(line, frame)
        #print("%05.2f " % (steering_new_result), end="")
        steering_delta_result = (steering_new_result - steering_old_result) if (steering_old_result != None) else 0
        steering_old_result = steering_new_result

        steering_p_output = steering_new_result # Standard PID Stuff here... nothing particularly interesting :)
        steering_i_output = max(min(steering_i_output + steering_new_result, STEERING_I_MAX), STEERING_I_MIN)
        steering_d_output = ((steering_delta_result * 1000) / delta_time) if delta_time else 0
        steering_pid_output = (STEERING_P_GAIN * steering_p_output) + \
                              (STEERING_I_GAIN * steering_i_output) + \
                              (STEERING_D_GAIN * steering_d_output)

        # Steering goes from [-90,90] but we need to output [0,180] for the servos.
        steering_output = (STEERING_OFFSET + steering_pid_output) % 180

        #
        # Figure out throttle and do throttle PID
        #

        throttle_new_result = figure_out_my_throttle(steering_output)
        throttle_delta_result = (throttle_new_result - throttle_old_result) if (throttle_old_result != None) else 0
        throttle_old_result = throttle_new_result

        throttle_p_output = throttle_new_result # Standard PID Stuff here... nothing particularly interesting :)
        throttle_i_output = max(min(throttle_i_output + throttle_new_result, THROTTLE_I_MAX), THROTTLE_I_MIN)
        throttle_d_output = ((throttle_delta_result * 1000) / delta_time) if delta_time else 0
        throttle_pid_output = (THROTTLE_P_GAIN * throttle_p_output) + \
                              (THROTTLE_I_GAIN * throttle_i_output) + \
                              (THROTTLE_D_GAIN * throttle_d_output)

        # Throttle goes from 0% to 100%.
        throttle_output = max(min(throttle_pid_output, 100), 0)

        print_string = "Line Ok   - throttle %03d, steering %03d, frame %05d" % \
            (throttle_output , steering_output, frame_cnt)

    else:
        throttle_output = throttle_output * 0.99
        print_string = "Line Lost - throttle %03d, steering %03d, frame %05d" % \
            (throttle_output , steering_output, frame_cnt)

    set_servos(throttle_output, steering_output)
    if BINARY_VIEW:
        frame_file_name = "%s/cam%05d.png" % (IMG_DIR, frame_cnt)
        res_file_name = "%s/res%05d.png" % (IMG_DIR, frame_cnt)
        frame_cnt += 1
        cv2.imwrite(frame_file_name, frame)
        if line:
            res = cv2.line(res, (x,0), (x,119), (0,255,0), 2)
        cv2.imwrite(res_file_name, res)
    print("FPS %05.2f - %s\r" % (clock.get_fps(), print_string), end="")

## motors.py
#!/usr/bin/env python3
# Run the motors
# Based on: https://github.com/mcdeoliveira/rcpy/raw/master/examples/rcpy_test_motors.py
# import python libraries

import rcpy
import rcpy.motor as motor
import sys
import time

class Manual:

    # CONSTANT VARIABLES #
     # CONTROL CONSTANTS #
    BACKWARD = "\033[B" # Down
    FORWARD = "\033[A" # Up
    KILLSWITCH = "^C" # Select
    LEFT = "\033[D" # Left
    RIGHT = "\033[C" # Right
    SELECT_MODE = "3" # Start / Pause
    THROTTLE_DOWN = "4" # Left Trigger
    THROTTLE_UP = "6" # Right Trigger

     # THROTTLE CONSTANTS (Forward and Backward) #
    DIRECTION_THROTTLE_BACKWARD = -1 # Negative to go backward
    DIRECTION_THROTTLE_FORWARD = 1 # Positive to go forward
    DIRECTION_THROTTLE_DEFAULT = DIRECTION_THROTTLE_FORWARD # The direction to start travelling in
    DUTY_THROTTLE_MAX = 1.0 # Max duty allowed for throttle
    DUTY_THROTTLE_STEPS = 10 # Number of 'gears' to max throttle
    DUTY_THROTTLE_ITERATION_VALUE = DUTY_THROTTLE_MAX / DUTY_THROTTLE_STEPS

     # TURN CONSTANTS (Left and Right) #
    DIRECTION_TURN_CENTER = 0 # Directional value to keep wheels straight
    DIRECTION_TURN_DEFAULT = DIRECTION_TURN_CENTER # The direction the wheels start in
    DIRECTION_TURN_LEFT = 1 # Directional value to turn wheels towards the left
    DIRECTION_TURN_RIGHT = -1 # Directional value to turn wheels towards the right
    DUTY_TURN_MAX = 1.0 # Max duty allowed for turning
    DUTY_TURN_STEPS = 30 # Number of steps before reaching max turn radius
    DUTY_TURN_ITERATION_VALUE = DUTY_TURN_MAX / DUTY_TURN_STEPS

    # CONSTRUCTOR #
    def __init__(self):
        self.direction = self.DIRECTION_THROTTLE_DEFAULT
        self.direction_turn = self.DIRECTION_TURN_DEFAULT
        self.duty_throttle = 0
        self.duty_turn = 0
        rcpy.set_state(rcpy.RUNNING)
        print("Manual Initiated.")

    def change_direction(self, direction):
        if abs(direction) == 1: # Check if direction is either 1 or -1
            self.duty_throttle = 0 # Stop first
            self.direction = direction

    def check_hold(self, direction):
        if abs(direction) == 1 or direction == 0: # Check if direction is either 1, -1 or 0
            if self.direction_turn != direction: # If the input direction doesn't match the previously chosen...
                self.direction_turn = direction # ...change to new direction
                self.duty_turn = 0 # and reset the turn duty

    def decrease_throttle(self):
        if self.duty_throttle >= self.DUTY_THROTTLE_ITERATION_VALUE: # Check if duty is at least what will be subtracted
            self.duty_throttle = self.duty_throttle - self.DUTY_THROTTLE_ITERATION_VALUE # Iterate negatively
        else: # Duty is less than iteration value
            self.duty_throttle = 0 # Set duty to min

    def increase_throttle(self):
        if self.duty_throttle <= self.DUTY_THROTTLE_MAX - self.DUTY_THROTTLE_ITERATION_VALUE: # Check if duty is at most one iteration from max
            self.duty_throttle = self.duty_throttle + self.DUTY_THROTTLE_ITERATION_VALUE #Iterate Positively
        else: # Duty is greater than one iteration from max
            self.duty_throttle = self.DUTY_THROTTLE_MAX # Set duty to max

    def get_duty_throttle(self):
        return self.duty_throttle * self.direction # Return duty in the current direction

    def get_duty_turn(self):
        return self.duty_turn * self.direction_turn # Return duty in the current direction

    def kill(self):
        self.direction = 1
        self.direction_turn = 0
        self.duty_throttle = 0
        self.duty_turn = 0
        motor.set(1, 0)
        motor.set(2, 0)
        print("Killswitch activated.")

    def read_key(self):
        key = sys.stdin.read(1)
        if ord(key) == 27:
            key = key + sys.stdin.read(2)
        elif ord(key) == 3:
            raise KeyboardInterrupt
        return key

    def turning(self, key):
        # TODO: Use 'keyboard' module to read multiple keys at once
        if (key == self.LEFT) ^ (key == self.RIGHT): # If keystrokes contain -EITHER- LEFT or RIGHT...
            if key == self.LEFT: # If it's LEFT
                self.check_hold(self.DIRECTION_TURN_LEFT)
            else: # If it's RIGHT
                self.check_hold(self.DIRECTION_TURN_RIGHT)

            if self.duty_turn <= self.DUTY_TURN_MAX - self.DUTY_TURN_ITERATION_VALUE: # If the next iteration is at most one iteration from max duty...
                self.duty_turn = self.duty_turn + self.DUTY_TURN_ITERATION_VALUE # Iterate
            else: # Value is greater than one iteration away from max duty
                self.duty_turn = self.DUTY_TURN_MAX # Set duty to its max value
        else: # Neither or both directions selected
            self.check_hold(self.DIRECTION_TURN_CENTER) # Set direction to straight and reset duty

try:
    m = Manual()

    m.change_direction(m.DIRECTION_THROTTLE_BACKWARD)
    m.increase_throttle()

    while rcpy.get_state() != rcpy.EXITING:

        m.turning(m.LEFT)
        motor.set(1, m.get_duty_turn())
        motor.set(2, m.get_duty_throttle())

        time.sleep(.1)  # sleep some

except KeyboardInterrupt:
    m.kill()
    pass

finally:
    print("\nBye BeagleBone!")

## steering.py
#!/usr/bin/env python3
import os, sys
if not os.geteuid() == 0:
    sys.exit("\nPlease run as root.\n")
if sys.version_info < (3,0):
    sys.exit("\nPlease run under python3.\n")

import rcpy, time
from rcpy.servo import servo1
rcpy.servo.enable()
servo1.set(0)
servo1clk = rcpy.clock.Clock(servo1, 0.02)
servo1clk.start()

time.sleep(1)
servo1.set(-0.4)
time.sleep(1)
servo1.set(0.6)
time.sleep(1)
servo1.set(0)
time.sleep(1)

servo1clk.stop()
rcpy.servo.disable()

## stream_camera.sh
#!/bin/sh
#/usr/bin/mjpg_streamer -i "/usr/lib/mjpg-streamer/input_file.so -e -f ${PWD}" -o "/usr/lib/mjpg-streamer/output_http.so -p 8090 -w /usr/share/mjpg-streamer/www"
mkdir -p /run/bluedonkey
chown debian.debian /run/bluedonkey
chmod ugo+rwx /run/bluedonkey
cp images.html /run/bluedonkey/

## throttle.py
#!/usr/bin/env python3
import os, sys
if not os.geteuid() == 0:
    sys.exit("\nPlease run as root.\n")
if sys.version_info < (3,0):
    sys.exit("\nPlease run under python3.\n")

import rcpy, time
from rcpy.servo import servo3
rcpy.servo.enable()
servo3.set(0)
servo3clk = rcpy.clock.Clock(servo3, 0.02)
servo3clk.start()

time.sleep(1)
print("Arming")
servo3.set(-0.1)
time.sleep(3)
print("Idle")
servo3.set(0)
time.sleep(3)
#print("Medium")
#servo3.set(0.1)
#time.sleep(1)
print("Slow")
servo3.set(0.05)
time.sleep(3)
#print("Fast")
#servo3.set(0.2)
#time.sleep(2)
print("Brake")
servo3.set(-1)
time.sleep(1)
#print("Idle")
#servo3.set(0)
#time.sleep(2)
#print("Reverse")
#servo3.set(-0.3)
#time.sleep(1)
print("Stop")
servo3.set(0)
time.sleep(1)

servo3clk.stop()
rcpy.servo.disable()
	#!/usr/bin/env python3
	import os, sys
	#if not os.geteuid() == 0:
	# sys.exit("\nPlease run as root.\n")
	if sys.version_info < (3,0):
	sys.exit("\nPlease run under python3.\n")

	import cv2, time
	#capture = cv2.VideoCapture(0)
	#capture.set(cv2.CAP_PROP_FPS, 10)
	#capture.set(cv2.CAP_PROP_FRAME_WIDTH, 160)
	#capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 120)
	#capture.set(cv2.CAP_PROP_AUTO_EXPOSURE, 0.25)
	#capture.set(cv2.CAP_PROP_EXPOSURE, 0.003)

	import numpy

	def capture_and_process_frame():
	#ret, frame = capture.read()
	frame = cv2.imread('camera01.jpg')
	ret = True

	if ret:
	cv2.imwrite('/run/bluedonkey/camera.jpg', frame)
	lower_yellow = numpy.array([0,160,160])
	upper_yellow = numpy.array([220,255,255])
	mask = cv2.inRange(frame, lower_yellow, upper_yellow)
	res = cv2.bitwise_and(frame, frame, mask=mask)
	cv2.rectangle(res, (0,0), (159,119), (0,0,0), 2)
	#cv2.imwrite('/run/bluedonkey/filtered.jpg', res)
	gray = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
	kernel = numpy.ones((3,3), numpy.uint8)
	#erosion = cv2.erode(gray, kernel, iterations = 1)
	dilation = cv2.dilate(gray, kernel, iterations = 1)
	ret, thresh = cv2.threshold(dilation, 127, 255, cv2.THRESH_BINARY)
	#cv2.imwrite('/run/bluedonkey/filtered.jpg', thresh)
	im2, contours, hierarchy = cv2.findContours(thresh, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
	#cnt = contours[1]
	cv2.drawContours(res, contours, -1, (255,0,0), 3)
	params = cv2.SimpleBlobDetector_Params()
	params.filterByArea = True;
	params.minArea = 10;
	params.maxArea = 1000000;
	params.filterByColor = True;
	params.blobColor = 255;
	detector = cv2.SimpleBlobDetector_create(params)
	keypoints = detector.detect(thresh)
	res = cv2.drawKeypoints(res, keypoints, numpy.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
	#[vx,vy,x,y] = cv2.fitLine(cnt, cv2.DIST_L2, 0, 0.01, 0.01)
	#lefty = int((-x*vy/vx) + y)
	#righty = int(((160-x)*vy/vx)+y)
	#res = cv2.line(res, (159,righty), (0,lefty), (0,255,0), 2)
	#edges = cv2.Canny(thresh, 5, 15, apertureSize = 3)
	#res = numpy.bitwise_or(res, edges[:,:,numpy.newaxis])
	#lines = cv2.HoughLines(edges, 1, numpy.pi/180, 100)
	#for rho,theta in lines[0]:
	# a = np.cos(theta)
	# b = np.sin(theta)
	# x0 = a*rho
	# y0 = b*rho
	# x1 = int(x0 + 160*(-b))
	# y1 = int(y0 + 120*(a))
	# x2 = int(x0 - 160*(-b))
	# y2 = int(y0 - 120*(a))
	# cv2.line(img,(x1,y1),(x2,y2),(0,0,255),2)
	cv2.imwrite('/run/bluedonkey/filtered.jpg', res)

	time.sleep(0.2)

	#while(capture.isOpened()):
	# capture_and_process_frame()
	capture_and_process_frame()
	#capture.release()
	<html>
	<head>
	<script src="https://ajax.googleapis.com/ajax/libs/jquery/3.3.1/jquery.min.js"></script>
	<script type="text/javascript">
	var timeoutPeriod = 100;
	var cameraURI = '/bluedonkey/camera.png';
	var filteredURI = '/bluedonkey/filtered.png';

	function timedRefresh() {
	$('#camera').attr("src", cameraURI + '?d=' + Date.now());
	$('#filtered').attr("src", filteredURI + '?d=' + Date.now());
	setTimeout(timedRefresh,timeoutPeriod);
	}
	</script>

	<title>Live images</title>

	</head>

	<body onload="timedRefresh();">

	<img id="camera" width="160" height="120" />
	<img id="filtered" width="160" height="120" />

	</body>
	</html>
	#!/usr/bin/env python3
	import os, sys
	if not os.geteuid() == 0:
	sys.exit("\nPlease run as root.\n")
	if sys.version_info < (3,0):
	sys.exit("\nPlease run under python3.\n")

	import cv2, rcpy, datetime, time, numpy, pygame, threading, math

	# Enable steering servo
	from rcpy.servo import servo1
	rcpy.servo.enable()
	servo1.set(0)
	servo1clk = rcpy.clock.Clock(servo1, 0.02)
	servo1clk.start()

	# Enable throttle
	from rcpy.servo import servo3
	rcpy.servo.enable()
	servo3.set(0)
	servo3clk = rcpy.clock.Clock(servo3, 0.02)
	servo3clk.start()
	time.sleep(1)
	print("Arming throttle")
	servo3.set(-0.1)
	time.sleep(3)
	servo3.set(0)

	# This file was originally part of the OpenMV project.
	# Copyright (c) 2013-2017 Ibrahim Abdelkader <iabdalkader@openmv.io> & Kwabena W. Agyeman <kwagyeman@openmv.io>
	# This work is licensed under the MIT license, see the file LICENSE for details.

	###########
	# Settings
	###########

	IMG_DIR = "/var/lib/cloud9/mnt"
	#IMG_DIR = "/run/bluedonkey"

	COLOR_LINE_FOLLOWING = True # False to use grayscale thresholds, true to use color thresholds.
	COLOR_THRESHOLDS = [( 85, 100, -40, -10, 0, 127)] # Yellow Line.
	GRAYSCALE_THRESHOLDS = [(240, 255)] # White Line.
	COLOR_HIGH_LIGHT_THRESHOLDS = [(80, 100, -10, 10, -10, 10)]
	# https://pythonprogramming.net/color-filter-python-opencv-tutorial/
	#COLOR_HIGH_LIGHT_THRESHOLDS_MAX = numpy.array([255,255,255])
	#COLOR_HIGH_LIGHT_THRESHOLDS_MIN = numpy.array([230,230,230])
	COLOR_HIGH_LIGHT_THRESHOLDS_MAX = numpy.array([255,190,60])
	COLOR_HIGH_LIGHT_THRESHOLDS_MIN = numpy.array([165,40,0])
	#FRAME_EXPOSURE = 0.000001
	FRAME_EXPOSURE = 0
	GRAYSCALE_HIGH_LIGHT_THRESHOLDS = [(250, 255)]
	BINARY_VIEW = False # Helps debugging but costs FPS if on.
	#BINARY_VIEW = True # Helps debugging but costs FPS if on.
	DO_NOTHING = False # Just capture frames...
	#FRAME_SIZE = sensor.QQVGA # Frame size.
	FRAME_WIDTH = 160
	FRAME_HEIGHT = 120
	FRAME_REGION = 0.75 # Percentage of the image from the bottom (0 - 1.0).
	FRAME_WIDE = 1.0 # Percentage of the frame width.
	ROI_VERTICES = numpy.array([[0,119], [0,90], [70,20], [90,20], [159,90], [159,119]], dtype=numpy.int32)
	ROI_MASK = numpy.zeros((120, 160), numpy.uint8)
	cv2.fillConvexPoly(ROI_MASK, ROI_VERTICES, 255)

	AREA_THRESHOLD = 0 # Raise to filter out false detections.
	PIXELS_THRESHOLD = 40 # Raise to filter out false detections.
	MAG_THRESHOLD = 4 # Raise to filter out false detections.
	MIXING_RATE = 0.9 # Percentage of a new line detection to mix into current steering.

	# Tweak these values for your robocar.
	THROTTLE_CUT_OFF_ANGLE = 3.0 # Maximum angular distance from 90 before we cut speed [0.0-90.0).
	THROTTLE_CUT_OFF_RATE = 0.8 # How much to cut our speed boost (below) once the above is passed (0.0-1.0].
	THROTTLE_GAIN = 0.0 # e.g. how much to speed up on a straight away
	THROTTLE_OFFSET = 75.0 # e.g. default speed (0 to 100)
	THROTTLE_P_GAIN = 1.0
	THROTTLE_I_GAIN = 0.0
	THROTTLE_I_MIN = -0.0
	THROTTLE_I_MAX = 0.0
	THROTTLE_D_GAIN = 0.0

	# Tweak these values for your robocar.
	STEERING_OFFSET = 90 # Change this if you need to fix an imbalance in your car (0 to 180).
	STEERING_P_GAIN = -30.0 # Make this smaller as you increase your speed and vice versa.
	STEERING_I_GAIN = 0.0
	STEERING_I_MIN = -0.0
	STEERING_I_MAX = 0.0
	STEERING_D_GAIN = -7 # Make this larger as you increase your speed and vice versa.

	# Tweak these values for your robocar.
	#THROTTLE_SERVO_MIN_US = 1500
	#THROTTLE_SERVO_MAX_US = 2000
	THROTTLE_SERVO_MIN_US = 0
	THROTTLE_SERVO_MAX_US = 0.1

	# Tweak these values for your robocar.
	#STEERING_SERVO_MIN_US = 700
	#STEERING_SERVO_MAX_US = 2300
	STEERING_SERVO_MIN_US = -1.5
	STEERING_SERVO_MAX_US = 1.5

	###########
	# Setup
	###########

	FRAME_REGION = max(min(FRAME_REGION, 1.0), 0.0)
	FRAME_WIDE = max(min(FRAME_WIDE, 1.0), 0.0)
	MIXING_RATE = max(min(MIXING_RATE, 1.0), 0.0)

	THROTTLE_CUT_OFF_ANGLE = max(min(THROTTLE_CUT_OFF_ANGLE, 89.99), 0)
	THROTTLE_CUT_OFF_RATE = max(min(THROTTLE_CUT_OFF_RATE, 1.0), 0.01)

	THROTTLE_OFFSET = max(min(THROTTLE_OFFSET, 100), 0)
	STEERING_OFFSET = max(min(STEERING_OFFSET, 180), 0)

	# Handle if these were reversed...
	tmp = max(THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US)
	THROTTLE_SERVO_MIN_US = min(THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US)
	THROTTLE_SERVO_MAX_US = tmp

	# Handle if these were reversed...
	tmp = max(STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US)
	STEERING_SERVO_MIN_US = min(STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US)
	STEERING_SERVO_MAX_US = tmp

	# This function maps the output of the linear regression function to a driving vector for steering
	# the robocar. See https://openmv.io/blogs/news/linear-regression-line-following for more info.

	old_cx_normal = None
	def figure_out_my_steering(line, img):
	global old_cx_normal
	[vx,vy,x,y] = line
	height, width, layers = img.shape
	cx_middle = x - (width / 2)
	cx_normal = cx_middle / (width / 2)

	if old_cx_normal != None:
	old_cx_normal = (cx_normal * MIXING_RATE) + (old_cx_normal * (1.0 - MIXING_RATE))
	else:
	old_cx_normal = cx_normal
	return old_cx_normal

	# Solve: THROTTLE_CUT_OFF_RATE = pow(sin(90 +/- THROTTLE_CUT_OFF_ANGLE), x) for x...
	# -> sin(90 +/- THROTTLE_CUT_OFF_ANGLE) = cos(THROTTLE_CUT_OFF_ANGLE)
	t_power = math.log(THROTTLE_CUT_OFF_RATE) / math.log(math.cos(math.radians(THROTTLE_CUT_OFF_ANGLE)))

	def figure_out_my_throttle(steering): # steering -> [0:180]

	# pow(sin()) of the steering angle is only non-zero when driving straight... e.g. steering ~= 90
	t_result = math.pow(math.sin(math.radians(max(min(steering, 179.99), 0.0))), t_power)

	return (t_result * THROTTLE_GAIN) + THROTTLE_OFFSET

	#
	# Servo Control Code
	#

	# throttle [0:100] (101 values) -> [THROTTLE_SERVO_MIN_US, THROTTLE_SERVO_MAX_US]
	# steering [0:180] (181 values) -> [STEERING_SERVO_MIN_US, STEERING_SERVO_MAX_US]
	def set_servos(throttle, steering):
	throttle = THROTTLE_SERVO_MIN_US + ((throttle/100) * (THROTTLE_SERVO_MAX_US - THROTTLE_SERVO_MIN_US))
	steering = STEERING_SERVO_MIN_US + ((steering/180) * (STEERING_SERVO_MAX_US - STEERING_SERVO_MIN_US))
	#print(steering, end="")
	servo3.set(throttle)
	servo1.set(steering)

	#
	# Camera Control Code
	#

	capture = cv2.VideoCapture(0)
	capture.set(cv2.CAP_PROP_FPS, 30)
	capture.set(cv2.CAP_PROP_FRAME_WIDTH, FRAME_WIDTH)
	capture.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT)
	if FRAME_EXPOSURE > 0:
	capture.set(cv2.CAP_PROP_AUTO_EXPOSURE, 0.25)
	capture.set(cv2.CAP_PROP_EXPOSURE, FRAME_EXPOSURE)
	frame = numpy.zeros((120, 160, 3), dtype=numpy.uint8)
	frame_in = numpy.zeros((120, 160, 3), dtype=numpy.uint8)

	class cameraThread(threading.Thread):
	def run(self):
	global frame, frame_in
	while True:
	ret, frametmp = capture.read()
	if ret:
	frame_in = frametmp
	k = cv2.waitKey(5) & 0xFF
	if k == ord('q'):
	break

	thread = cameraThread()
	thread.start()

	clock = pygame.time.Clock()

	###########
	# Loop
	###########

	old_time = datetime.datetime.now()

	throttle_old_result = None
	throttle_i_output = 0
	throttle_output = THROTTLE_OFFSET

	steering_old_result = None
	steering_i_output = 0
	steering_output = STEERING_OFFSET

	frame_cnt = 0

	while True:
	clock.tick()
	#ret, frame = capture.read()
	frame = frame_in
	color_mask = cv2.inRange(frame, COLOR_HIGH_LIGHT_THRESHOLDS_MIN, COLOR_HIGH_LIGHT_THRESHOLDS_MAX)
	res = cv2.bitwise_and(frame, frame, mask=color_mask)
	res = cv2.bitwise_and(res, res, mask=ROI_MASK)
	gray = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
	pixelpoints = cv2.findNonZero(gray)
	if pixelpoints is not None:
	vx = 0
	vy = 1
	x = int(pixelpoints[:,:,0].mean())
	y = 50
	line = [vx,vy,x,y]
	else:
	line = False

	print_string = ""
	if line:
	new_time = datetime.datetime.now()
	delta_time = (new_time - old_time).microseconds / 1000
	old_time = new_time

	#
	# Figure out steering and do steering PID
	#

	steering_new_result = figure_out_my_steering(line, frame)
	#print("%05.2f " % (steering_new_result), end="")
	steering_delta_result = (steering_new_result - steering_old_result) if (steering_old_result != None) else 0
	steering_old_result = steering_new_result

	steering_p_output = steering_new_result # Standard PID Stuff here... nothing particularly interesting :)
	steering_i_output = max(min(steering_i_output + steering_new_result, STEERING_I_MAX), STEERING_I_MIN)
	steering_d_output = ((steering_delta_result * 1000) / delta_time) if delta_time else 0
	steering_pid_output = (STEERING_P_GAIN * steering_p_output) + \
	(STEERING_I_GAIN * steering_i_output) + \
	(STEERING_D_GAIN * steering_d_output)

	# Steering goes from [-90,90] but we need to output [0,180] for the servos.
	steering_output = (STEERING_OFFSET + steering_pid_output) % 180

	#
	# Figure out throttle and do throttle PID
	#

	throttle_new_result = figure_out_my_throttle(steering_output)
	throttle_delta_result = (throttle_new_result - throttle_old_result) if (throttle_old_result != None) else 0
	throttle_old_result = throttle_new_result

	throttle_p_output = throttle_new_result # Standard PID Stuff here... nothing particularly interesting :)
	throttle_i_output = max(min(throttle_i_output + throttle_new_result, THROTTLE_I_MAX), THROTTLE_I_MIN)
	throttle_d_output = ((throttle_delta_result * 1000) / delta_time) if delta_time else 0
	throttle_pid_output = (THROTTLE_P_GAIN * throttle_p_output) + \
	(THROTTLE_I_GAIN * throttle_i_output) + \
	(THROTTLE_D_GAIN * throttle_d_output)

	# Throttle goes from 0% to 100%.
	throttle_output = max(min(throttle_pid_output, 100), 0)

	print_string = "Line Ok - throttle %03d, steering %03d, frame %05d" % \
	(throttle_output , steering_output, frame_cnt)

	else:
	throttle_output = throttle_output * 0.99
	print_string = "Line Lost - throttle %03d, steering %03d, frame %05d" % \
	(throttle_output , steering_output, frame_cnt)

	set_servos(throttle_output, steering_output)
	if BINARY_VIEW:
	frame_file_name = "%s/cam%05d.png" % (IMG_DIR, frame_cnt)
	res_file_name = "%s/res%05d.png" % (IMG_DIR, frame_cnt)
	frame_cnt += 1
	cv2.imwrite(frame_file_name, frame)
	if line:
	res = cv2.line(res, (x,0), (x,119), (0,255,0), 2)
	cv2.imwrite(res_file_name, res)
	print("FPS %05.2f - %s\r" % (clock.get_fps(), print_string), end="")
	#!/usr/bin/env python3
	# Run the motors
	# Based on: https://github.com/mcdeoliveira/rcpy/raw/master/examples/rcpy_test_motors.py
	# import python libraries

	import rcpy
	import rcpy.motor as motor
	import sys
	import time

	class Manual:

	# CONSTANT VARIABLES #
	# CONTROL CONSTANTS #
	BACKWARD = "\033[B" # Down
	FORWARD = "\033[A" # Up
	KILLSWITCH = "^C" # Select
	LEFT = "\033[D" # Left
	RIGHT = "\033[C" # Right
	SELECT_MODE = "3" # Start / Pause
	THROTTLE_DOWN = "4" # Left Trigger
	THROTTLE_UP = "6" # Right Trigger

	# THROTTLE CONSTANTS (Forward and Backward) #
	DIRECTION_THROTTLE_BACKWARD = -1 # Negative to go backward
	DIRECTION_THROTTLE_FORWARD = 1 # Positive to go forward
	DIRECTION_THROTTLE_DEFAULT = DIRECTION_THROTTLE_FORWARD # The direction to start travelling in
	DUTY_THROTTLE_MAX = 1.0 # Max duty allowed for throttle
	DUTY_THROTTLE_STEPS = 10 # Number of 'gears' to max throttle
	DUTY_THROTTLE_ITERATION_VALUE = DUTY_THROTTLE_MAX / DUTY_THROTTLE_STEPS

	# TURN CONSTANTS (Left and Right) #
	DIRECTION_TURN_CENTER = 0 # Directional value to keep wheels straight
	DIRECTION_TURN_DEFAULT = DIRECTION_TURN_CENTER # The direction the wheels start in
	DIRECTION_TURN_LEFT = 1 # Directional value to turn wheels towards the left
	DIRECTION_TURN_RIGHT = -1 # Directional value to turn wheels towards the right
	DUTY_TURN_MAX = 1.0 # Max duty allowed for turning
	DUTY_TURN_STEPS = 30 # Number of steps before reaching max turn radius
	DUTY_TURN_ITERATION_VALUE = DUTY_TURN_MAX / DUTY_TURN_STEPS

	# CONSTRUCTOR #
	def __init__(self):
	self.direction = self.DIRECTION_THROTTLE_DEFAULT
	self.direction_turn = self.DIRECTION_TURN_DEFAULT
	self.duty_throttle = 0
	self.duty_turn = 0
	rcpy.set_state(rcpy.RUNNING)
	print("Manual Initiated.")

	def change_direction(self, direction):
	if abs(direction) == 1: # Check if direction is either 1 or -1
	self.duty_throttle = 0 # Stop first
	self.direction = direction

	def check_hold(self, direction):
	if abs(direction) == 1 or direction == 0: # Check if direction is either 1, -1 or 0
	if self.direction_turn != direction: # If the input direction doesn't match the previously chosen...
	self.direction_turn = direction # ...change to new direction
	self.duty_turn = 0 # and reset the turn duty

	def decrease_throttle(self):
	if self.duty_throttle >= self.DUTY_THROTTLE_ITERATION_VALUE: # Check if duty is at least what will be subtracted
	self.duty_throttle = self.duty_throttle - self.DUTY_THROTTLE_ITERATION_VALUE # Iterate negatively
	else: # Duty is less than iteration value
	self.duty_throttle = 0 # Set duty to min

	def increase_throttle(self):
	if self.duty_throttle <= self.DUTY_THROTTLE_MAX - self.DUTY_THROTTLE_ITERATION_VALUE: # Check if duty is at most one iteration from max
	self.duty_throttle = self.duty_throttle + self.DUTY_THROTTLE_ITERATION_VALUE #Iterate Positively
	else: # Duty is greater than one iteration from max
	self.duty_throttle = self.DUTY_THROTTLE_MAX # Set duty to max

	def get_duty_throttle(self):
	return self.duty_throttle * self.direction # Return duty in the current direction

	def get_duty_turn(self):
	return self.duty_turn * self.direction_turn # Return duty in the current direction

	def kill(self):
	self.direction = 1
	self.direction_turn = 0
	self.duty_throttle = 0
	self.duty_turn = 0
	motor.set(1, 0)
	motor.set(2, 0)
	print("Killswitch activated.")

	def read_key(self):
	key = sys.stdin.read(1)
	if ord(key) == 27:
	key = key + sys.stdin.read(2)
	elif ord(key) == 3:
	raise KeyboardInterrupt
	return key

	def turning(self, key):
	# TODO: Use 'keyboard' module to read multiple keys at once
	if (key == self.LEFT) ^ (key == self.RIGHT): # If keystrokes contain -EITHER- LEFT or RIGHT...
	if key == self.LEFT: # If it's LEFT
	self.check_hold(self.DIRECTION_TURN_LEFT)
	else: # If it's RIGHT
	self.check_hold(self.DIRECTION_TURN_RIGHT)

	if self.duty_turn <= self.DUTY_TURN_MAX - self.DUTY_TURN_ITERATION_VALUE: # If the next iteration is at most one iteration from max duty...
	self.duty_turn = self.duty_turn + self.DUTY_TURN_ITERATION_VALUE # Iterate
	else: # Value is greater than one iteration away from max duty
	self.duty_turn = self.DUTY_TURN_MAX # Set duty to its max value
	else: # Neither or both directions selected
	self.check_hold(self.DIRECTION_TURN_CENTER) # Set direction to straight and reset duty

	try:
	m = Manual()

	m.change_direction(m.DIRECTION_THROTTLE_BACKWARD)
	m.increase_throttle()

	while rcpy.get_state() != rcpy.EXITING:

	m.turning(m.LEFT)
	motor.set(1, m.get_duty_turn())
	motor.set(2, m.get_duty_throttle())

	time.sleep(.1) # sleep some

	except KeyboardInterrupt:
	m.kill()
	pass

	finally:
	print("\nBye BeagleBone!")
	#!/bin/sh
	#/usr/bin/mjpg_streamer -i "/usr/lib/mjpg-streamer/input_file.so -e -f ${PWD}" -o "/usr/lib/mjpg-streamer/output_http.so -p 8090 -w /usr/share/mjpg-streamer/www"
	mkdir -p /run/bluedonkey
	chown debian.debian /run/bluedonkey
	chmod ugo+rwx /run/bluedonkey
	cp images.html /run/bluedonkey/