def servo_turn(self, cur_angle=-90):
if cur_angle == -90:
return
if CTRL_DEBUG:
print("&Before PID angle =", cur_angle)
steering_angle = int(self.pid(cur_angle)) + 90
if CTRL_DEBUG:
print("$After PID angle =", steering_angle)
car_dir.turn(steering_angle)
def do_POST(self):
req = urlparse(self.path)
if req.path == re.search('/turn', self.path):
car_dir.turn(req.body.angle)
elif req.path == re.search('/motors', self.path):
if req.body.forward == True:
motor.forward()
elif req.body.backward == True:
motor.forward()
elif req.body.stop == True:
motor.forward()
elif req.path == re.search('/speed', self.path):
motor.setSpeed(req.body.speed)
self.send_response(200)
self.send_header('Content-type', 'application/json')
self.end_headers()
def run_server(key):
#!/usr/bin/env python
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)
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:
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:
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()
def turn_angle(self, angle):
cd.turn(angle)
def on_message(ws, data):
print data
# Analyze the command received and control the car accordingly.
#if not data:
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:12] == ctrl_cmd[13]: # 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(30)
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 = 30
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
cap = cv2.VideoCapture(0)
cv2.namedWindow('frame')
cur_time = 0
motor.setSpeed(40)
while (cur_time < 2500):
line = ser.readline()
magData = open('magData.txt', 'a')
magData.write(str(cur_time))
magData.write(',')
magData.write(line)
magData.close()
motor.forward()
cur_time += 1
car_dir.turn(determine_steering_angle(cur_time) + 20)
time.sleep(1 / 60)
if (cur_time % 200 == 0):
motor.setSpeed(0)
time.sleep(5)
motor.setSpeed(40)
## ret, frame = cap.read()
##
## numRows = 10
## processed = readyImage(frame)
## imageRows = splitImage(processed, numRows)
##
## centroidArray = []
## centroidData = open('centroidData.txt', 'a')
## for i in range(7, len(imageRows)):
def run(self):
while True:
print 'Waiting for connection...'
tcpCliSock, addr = tcpSerSock.accept()
print '...connected from :', addr
while True:
data = ''
data = tcpCliSock.recv(BUFSIZ)
if not data:
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':
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=':
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
tcpSerSock.close()
controlState = 'manual' #start the process in manual control
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.
cur_time = 0
while True:
motor.forward()
cur_time += 1
car_dir.turn(frequency_sweep.determine_steering_angle(cur_time))
data = ''
data = tcpCliSock.recv(BUFSIZ) # Receive data sent from the client.
# Analyze the command received and control the car accordingly.
if not data:
break
if data[0:3] == 'mag':
controlState = 'magnetic'
elif data[0:3] == 'vis':
controlState = 'visual'
elif data[0:6] == 'manual':
controlState = 'manual'
cap = cv2.VideoCapture(0)
if display == 1:
cv2.namedWindow('frame')
sampleTime = .0166667
dt = sampleTime
motor.setSpeed(0)
totalTime = 0
errorSum = 0
prevError = 0
time.sleep(1)
car_dir.turn(100)
time.sleep(1)
car_dir.turn(0)
time.sleep(1)
car_dir.turn(80)
time.sleep(1)
car_dir.turn(0)
time.sleep(1)
car_dir.turn(60)
time.sleep(1)
car_dir.turn(0)
# ret, frame = cap.read()
# numRows = 8
# processed = readyImage(frame)
# imageRows = splitImage(processed, numRows)
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)
showCentroids(frame, numRows, centroidArray)
cv2.imshow('frame', frame)
cv2.waitKey(0)
# PID controller
error = errorCalc(frame, centroidArray, sampleTime)
error = error[-1]
errorSum += error * dt
errordt = (error - prevError) / dt
u = kp * error + ki * errorSum + kd * errordt
prevError = error
motor.forward()
car_dir.turn(int(Map(np.pi/2 + u, 0, np.pi, 0, 255)))
visData = open('visData.txt', 'a')
visData.write(str(totalTime))
visData.write(',')
visData.write(str(error))
visData.write(',')
visData.write(str(errordt))
visData.write(',')
visData.write(str(u))
visData.write(',')
visData.write(raw_vals)
visData.close()
time.sleep(sampleTime)
def saturate(x,ub,lb):
if x>ub:
return(ub)
elif x<lb:
return(lb)
else:
return(x)
# what is thiserror supposed to be? Is it the erorr in the heading angle for the car?
# I'm supposing that it's the error in the heading angle for the car, and if it's not,
# then we can convert this function to be exactly that.
# Uncomment delay if you want to run the car on the ground.
car_dir.turn(25)
time.sleep(5)
tot_time = 0
start_time = 0
SampleTime = .01666667
lbase = -75 # numeric values for the turn limits of the car (lbase = left rbase = right)
rbase = 125
langle = np.pi/2-np.pi/6-0.25 # Angular limits for turning the car
rangle = np.pi/2+np.pi/6+0.25
c_alpha = Map(25,lbase,rbase,langle,rangle) # Initial car-heading, set at straight ahead, 90 degrees
interror = 0
# Gain
Kp = 0.9 # used to be 1
Kd = 0 # used to be 120
Ki = 0.2
def process_request(data):
# Analyze the command received and control the car accordingly.
if not data:
return "cmd not understood"
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)
def turn(angle_in_degree):
car_dir.turn(angle_in_degree)
car_dir.home()
pass
motor.ctrl(0)
#while (datetime.datetime.now() -
#car_dir.turn_right()
#car_dir.turn_right()
#motor.forward()
print "pausing"
start = datetime.datetime.now()
#pause for 2 seconds
while (datetime.datetime.now() - start).seconds < 2:
pass
print "turnign left"
car_dir.turn(turn_angle)
motor.forward()
start = datetime.datetime.now()
while (datetime.datetime.now() - start).seconds < turn_time:
car_dir.turn(turn_angle)
pass
motor.ctrl(0)
print "pausing again"
print "go home"
car_dir.home()
motor.forward()
start = datetime.datetime.now()
while (datetime.datetime.now() - start).seconds < forward_time:
pass
def turning(request, angle): angle = int(angle) car_dir.turn(angle) text = "Turing angle", angle return HttpResponse(text)
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)
def setup():
car_dir.setup(busnum=busnum)
motor.setup(busnum=busnum)
car_dir.calibrate(0)
motor.setSpeed(50)
if __name__ == "__main__":
#try:
#setup()
#loop()
#except KeyboardInterrupt:
# tcpSerSock.close()
setup()
car_dir.turn(0);
time.sleep(2);
car_dir.turn(45);
time.sleep(2);
car_dir.turn(90);
time.sleep(2);
while (True):
time_before = time.time()
motor.forward()
# To do
#motor.ctrl(1)
#time.sleep(3)
#motor.ctrl(1, -1)
def steer(self, str_cmd):
MAX_STEER_DIFF = 1
steer_motor.turn(self.str_cmd)
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])
print("angle: ", data[1])
distance = data[0]
if distance != 0:
scale = gain / distance # the further away something is, the less we have to turn
else:
scale = gain
angle = data[1]
angle = kalman(
numLen = len(data) - len('speed')
if numLen == 1 or numLen == 2 or numLen == 3:
tmp = data[-numLen:]
spd = int(tmp)
if spd < 24:
spd = 24
motor.setSpeed(spd)
elif data[0:5] == 'turn=': #Turning Angle
angle = data.split('=')[1]
try:
angle = int(angle)
car_dir.turn(angle)
except:
print('Error: angle ='+ str(angle))
elif data[0:8] == 'forward=':
spd = data[8:]
try:
spd = int(spd)
motor.forward(spd)
except:
print('Error speed ='+str(spd))
elif data[0:9] == 'backward=':
spd = data.split('=')[1]
try:
spd = int(spd)
error = errorCalc(frame, centroidArray)
error = error[5]
errorSum += error * dt
errordt = (error - prevError) / dt
u = kp * error + ki * errorSum + kd * errordt
# u = saturate(u, rangle, langle)
prevError = error
motor.forward()
# print 'u is: ', u
# controlsignal = int(Map(np.pi/2 + u, 0, np.pi, 0, 255))
controlsignal = int(Map(u, ledge, redge, lbase, rbase))
print 'Control siganl is: ', controlsignal
car_dir.turn(controlsignal)
visData = open('visData3.txt', 'a')
visData.write(str(totalTime))
visData.write(',')
visData.write(str(error))
visData.write(',')
# visData.write(str(errordt))
# visData.write(',')
visData.write(str(u))
visData.write('\n')
visData.close()
time.sleep(sampleTime)
motor.setSpeed(0)
