def run():
while True:
print 'waiting for connection...'
tcpCliSock, addr = tcpSerSock.accept()
print '...connected from :', addr
while True:
data = ''
data = tcpCliSock.recv(BUFSIZ)
if not data:
continue
if data == ctrl_cmd[0]:
global spd
print 'motor moving forward'
motor.motor(status, forward, spd)
direction = forward
print 'spd = %d' % spd
ledon = threading.Thread(target=ledOn)
ledon.start()
elif data == ctrl_cmd[1]:
print 'recv backward cmd'
motor.motor(status, backward, spd)
direction = backward
print 'spd = %d' % spd
breathingon = threading.Thread(target=breathingOn)
breathingon.start()
elif data == ctrl_cmd[2]:
print 'recv left cmd'
rgbLed.setup(leftRgb)
car_dir.dir_left(pwm0)
car_dir.dis_left(pwm1)
leftrgb = threading.Thread(target=leftrgbCtrl)
leftrgb.start()
rgbLed.stop(rightRgb)
continue
elif data == ctrl_cmd[3]:
print 'recv right cmd'
rgbLed.setup(rightRgb)
car_dir.dir_right(pwm0)
car_dir.dis_right(pwm1)
rightrgb = threading.Thread(target=rightrgbCtrl)
rightrgb.start()
rgbLed.stop(leftRgb)
continue
elif data == ctrl_cmd[4]:
print 'recv home cmd'
car_dir.dir_home(pwm0)
car_dir.dis_home(pwm1)
rgbLed.stop(rightRgb)
rgbLed.stop(leftRgb)
leftrgb = Process(target=homeRgbCtrl1)
leftrgb.start()
rightrgb = Process(target=homeRgbCtrl2)
rightrgb.start()
continue
elif data == ctrl_cmd[5]:
print 'recv distance cmd'
print 'Distance: %0.2fm' % ultrasonic.checkdist()
continue
elif data == ctrl_cmd[6]:
print 'recv whistle cmd'
activeBuzzer.loop()
continue
elif data == ctrl_cmd[7]:
print 'recv stop cmd'
setup()
motor.motorStop()
breathingLed.stop(b_LedPin)
rgbLed.stop(leftRgb)
rgbLed.stop(rightRgb)
activeBuzzer.stop()
GPIO.cleanup()
setup()
continue
elif data == ctrl_cmd[8]:
print 'recv exit cmd'
GPIO.cleanup()
tcpSerSock.close()
os.system('sudo init 0')
elif data == ctrl_cmd[9]:
print 'recev auto cmd'
auto = threading.Thread(target=autoMode)
auto.start()
continue
elif data[0:5] == 'speed':
print 'recv speed cmd'
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
print 'direction = %d' % direction
if spd < minspd:
spd = minspd
elif spd > maxspd:
spd = maxspd
motor.motor(status, direction, spd)
continue
else:
print 'Command Error! Cannot recongnize command: ' + data
def run(): #Main function
global ip_con
while True:
#print('SET %s'%dir_mid+' %s'%dis_mid+' %s'%b_spd+' %s'%t_spd+' %s'%left+' %s'%right)
print('waiting for connection...')
tcpCliSock, addr = tcpSerSock.accept()#Determine whether to connect
print('...connected from :', addr)
tcpCliSock.send('SET %s'%dir_mid+' %s'%dis_mid+' %s'%b_spd+' %s'%t_spd+' %s'%left+' %s'%right)
break
vn=threading.Thread(target=video_net) #Define a thread for connection
vn.setDaemon(True) #'True' means it is a front thread,it would close when the mainloop() closes
vn.start() #Thread starts
while True:
data = ''
data = tcpCliSock.recv(BUFSIZ).decode()#Get instructions
#print(data)
if not data:
continue
elif 'spdset' in data:
global spd_ad
try:
spd_ad=float((str(data))[7:]) #Speed Adjustment
except:
print('wrong speed value')
elif 'EC1set' in data: #Camera Adjustment
try:
new_EC1=int((str(data))[7:])
replace_num('E_C1:',new_EC1)
except:
pass
elif 'EC2set' in data: #Ultrasonic Adjustment
try:
new_EC2=int((str(data))[7:])
replace_num('E_C2:',new_EC2)
except:
pass
elif 'EM1set' in data: #Motor A Speed Adjustment
try:
new_EM1=int((str(data))[7:])
replace_num('E_M1:',new_EM1)
except:
pass
elif 'EM2set' in data: #Motor B Speed Adjustment
try:
new_EM2=int((str(data))[7:])
replace_num('E_M2:',new_EM2)
except:
pass
elif 'ET1set' in data: #Motor A Turningf Speed Adjustment
try:
new_ET1=int((str(data))[7:])
replace_num('E_T1:',new_ET1)
except:
pass
elif 'ET2set' in data: #Motor B Turningf Speed Adjustment
try:
new_ET2=int((str(data))[7:])
replace_num('E_T2:',new_ET2)
except:
pass
elif 'scan' in data:
dis_can=scan() #Start Scanning
str_list_1=dis_can[0:100] #Divide the list to make it samller to send
str_index=' ' #Separate the values by space
str_send_1=str_index.join(str_list_1)+' '
tcpCliSock.send(str(str_send_1)) #Send Part 1
#print(str_send_1)
time.sleep(0.3)
str_list_2=dis_can[101:200]
str_send_2=str_index.join(str_list_2)+' '
tcpCliSock.send(str(str_send_2)) #Send Part 2
#print(str_send_2)
time.sleep(0.3)
str_list_3=dis_can[201:300]
str_send_3=str_index.join(str_list_3)+' '
tcpCliSock.send(str(str_send_3)) #Send Part 3
#print(str_send_3)
time.sleep(0.3)
str_list_4=dis_can[301:408]
str_send_4=str_index.join(str_list_4)
tcpCliSock.send(str(str_send_4)) #Send Part 4
#print(str_send_4)
time.sleep(0.3)
tcpCliSock.send('finished') #Send 'finished' tell the client to stop receiving the list of dis_can
#print(dis_can)
#print(len(dis_can))
elif 'forward' in data: #When server receive "forward" from client,car moves forward
global b_spd
global t_spd
tcpCliSock.send('1')
motor.motor(status, forward, b_spd*spd_ad)
motor.motor1(status,forward,t_spd*spd_ad)
direction = forward
elif 'backward' in data: #When server receive "backward" from client,car moves backward
tcpCliSock.send('2')
motor.motor(status, backward, b_spd*spd_ad)
motor.motor1(status, backward, t_spd*spd_ad)
direction = backward
elif 'left' in data: #When server receive "left" from client,camera turns left
tcpCliSock.send('7')
car_dir.dir_left(pwm1)
continue
elif 'right' in data: #When server receive "right" from client,camera turns right
tcpCliSock.send('8')
car_dir.dir_right(pwm1)
continue
elif 'on' in data: #When server receive "on" from client,camera looks up
tcpCliSock.send('5')
car_dir.dir_Left(pwm0)
continue
elif 'under' in data: #When server receive "under" from client,camera looks down
tcpCliSock.send('6')
car_dir.dir_Right(pwm0)
continue
elif 'Left' in data: #When server receive "Left" from client,car turns left
tcpCliSock.send('3')
motor.motor(status, forward, left*spd_ad)
motor.motor1(status, forward, b_spd*spd_ad)
#print('LLL')
continue
elif 'BLe' in data: #When server receive "BLeft" from client,car move back and left
tcpCliSock.send('3')
motor.motor(status, 1, left*spd_ad)
motor.motor1(status, 1, b_spd*spd_ad)
#print("BL")
continue
elif 'Right' in data: #When server receive "Right" from client,car turns right
tcpCliSock.send('4')
motor.motor(status, forward, b_spd*spd_ad)
motor.motor1(status, forward, right*spd_ad)
continue
elif 'BRi' in data: #When server receive "BRight" from client,car move back and right
tcpCliSock.send('4')
motor.motor(status, backward, b_spd*spd_ad)
motor.motor1(status, backward, right*spd_ad)
continue
elif 'exit' in data: #When server receive "exit" from client,server shuts down
GPIO.cleanup()
tcpSerSock.close()
os.system('sudo init 0')
continue
elif 'stop' in data: #When server receive "stop" from client,car stops moving
tcpCliSock.send('9')
#setup()
motor.motorStop()
#GPIO.cleanup()
#setup()
continue
elif 'home' in data: #When server receive "home" from client,camera looks forward
car_dir.dir_home(pwm0)
car_dir.dir_home(pwm1)
continue
elif 'Stop' in data: #When server receive "Stop" from client,Auto Mode switches off
tcpCliSock.send('9')
try:
st.stop()
except:
pass
motor.motorStop()
elif 'auto' in data: #When server receive "auto" from client,start Auto Mode
tcpCliSock.send('0')
st = Job()
st.start()
continue
elif 'IPCON' in data:
try:
data=str(data)
ip_var=data.split()
ip_con=ip_var[1]
print(ip_con)
vi=threading.Thread(target=video_net) #Define a thread for data receiving
vi.setDaemon(True) #'True' means it is a front thread,it would close when the mainloop() closes
vi.start() #Thread starts
print('start thread')
except:
pass
else:
print 'Command Error! Cannot recongnize command: ' +data
def run(): #Main function
global ip_con, addr
st_s = 0
while True:
#print('SET %s'%dir_mid+' %s'%dis_mid+' %s'%b_spd+' %s'%t_spd+' %s'%left+' %s'%right)
print('waiting for connection...')
tcpCliSock, addr = tcpSerSock.accept() #Determine whether to connect
print('...connected from :', addr)
tcpCliSock.send(
('SET %s' % dir_mid + ' %s' % dis_mid + ' %s' % b_spd +
' %s' % t_spd + ' %s' % left + ' %s' % right).encode())
break
fps_threading = threading.Thread(
target=FPV_thread) #Define a thread for FPV and OpenCV
fps_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
fps_threading.start() #Thread starts
st = Job()
while True:
data = ''
data = tcpCliSock.recv(BUFSIZ).decode() #Get instructions
#print(data)
if not data:
continue
elif 'spdset' in data:
global spd_ad
try:
spd_ad = float((str(data))[7:]) #Speed Adjustment
except:
print('wrong speed value')
elif 'EC1set' in data: #Camera Adjustment
try:
new_EC1 = int((str(data))[7:])
replace_num('E_C1:', new_EC1)
except:
pass
elif 'EC2set' in data: #Ultrasonic Adjustment
try:
new_EC2 = int((str(data))[7:])
replace_num('E_C2:', new_EC2)
except:
pass
elif 'EM1set' in data: #Motor A Speed Adjustment
try:
new_EM1 = int((str(data))[7:])
replace_num('E_M1:', new_EM1)
except:
pass
elif 'EM2set' in data: #Motor B Speed Adjustment
try:
new_EM2 = int((str(data))[7:])
replace_num('E_M2:', new_EM2)
except:
pass
elif 'ET1set' in data: #Motor A Turningf Speed Adjustment
try:
new_ET1 = int((str(data))[7:])
replace_num('E_T1:', new_ET1)
except:
pass
elif 'ET2set' in data: #Motor B Turningf Speed Adjustment
try:
new_ET2 = int((str(data))[7:])
replace_num('E_T2:', new_ET2)
except:
pass
elif 'scan' in data:
dis_can = scan() #Start Scanning
str_list_1 = dis_can #Divide the list to make it samller to send
str_index = ' ' #Separate the values by space
str_send_1 = str_index.join(str_list_1) + ' '
tcpCliSock.sendall((str(str_send_1)).encode()) #Send Data
tcpCliSock.send(
'finished'.encode()
) #Sending 'finished' tell the client to stop receiving the list of dis_can
elif 'forward' in data: #When server receive "forward" from client,car moves forward
tcpCliSock.send('1'.encode())
motor.motor(status, forward, b_spd * spd_ad)
motor.motor1(status, forward, t_spd * spd_ad)
direction = forward
elif 'backward' in data: #When server receive "backward" from client,car moves backward
tcpCliSock.send('2'.encode())
motor.motor(status, backward, b_spd * spd_ad)
motor.motor1(status, backward, t_spd * spd_ad)
direction = backward
elif 'left' in data: #When server receive "left" from client,camera turns left
tcpCliSock.send('7'.encode())
car_dir.dir_left(pwm1)
#car_dir.dir_right(pwm1)
continue
elif 'right' in data: #When server receive "right" from client,camera turns right
tcpCliSock.send('8'.encode())
car_dir.dir_right(pwm1)
#car_dir.dir_left(pwm1)
continue
elif 'on' in data: #When server receive "on" from client,camera looks up
tcpCliSock.send('5'.encode())
car_dir.dir_Left(pwm0)
#car_dir.dir_Right(pwm0)
continue
elif 'under' in data: #When server receive "under" from client,camera looks down
tcpCliSock.send('6'.encode())
car_dir.dir_Right(pwm0)
#car_dir.dir_Left(pwm0)
continue
elif 'Left' in data: #When server receive "Left" from client,car turns left
tcpCliSock.send('3'.encode())
motor.motor(0, forward, left * spd_ad)
motor.motor1(status, forward, b_spd * spd_ad)
#print('LLL')
continue
elif 'BLe' in data: #When server receive "BLeft" from client,car move back and left
tcpCliSock.send('3'.encode())
motor.motor(0, backward, left * spd_ad)
motor.motor1(status, backward, b_spd * spd_ad)
#print("BL")
continue
elif 'Right' in data: #When server receive "Right" from client,car turns right
tcpCliSock.send('4'.encode())
motor.motor(status, forward, b_spd * spd_ad)
motor.motor1(0, forward, right * spd_ad)
continue
elif 'BRi' in data: #When server receive "BRight" from client,car move back and right
tcpCliSock.send('4'.encode())
motor.motor(status, backward, b_spd * spd_ad)
motor.motor1(0, backward, right * spd_ad)
continue
elif 'exit' in data: #When server receive "exit" from client,server shuts down
GPIO.cleanup()
tcpSerSock.close()
os.system('sudo init 0')
continue
elif 'stop' in data: #When server receive "stop" from client,car stops moving
tcpCliSock.send('9'.encode())
#setup()
motor.motorStop()
#GPIO.cleanup()
#setup()
continue
elif 'home' in data: #When server receive "home" from client,camera looks forward
car_dir.dir_home(pwm0)
car_dir.dir_home(pwm1)
continue
elif 'Stop' in data: #When server receive "Stop" from client,Auto Mode switches off
tcpCliSock.send('9'.encode())
try:
st.pause()
except:
pass
motor.motorStop()
time.sleep(0.4)
motor.motorStop()
elif 'auto' in data: #When server receive "auto" from client,start Auto Mode
tcpCliSock.send('0'.encode())
if st_s == 0:
st.start()
st_s = 1
else:
st.resume()
continue
else:
pass
def loop(): while True: while True: car_dir.dis_home(pwm1) car_dir.dir_home(pwm0) time.sleep(0.5) homedis = ultrasonic.checkdist() print 'homedis = %0.2f m' %homedis motor.motorStop() if homedis > dis: motor.motor(status, forward, f_spd) break elif homedis < dis: car_dir.dis_left(pwm1) time.sleep(0.5) leftdis = ultrasonic.checkdist() print 'leftdis = %0.2f m' %leftdis car_dir.dis_right(pwm1) time.sleep(0.5) rightdis = ultrasonic.checkdist() print 'rightdis = %0.2f m' %rightdis if leftdis < dis and rightdis < dis: if leftdis >= rightdis: motor.motor(status, backward, b_spd) time.sleep(1) car_dir.dir_left(pwm0) motor.motor(status, backward, b_spd) time.sleep(1) break else: motor.motor(status, backward, b_spd) time.sleep(1) car_dir.dir_right(pwm0) motor.motor(status, backward, b_spd) time.sleep(1) break elif leftdis > dis and rightdis <= dis: if homedis < mindis: motor.motor(status, backward, b_spd) time.sleep(1) car_dir.dir_left(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break else: car_dir.dir_left(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break elif rightdis > dis and leftdis <= dis: if homedis < mindis: car_dir.dir_left(pwm0) motor.motor(status, backward, b_spd) time.sleep(1) car_dir.dir_right(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break else: car_dir.dir_right(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break elif rightdis > dis and leftdis > dis: if rightdis > leftdis: if homedis < mindis: motor.motor(status, backward, b_spd) time.sleep(1) car_dir.dir_right(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break else: car_dir.dir_right(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break elif rightdis < leftdis: if homedis < mindis: motor.motor(status, backward, b_spd) time.sleep(1) car_dir.dir_left(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break else: car_dir.dir_left(pwm0) motor.motor(status, forward, f_spd) time.sleep(1.5) break elif leftdis == rightdis: motor.motor(status, backward, b_spd) time.sleep(1) break