jadonk · June 22, 2018 15:01
diff --git a/README.md b/README.md
diff --git a/camera.py b/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()
diff --git a/camera01.jpg b/camera01.jpg
diff --git a/camera02.jpg b/camera02.jpg
diff --git a/camera03.png b/camera03.png
diff --git a/images.html b/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>
diff --git a/lane_detector.py b/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()
diff --git a/line_follower.py b/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 <[email protected]> & Kwabena W. Agyeman <[email protected]>
 # 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="")
diff --git a/motors.py b/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!")
diff --git a/steering.py b/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()
diff --git a/stream_camera.sh b/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/
diff --git a/throttle.py b/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 <[email protected]> & Kwabena W. Agyeman <[email protected]>
	# 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/