def setup():
global offset_x, offset_y, offset, forward0, forward1
offset_x = 0
offset_y = 0
offset = 0
forward0 = 'True'
forward1 = 'False'
try:
for line in open('config'):
if line[0:8] == 'offset_x':
offset_x = int(line[11:-1])
print 'offset_x =', offset_x
if line[0:8] == 'offset_y':
offset_y = int(line[11:-1])
print 'offset_y =', offset_y
if line[0:8] == 'offset =':
offset = int(line[9:-1])
print 'offset =', offset
if line[0:8] == "forward0":
forward0 = line[11:-1]
print 'turning0 =', forward0
if line[0:8] == "forward1":
forward1 = line[11:-1]
print 'turning1 =', forward1
except:
print 'no config file, set config to original'
video_dir.setup(busnum=busnum)
car_dir.setup(busnum=busnum)
motor.setup(busnum=busnum)
video_dir.calibrate(offset_x, offset_y)
car_dir.calibrate(offset)
def setup():
global offset_x, offset_y, offset, forward0, forward1
offset_x = 0
offset_y = 0
offset = 0
forward0 = 'True'
forward1 = 'False'
try:
for line in open('config'):
if line[0:8] == 'offset_x':
offset_x = int(line[11:-1])
print 'offset_x =', offset_x
if line[0:8] == 'offset_y':
offset_y = int(line[11:-1])
print 'offset_y =', offset_y
if line[0:8] == 'offset =':
offset = int(line[9:-1])
print 'offset =', offset
if line[0:8] == "forward0":
forward0 = line[11:-1]
print 'turning0 =', forward0
if line[0:8] == "forward1":
forward1 = line[11:-1]
print 'turning1 =', forward1
except:
print 'no config file, set config to original'
video_dir.setup(busnum=busnum)
car_dir.setup(busnum=busnum)
motor.setup(busnum=busnum)
video_dir.calibrate(offset_x, offset_y)
car_dir.calibrate(offset)
def run_mode(request):
video_dir.setup()
car_dir.setup()
motor.setup()
video_dir.home_x_y()
car_dir.home()
motor.setSpeed(50)
return HttpResponse("Run mode start")
def setup(): # Setup and calibrate direction and motor car_dir.setup(busnum=busnum) motor.setup(busnum=busnum) car_dir.home() # Starting streamer thread for image processing mjpg_st_th = MJPGStreamerThread() mjpg_st_th.start() em_th = EmergencyModule() em_th.start()
def __init__(self):
print('debug')
busnum = 0
logging.info('Creating an SDC object')
steer_motor.setup()
print('debug2')
drive_motor.setup()
drive_motor.ctrl(1)
self.spd_cmd = 0
self.spd_cur = self.spd_cmd
self.str_cmd = 450
self.str_cur = self.str_cmd
steer_motor.home()
def setup():
global offset_x, offset_y, offset, forward0, forward1, backward0, backward1
offset_x = 0
offset_y = 0
offset = 0
forward0 = 'True'
forward1 = 'False'
# Read calibration value from config file
try:
for line in open('config'):
if line[0:8] == 'offset_x':
offset_x = int(line[11:-1])
print
'offset_x =', offset_x
if line[0:8] == 'offset_y':
offset_y = int(line[11:-1])
print
'offset_y =', offset_y
if line[0:8] == 'offset =':
offset = int(line[9:-1])
print
'offset =', offset
if line[0:8] == "forward0":
forward0 = line[11:-1]
print
'turning0 =', forward0
if line[0:8] == "forward1":
forward1 = line[11:-1]
print
'turning1 =', forward1
except:
print
'no config file, set config to original'
video_dir.setup(busnum=busnum)
car_dir.setup(busnum=busnum)
motor.setup(busnum=busnum)
video_dir.calibrate(offset_x, offset_y)
car_dir.calibrate(offset)
# Set the motor's true / false value to the opposite.
backward0 = REVERSE(forward0)
backward1 = REVERSE(forward1)
def mover():
sRobotDirection = Direction()
rospy.init_node('mover', anonymous=True)
rospy.Subscriber("motioncommand", String,
sRobotDirection.direction_callback)
video_dir.setup(busnum=busnum)
car_dir.setup(busnum=busnum)
motor.setup(
busnum=busnum
) # Initialize the Raspberry Pi GPIO connected to the DC motor.
video_dir.home_x_y()
car_dir.home()
#data = sRobotDirection.directionMsg
rospy.spin()
def setup():
global offset, offset_x, offset_y, forward0, forward1, backward0, backward1
offset_x = 0
offset_y = 0
offset = 0
forward0 = 'True'
forward1 = 'False'
# Read calibration value from config file
try:
for line in open('config'):
find_line(offset_x, "offset_x", 'offset_x')
find_line(offset_y, "offset_y", 'offset_y')
find_line(forward0, "forward0", 'turning0')
find_line(forward1, "forward1", 'turning1')
if line[0:8] == 'offset =':
offset = int(line[9:-1])
print
'offset =', offset
except:
print
'no config file, set config to original'
video_dir.setup(busnum=busnum)
car_dir.setup(busnum=busnum)
motor.setup(busnum=busnum)
video_dir.calibrate(offset_x, offset_y)
car_dir.calibrate(offset)
# Set the motor's true / false value to the opposite.
backward0 = REVERSE(forward0)
backward1 = REVERSE(forward1)
def drive(stops):
# setup all devices and initialize values
busnum = 1
car_dir.setup(busnum=busnum)
motor.setup(busnum=busnum)
distance_detect.setup()
car_dir.home()
motor.setSpeed(30)
video_capture = cv2.VideoCapture(-1)
video_capture.set(3, 160)
video_capture.set(4, 120)
command = 0
stopCount = 0
stopFound = 0
#drive until passed certain amount of stops
while True:
# capture video frames
ret, frame = video_capture.read()
# set color masking boundaries for red and mask image
colorLower = np.array([0, 0, 100], dtype='uint8')
colorUpper = np.array([50, 50, 255], dtype='uint8')
colorMask = cv2.inRange(frame, colorLower, colorUpper)
outMask = cv2.bitwise_and(frame, frame, mask=colorMask)
# create mask image, convert to grayscale, and blur
clonedImg = outMask.copy()
clonedImg = cv2.cvtColor(clonedImg, cv2.COLOR_BGR2GRAY)
clonedImg = cv2.GaussianBlur(clonedImg, (5, 5), 0)
# show current image
cv2.namedWindow('Gray color select', cv2.WINDOW_NORMAL)
cv2.imshow('Gray color select', clonedImg)
# calculate circles within image
circles = cv2.HoughCircles(clonedImg,
cv2.cv.CV_HOUGH_GRADIENT,
1,
20,
param1=50,
param2=30,
minRadius=15,
maxRadius=100)
# if a circle was detected
if circles != None:
# if this is first time encountering this stop increase stop count
if stopFound == 0:
stopCount += 1
stopFound = 1
# map out circles for display
circles = np.uint16(np.around(circles))
for i in circles[0, :]:
cv2.circle(clonedImg, (i[0], i[1]), i[2], (0, 255, 0), 2)
cv2.circle(clonedImg, (i[0], i[1]), 2, (0, 255, 0), 3)
elif (cv2.countNonZero(clonedImg) == 0):
stopFound = 0
# display camera feed and circles
cv2.namedWindow('Circles', cv2.WINDOW_NORMAL)
cv2.imshow('Circles', clonedImg)
# crop the image
crop_img = frame[60:120, 0:160]
# convert to grayscale
gray = cv2.cvtColor(crop_img, cv2.COLOR_BGR2GRAY)
# gaussian blur
blur = cv2.GaussianBlur(gray, (5, 5), 0)
# color thresholding
ret, thresh = cv2.threshold(blur, 60, 255, cv2.THRESH_BINARY_INV)
# find the contours of the frame
contours, hierarchy = cv2.findContours(thresh.copy(), 1,
cv2.CHAIN_APPROX_NONE)
# detect distance to closes object
dis = distance_detect.checkdis()
print "distance: ", dis
# find the biggest contour (if detected)
if len(contours) > 0 and dis >= 15:
c = max(contours, key=cv2.contourArea)
M = cv2.moments(c)
if M['m00'] != 0:
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
cv2.line(crop_img, (cx, 0), (cx, 720), (255, 0, 0), 1)
cv2.line(crop_img, (0, cy), (1280, cy), (255, 0, 0), 1)
cv2.drawContours(crop_img, contours, -1, (0, 255, 0), 1)
if cx >= 120:
print "Turn Right ", cx
car_dir.turn_right()
motor.forward()
if cx < 120 and cx > 50:
print "On Track! ", cx
car_dir.home()
motor.forward()
if cx <= 50:
print "Turn Left! ", cx
car_dir.turn_left()
motor.forward()
else:
if dis < 15:
print "something blocking me"
car_dir.home()
motor.stop()
else:
print "I don't see the line"
car_dir.home()
motor.backward()
# display the resulting frame
cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
cv2.imshow('frame', crop_img)
# exit condition after passing certain amount of stops or 'q' is pressed
if stopCount == stops or (cv2.waitKey(1) & 0xFF == ord('q')):
# clean up
motor.stop()
distance_detect.cleanup()
cv2.destroyAllWindows()
break
from time import ctime # Import necessary modules
ctrl_cmd = ['forward', 'backward', 'left', 'right', 'stop', 'read cpu_temp', 'home', 'distance', 'x+', 'x-', 'y+', 'y-', 'xy_home']
HOST = '' # The variable of HOST is null, so the function bind( ) can be bound to all valid addresses.
PORT = 21567
BUFSIZ = 1024 # Size of the buffer
ADDR = (HOST, PORT)
tcpSerSock = socket(AF_INET, SOCK_STREAM) # Create a socket.
tcpSerSock.bind(ADDR) # Bind the IP address and port number of the server.
tcpSerSock.listen(5) # The parameter of listen() defines the number of connections permitted at one time. Once the
# connections are full, others will be rejected.
video_dir.setup()
car_dir.setup()
motor.setup() # Initialize the Raspberry Pi GPIO connected to the DC motor.
video_dir.home_x_y()
car_dir.home()
while True:
print 'Waiting for connection...'
# Waiting for connection. Once receiving a connection, the function accept() returns a separate
# client socket for the subsequent communication. By default, the function accept() is a blocking
# one, which means it is suspended before the connection comes.
tcpCliSock, addr = tcpSerSock.accept()
print '...connected from :', addr # Print the IP address of the client connected with the server.
while True:
data = ''
data = tcpCliSock.recv(BUFSIZ) # Receive data sent from the client.
def __init__(self):
motor.setup(busnum=1)
car_dir.setup()
def __init__(self):
m.setup() #Have to be done
cd.setup()
ctrl_cmd = ['forward', 'backward', 'left', 'right', 'stop', 'read cpu_temp', 'home', 'distance', 'x+', 'x-', 'y+', 'y-', 'xy_home']
busnum = 1 # Edit busnum to 0, if you uses Raspberry Pi 1 or 0
HOST = '' # The variable of HOST is null, so the function bind( ) can be bound to all valid addresses.
PORT = 21567
BUFSIZ = 1024 # Size of the buffer
ADDR = (HOST, PORT)
tcpSerSock = socket(AF_INET, SOCK_STREAM) # Create a socket.
tcpSerSock.bind(ADDR) # Bind the IP address and port number of the server.
tcpSerSock.listen(5) # The parameter of listen() defines the number of connections permitted at one time. Once the
# connections are full, others will be rejected.
video_dir.setup(busnum=busnum)
car_dir.setup(busnum=busnum)
motor.setup(busnum=busnum) # Initialize the Raspberry Pi GPIO connected to the DC motor.
video_dir.home_x_y()
car_dir.home()
while True:
print 'Waiting for connection...'
# Waiting for connection. Once receiving a connection, the function accept() returns a separate
# client socket for the subsequent communication. By default, the function accept() is a blocking
# one, which means it is suspended before the connection comes.
tcpCliSock, addr = tcpSerSock.accept()
print '...connected from :', addr # Print the IP address of the client connected with the server.
while True:
data = ''
data = tcpCliSock.recv(BUFSIZ) # Receive data sent from the client.
def setup():
car_dir.setup(busnum=busnum)
motor.setup(busnum=busnum)
car_dir.calibrate(0)
motor.setSpeed(50)
import numpy as np
import cv2
import matplotlib as mp
import motor
import car_dir as turn
import time
import video_dir as camTurn
from threading import Thread
cap = cv2.VideoCapture(0)
cascPath = "closed_frontal_palm.xml"
handCascade = cv2.CascadeClassifier(cascPath)
motor.setup()
turn.setup()
turn.home()
def forward():
turn.home()
print("go forward")
motor.forwardWithSpeed(spd=100)
time.sleep(1)
motor.stop()
def right_forward():
turn.home()
print("turn right")
turn.turn_right()
motor.forwardWithSpeed(spd=100)
time.sleep(1)
motor.stop()
def __init__(self):
self.busnum = 1 # See sunfounder's documentation, should be 1 for RPi3
sfdriving.setup(busnum=self.busnum)
sfsteering.setup(busnum=self.busnum)
sfpantilt.setup(busnum=self.busnum)
import socket
import car_dir
import motor
import time
import re
import pyttsx3
corpus = [
"we just reached out first attractions, we will begin tour after 10 seconds",
"we just reached out second attractions, we will begin tour after 10 seconds"
]
motor_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
motor_socket.connect(('192.168.25.9', 1325))
car_dir.setup(busnum=1)
motor.setup(busnum=1)
car_dir.home()
engine = pyttsx3.init()
rate = engine.getProperty('rate')
engine.setProperty('rate', rate - 30)
def tts_guide(text):
engine.say(text) # location corresponding text
engine.runAndWait()
while True:
data = ''
saver.restore(sess, "saved_networks6/2018-12-06_15_52")
ctrl_cmd = ['left', 'right']
busnum = 1
HOST = ''
PORT = 21567
BUFSIZ = 1024
ADDR = (HOST, PORT)
tcpSerSock = socket(AF_INET, SOCK_STREAM)
tcpSerSock.bind(ADDR)
tcpSerSock.listen(5)
# Centerize Servo Motor
car_dir.setup(busnum=busnum)
now = time.time()
for i in range(100):
car_dir.home()
pwm = car_dir.setup()
# Stop DC Motor just in case
motor.setup(busnum=busnum)
motor.setSpeed(100)
motor.stop()
frames = 500000
def capture():
#flag = True
frame = 0
stream = io.BytesIO()
prev = 400
def mover():
#Init variables for the control loop
counter = 1
bDriving = False
bEndLoops = False
sRobotDirection = Direction()
rospy.init_node('mover', anonymous=True)
rospy.Subscriber("motioncommand", String,
sRobotDirection.direction_callback)
scooper_dir.setup(busnum=busnum)
car_dir.setup(busnum=busnum)
# Initialize the Raspberry Pi GPIO connected to the DC motor.
motor.setup(busnum=busnum)
#video_dir.home_x_y()
#scooper_dir.servo_test ()
car_dir.home()
# Loop to wait for received commands.
while (bEndLoops == False):
# Loop to perform movement controls while input received.
while (bEndLoops == False):
data = sRobotDirection.directionMsg
# Analyze the command received and control the car accordingly.
#doAction (data, counter)
#counter += 1
#print counter
if not data:
break
if data == ctrl_cmd[0]:
print 'ELFF WILL DRIVE'
counter += 1
print counter
try:
spd = 20
#print "Moving forward with speed!"
motor.forwardWithSpeed(spd)
except:
print 'Error speed =' + str(spd)
elif data == ctrl_cmd[1]:
print 'ELFF WILL REVERSE'
counter += 1
print counter
try:
spd = 20
#print "Moving backward with speed!"
motor.backwardWithSpeed(spd)
except:
print 'Error speed =' + str(spd)
elif data == ctrl_cmd[2]:
print 'ELFF WILL GO LEFT'
counter += 1
car_dir.turn_left()
elif data == ctrl_cmd[3]:
print 'ELFF WILL GO RIGHT'
counter += 1
car_dir.turn_right()
elif data == ctrl_cmd[4]:
print 'ELFF WILL SCOOP'
scooper_dir.home_x_y()
scooper_dir.doScoop()
scooper_dir.home_x_y()
# Used with publisher.py only as a debug method.
elif data == ctrl_cmd[5]:
print 'ELFF WILL RESET POSITION'
scooper_dir.home_x_y()
car_dir.home()
motor.ctrl(0)
bEndLoops = True
elif data == ctrl_cmd[6]:
print 'ELFF WILL STOP'
counter += 1
print counter
try:
spd = 0
motor.forwardWithSpeed(spd)
except:
print 'Error speed =' + str(spd)
else:
print 'Waiting to receive a command...'
def shutdown():
global run
print("Stopping")
motor.setSpeed(0)
port.closePort()
run = False
if __name__ == "__main__":
run = True
port = serialPort()
motor.setup(busnum=busnum)
steer.setup(busnum=busnum)
print 'motor moving forward'
motor.setSpeed(speed)
motor.motor0(forward0)
motor.motor1(forward1)
# motor.setSpeed(0)
# motor.motor0(forward0)
# motor.motor1(forward1)
steer.turn(0)
port.openPort("/dev/ttyUSB1")
# open lidar with width, depth in cm, refresh rate in hz
port.lidarStart(30, 200, 10)
while run:
try:
data = port.lidarRead()
print("distance: ", data[0])
import car_dir
import motor
import time
import numpy as np
import cv2
from visionAlg.myFunctions import readyImage, splitImage, findCentroid, showRows, showCentroids, errorCalc
car_dir.setup()
motor.setup()
motor.setSpeed(0)
def Map(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
def saturate(x, ub, lb):
if x > ub:
return (ub)
elif x < lb:
return (lb)
else:
return (x)
kp = 1
ki = 80
kd = 0
lbase = -70 # numeric values for the turn limits of the car (lbase = left rbase = right)
def run_server(key):
import RPi.GPIO as GPIO
import video_dir
import car_dir
import motor
from time import ctime # Import necessary modules
ctrl_cmd = [
'forward', 'backward', 'left', 'right', 'stop', 'read cpu_temp',
'home', 'distance', 'x+', 'x-', 'y+', 'y-', 'xy_home'
]
busnum = 1 # Edit busnum to 0, if you uses Raspberry Pi 1 or 0
HOST = '' # The variable of HOST is null, so the function bind( ) can be bound to all valid addresses.
PORT = 21567
BUFSIZ = 1024 # Size of the buffer
ADDR = (HOST, PORT)
try:
tcpSerSock = socket(AF_INET, SOCK_STREAM) # Create a socket.
tcpSerSock.bind(
ADDR) # Bind the IP address and port number of the server.
tcpSerSock.listen(
1
) # The parameter of listen() defines the number of connections permitted at one time. Once the
except:
print "#####\nTHE SOCKET ADDRESS IS ALREADY IN USE!\nPlease restart your device and run this program again"
video_dir.setup(busnum=busnum)
car_dir.setup(busnum=busnum)
motor.setup(
busnum=busnum
) # Initialize the Raspberry Pi GPIO connected to the DC motor.
video_dir.home_x_y()
car_dir.home()
while True:
print 'Waiting for connection...'
# Waiting for connection. Once receiving a connection, the function accept() returns a separate
# client socket for the subsequent communication. By default, the function accept() is a blocking
# one, which means it is suspended before the connection comes.
tcpCliSock, addr = tcpSerSock.accept()
print '...connected from :', addr # Print the IP address of the client connected with the server.
while True:
try:
data = ''
data = tcpCliSock.recv(
BUFSIZ) # Receive data sent from the client.
print "\nEncrypted command recieved from client = ,", eval(
data)[2]
data = crypto.AES_decrypt(eval(data), key)
except:
print "INCOMPLETE PACKET ERROR - try to input the command again"
# Analyze the command received and control the car accordingly.
if not data:
print "Session closed by client"
break
if data == ctrl_cmd[0]:
print 'motor moving forward'
motor.forward()
elif data == ctrl_cmd[1]:
print 'recv backward cmd'
motor.backward()
elif data == ctrl_cmd[2]:
print 'recv left cmd'
car_dir.turn_left()
elif data == ctrl_cmd[3]:
print 'recv right cmd'
car_dir.turn_right()
elif data == ctrl_cmd[6]:
print 'recv home cmd'
car_dir.home()
elif data == ctrl_cmd[4]:
print 'recv stop cmd'
motor.ctrl(0)
elif data == ctrl_cmd[5]:
print 'read cpu temp...'
temp = cpu_temp.read()
tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
elif data == ctrl_cmd[8]:
print 'recv x+ cmd'
video_dir.move_increase_x()
elif data == ctrl_cmd[9]:
print 'recv x- cmd'
video_dir.move_decrease_x()
elif data == ctrl_cmd[10]:
print 'recv y+ cmd'
video_dir.move_increase_y()
elif data == ctrl_cmd[11]:
print 'recv y- cmd'
video_dir.move_decrease_y()
elif data == ctrl_cmd[12]:
print 'home_x_y'
video_dir.home_x_y()
elif data[0:5] == 'speed': # Change the speed
print data
numLen = len(data) - len('speed')
if numLen == 1 or numLen == 2 or numLen == 3:
tmp = data[-numLen:]
print 'tmp(str) = %s' % tmp
spd = int(tmp)
print 'spd(int) = %d' % spd
if spd < 24:
spd = 24
motor.setSpeed(spd)
elif data[0:5] == 'turn=': #Turning Angle
print 'data =', data
angle = data.split('=')[1]
try:
angle = int(angle)
car_dir.turn(angle)
except:
print 'Error: angle =', angle
elif data[0:8] == 'forward=':
print 'data =', data
spd = data[8:]
try:
spd = int(spd)
motor.forward(spd)
except:
print 'Error speed =', spd
elif data[0:9] == 'backward=':
print 'data =', data
spd = data.split('=')[1]
try:
spd = int(spd)
motor.backward(spd)
except:
print 'ERROR, speed =', spd
else:
#print 'Command Error! Cannot recognize command: ' + data
print "COMMAND ERROR - Unable to interpret recieved command"
tcpSerSock.close()
ctrl_cmd = ['forward', 'backward', 'left', 'right', 'stop', 'read cpu_temp', 'home', 'distance', 'x+', 'x-', 'y+', 'y-', 'xy_home']
busnum = 1 # Edit busnum to 0, if you uses Raspberry Pi 1 or 0
HOST = '' # The variable of HOST is null, so the function bind( ) can be bound to all valid addresses.
PORT = 21565
BUFSIZ = 1024 # Size of the buffer
ADDR = (HOST, PORT)
tcpSerSock = socket(AF_INET, SOCK_STREAM) # Create a socket.
tcpSerSock.bind(ADDR) # Bind the IP address and port number of the server.
tcpSerSock.listen(5) # The parameter of listen() defines the number of connections permitted at one time. Once the
# connections are full, others will be rejected.
video_dir.setup(busnum=busnum)
car_dir.setup(busnum=busnum)
motor.setup(busnum=busnum) # Initialize the Raspberry Pi GPIO connected to the DC motor.
video_dir.home_x_y()
car_dir.home()
while True:
print ('Waiting for connection...')
# Waiting for connection. Once receiving a connection, the function accept() returns a separate
# client socket for the subsequent communication. By default, the function accept() is a blocking
# one, which means it is suspended before the connection comes.
tcpCliSock, addr = tcpSerSock.accept()
print ('...connected from :'+ str(addr)) # Print the IP address of the client connected with the server.
while True:
data = ''
data = tcpCliSock.recv(BUFSIZ) # Receive data sent from the client.
def setup(self):
car_dir.setup(busnum=1)
motor.setup(busnum=1)
self.servo_home()
self.motor_stop()
