Skip to content

Instantly share code, notes, and snippets.

@jadonk
Forked from kamalkiewicz/camera.py
Last active Jun 22, 2018
Embed
What would you like to do?
beaglebuggy

Moving this to https://github.com/jadonk/BlueDonkey

Switched to Buster now, so all this needs updating.

Some useful links:

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

Add python3-pygame from sid.

sudo apt-get update
sudo apt-get install -y python3-pip python3-wheel python3-opencv libopencv-dev
#!/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, 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()
#!/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!")
#!/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()
#!/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/
#!/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()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment