def opencv_thread(): #OpenCV and FPV video
global hoz_mid_orig, vtr_mid_orig
font = cv2.FONT_HERSHEY_SIMPLEX
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
image = frame.array
cv2.line(image, (300, 240), (340, 240), (128, 255, 128), 1)
cv2.line(image, (320, 220), (320, 260), (128, 255, 128), 1)
if opencv_mode == 1:
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, colorLower, colorUpper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
if len(cnts) > 0:
led.both_off()
led.green()
cv2.putText(image, 'Target Detected', (40, 60), font, 0.5,
(255, 255, 255), 1, cv2.LINE_AA)
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
X = int(x)
Y = int(y)
if radius > 10:
cv2.rectangle(image, (int(x - radius), int(y + radius)),
(int(x + radius), int(y - radius)),
(255, 255, 255), 1)
if X < 310:
mu1 = int((320 - X) / 3)
hoz_mid_orig += mu1
if hoz_mid_orig < look_left_max:
pass
else:
hoz_mid_orig = look_left_max
ultra_turn(hoz_mid_orig)
#print('x=%d'%X)
elif X > 330:
mu1 = int((X - 330) / 3)
hoz_mid_orig -= mu1
if hoz_mid_orig > look_right_max:
pass
else:
hoz_mid_orig = look_right_max
ultra_turn(hoz_mid_orig)
#print('x=%d'%X)
else:
turn.middle()
pass
mu_t = 390 - (hoz_mid - hoz_mid_orig)
v_mu_t = 390 + (hoz_mid + hoz_mid_orig)
turn.turn_ang(mu_t)
dis = dis_data
if dis < (distance_stay - 0.1):
led.both_off()
led.red()
turn.turn_ang(mu_t)
motor.motor_left(status, backward, left_spd * spd_ad_u)
motor.motor_right(status, forward,
right_spd * spd_ad_u)
cv2.putText(image, 'Too Close', (40, 80), font, 0.5,
(128, 128, 255), 1, cv2.LINE_AA)
elif dis > (distance_stay + 0.1):
motor.motor_left(status, forward, left_spd * spd_ad_2)
motor.motor_right(status, backward,
right_spd * spd_ad_2)
cv2.putText(image, 'OpenCV Tracking', (40, 80), font,
0.5, (128, 255, 128), 1, cv2.LINE_AA)
else:
motor.motorStop()
led.both_off()
led.blue()
cv2.putText(image, 'In Position', (40, 80), font, 0.5,
(255, 128, 128), 1, cv2.LINE_AA)
if dis < 8:
cv2.putText(image, '%s m' % str(round(dis, 2)),
(40, 40), font, 0.5, (255, 255, 255), 1,
cv2.LINE_AA)
if Y < 230:
mu2 = int((240 - Y) / 5)
vtr_mid_orig += mu2
if vtr_mid_orig < look_up_max:
pass
else:
vtr_mid_orig = look_up_max
camera_turn(vtr_mid_orig)
elif Y > 250:
mu2 = int((Y - 240) / 5)
vtr_mid_orig -= mu2
if vtr_mid_orig > look_down_max:
pass
else:
vtr_mid_orig = look_down_max
camera_turn(vtr_mid_orig)
if X > 280:
if X < 350:
#print('looked')
cv2.line(image, (300, 240), (340, 240),
(64, 64, 255), 1)
cv2.line(image, (320, 220), (320, 260),
(64, 64, 255), 1)
cv2.rectangle(image,
(int(x - radius), int(y + radius)),
(int(x + radius), int(y - radius)),
(64, 64, 255), 1)
else:
led.both_off()
led.yellow()
cv2.putText(image, 'Target Detecting', (40, 60), font, 0.5,
(255, 255, 255), 1, cv2.LINE_AA)
led_y = 1
motor.motorStop()
for i in range(1, len(pts)):
if pts[i - 1] is None or pts[i] is None:
continue
thickness = int(np.sqrt(args["buffer"] / float(i + 1)) * 2.5)
cv2.line(image, pts[i - 1], pts[i], (0, 0, 255), thickness)
else:
dis = dis_data
if dis < 8:
cv2.putText(image, '%s m' % str(round(dis, 2)), (40, 40), font,
0.5, (255, 255, 255), 1, cv2.LINE_AA)
encoded, buffer = cv2.imencode('.jpg', image)
jpg_as_text = base64.b64encode(buffer)
footage_socket.send(jpg_as_text)
rawCapture.truncate(0)
def run():
global v_command
# obtain audio from the microphone
r = sr.Recognizer()
with sr.Microphone(device_index =2,sample_rate=48000) as source:
r.record(source,duration=2)
#r.adjust_for_ambient_noise(source)
led.both_off()
led.yellow()
print("Command?")
audio = r.listen(source)
led.both_off()
led.blue()
try:
v_command = r.recognize_sphinx(audio,
keyword_entries=[('forward',1.0),('backward',1.0),
('left',1.0),('right',1.0),('stop',1.0)]) #You can add your own command here
print(v_command)
led.both_off()
led.cyan()
except sr.UnknownValueError:
print("say again")
led.both_off()
led.red()
except sr.RequestError as e:
led.both_off()
led.red()
pass
#print('pre')
if 'forward' in v_command:
motor.motor_left(status, forward,left_spd*spd_ad_2)
motor.motor_right(status,backward,right_spd*spd_ad_2)
time.sleep(2)
motor.motorStop()
elif 'backward' in v_command:
motor.motor_left(status, backward,left_spd)
motor.motor_right(status,forward,right_spd)
time.sleep(2)
motor.motorStop()
elif 'left' in v_command:
turn.left()
motor.motor_left(status, forward,left_spd*spd_ad_2)
motor.motor_right(status,backward,right_spd*spd_ad_2)
time.sleep(2)
motor.motorStop()
elif "right" in v_command:
turn.right()
motor.motor_left(status, forward,left_spd*spd_ad_2)
motor.motor_right(status,backward,right_spd*spd_ad_2)
time.sleep(2)
motor.motorStop()
elif 'stop' in v_command:
motor.motorStop()
else:
pass
def loop():
car_dir.dis_home(pwm1) #Ultrasound initial position
time.sleep(0.5)
a = ultrasonic.checkdist() #Get the ultrasonic detection distance
b = ultrasonic.checkdist() #Get the ultrasonic detection distance
c = ultrasonic.checkdist() #Get the ultrasonic detection distance
homedis = min(a, b, c) #Get the ultrasonic detection distance
print('homedis = %0.2f m' % homedis)
motor.motorStop() #Stop the car
if homedis > dis: #No obstacles
motor.motor(status, forward, b_spd)
motor.motor1(status, forward, t_spd)
elif homedis < dis: #Obstacles
car_dir.dis_left(pwm1)
time.sleep(1)
a = ultrasonic.checkdist()
b = ultrasonic.checkdist()
c = ultrasonic.checkdist()
leftdis = min(a, b, c)
print('leftdis = %0.2f m' % leftdis)
car_dir.dis_right(pwm1)
time.sleep(1)
a = ultrasonic.checkdist()
b = ultrasonic.checkdist()
c = ultrasonic.checkdist()
rightdis = min(a, b, c)
print('rightdis = %0.2f m' % rightdis)
if leftdis < dis and rightdis < dis: #Judgment left and right
if leftdis >= rightdis: #There are obstacles on the right
motor.motor(status, backward, b_spd)
#motor.motor(status,backward,left)
#motor.motor1(status,backward,t_spd)
motor.motor1(status, backward, right)
time.sleep(1)
motor.motor(status, forward, left)
motor.motor1(status, forward, b_spd)
time.sleep(0.5)
else: #There are obstacles on the left
motor.motor(status, backward, left)
motor.motor1(status, backward, t_spd)
time.sleep(1)
motor.motor(status, forward, b_spd)
motor.motor1(status, forward, right)
time.sleep(0.5)
elif leftdis > dis and rightdis <= dis: #There are obstacles on the right
if homedis < mindis: #Obstacle ahead
motor.motor(status, backward, b_spd)
motor.motor1(status, backward, t_spd)
time.sleep(1)
motor.motor(status, forward, left)
motor.motor1(status, forward, b_spd)
time.sleep(0.5)
else: #No obstacle ahead
motor.motor(status, forward, left)
motor.motor1(status, forward, b_spd)
time.sleep(0.5)
elif rightdis > dis and leftdis <= dis: #There are obstacles on the left
if homedis < mindis: #Obstacle ahead
motor.motor(status, backward, b_spd)
motor.motor1(status, backward, t_spd)
time.sleep(1)
motor.motor(status, forward, b_spd)
motor.motor1(status, forward, right)
time.sleep(0.5)
else: #No obstacle ahead
motor.motor(status, forward, b_spd)
motor.motor1(status, forward, right)
time.sleep(0.5)
elif rightdis > dis and leftdis > dis: #There are no obstacles
if rightdis > leftdis: #The distance to the right is greater than the left
if homedis < mindis: #Obstacle ahead
motor.motor(status, backward, b_spd)
motor.motor1(status, backward, t_spd)
time.sleep(1)
motor.motor(status, forward, b_spd)
motor.motor1(status, forward, right)
time.sleep(0.5)
else: #No obstacle ahead
motor.motor(status, forward, b_spd)
motor.motor1(status, forward, right)
time.sleep(0.5)
elif rightdis < leftdis: #The distance to the left is greater than the right
if homedis < mindis: #Obstacle ahead
motor.motor(status, backward, b_spd)
motor.motor1(status, backward, t_spd)
time.sleep(1)
motor.motor(status, forward, left)
motor.motor1(status, forward, b_spd)
time.sleep(0.5)
else: #NO Obstacle ahead
motor.motor(status, forward, left)
motor.motor1(status, forward, b_spd)
time.sleep(0.5)
elif leftdis == rightdis:
motor.motor(status, backward, b_spd)
motor.motor1(status, backward, t_spd)
time.sleep(1)
def run(): #Main loop
global hoz_mid, vtr_mid, ip_con, led_status, auto_status, opencv_mode, findline_mode, speech_mode, auto_mode, data, addr, footage_socket, ap_status, turn_status, wifi_status
led.setup()
while True: #Connection
print('waiting for connection...')
led.red()
tcpCliSock, addr = tcpSerSock.accept() #Determine whether to connect
led.both_off()
led.green()
print('...connected from :', addr)
#time.sleep(1)
tcpCliSock.send(('SET %s' % vtr_mid + ' %s' % hoz_mid +
' %s' % left_spd + ' %s' % right_spd +
' %s' % look_up_max + ' %s' % look_down_max).encode())
print('SET %s' % vtr_mid + ' %s' % hoz_mid + ' %s' % left_spd +
' %s' % right_spd + ' %s' % left + ' %s' % right)
break
#Threads start
findline_threading = threading.Thread(
target=findline_thread) #Define a thread for line tracking
findline_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
findline_threading.start() #Thread starts
auto_threading = threading.Thread(
target=auto_thread) #Define a thread for ultrasonic tracking
auto_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
auto_threading.start() #Thread starts
time.sleep(0.5)
tcpCliSock.send('TestVersion'.encode())
while True:
data = ''
data = tcpCliSock.recv(BUFSIZ).decode()
if not data:
continue
elif 'exit' in data:
pass
elif 'spdset' in data:
global spd_ad
spd_ad = float((str(data))[7:]) #Speed Adjustment
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 'scan_rev' in data:
dis_can = scan_rev() #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 'EC1set' in data: #Camera Adjustment
new_EC1 = int((str(data))[7:])
turn.camera_turn(new_EC1)
replace_num('E_C1:', new_EC1)
elif 'EC2set' in data: #Ultrasonic Adjustment
new_EC2 = int((str(data))[7:])
replace_num('E_C2:', new_EC2)
turn.ultra_turn(new_EC2)
elif 'EM1set' in data: #Motor A Speed Adjustment
new_EM1 = int((str(data))[7:])
replace_num('E_M1:', new_EM1)
elif 'EM2set' in data: #Motor B Speed Adjustment
new_EM2 = int((str(data))[7:])
replace_num('E_M2:', new_EM2)
elif 'LUMset' in data: #Motor A Turningf Speed Adjustment
new_ET1 = int((str(data))[7:])
replace_num('look_up_max:', new_ET1)
turn.camera_turn(new_ET1)
elif 'LDMset' in data: #Motor B Turningf Speed Adjustment
new_ET2 = int((str(data))[7:])
replace_num('look_down_max:', new_ET2)
turn.camera_turn(new_ET2)
elif 'stop' in data: #When server receive "stop" from client,car stops moving
tcpCliSock.send('9'.encode())
setup()
motor.motorStop()
setup()
if led_status == 0:
led.setup()
led.both_off()
continue
elif 'lightsON' in data: #Turn on the LEDs
led.both_on()
led_status = 1
tcpCliSock.send('lightsON'.encode())
elif 'lightsOFF' in data: #Turn off the LEDs
led.both_off()
led_status = 0
tcpCliSock.send('lightsOFF'.encode())
elif 'middle' in data: #Go straight
if led_status == 0:
led.side_color_off(left_R, left_G)
led.side_color_off(right_R, right_G)
else:
led.side_on(left_B)
led.side_on(right_B)
turn_status = 0
turn.middle()
elif 'Left' in data: #Turn left
if led_status == 0:
led.side_color_on(left_R, left_G)
else:
led.side_off(left_B)
turn.left()
tcpCliSock.send('3'.encode())
elif 'Right' in data: #Turn right
if led_status == 0:
led.side_color_on(right_R, right_G)
else:
led.side_off(right_B)
turn.right()
tcpCliSock.send('4'.encode())
elif 'backward' in data: #When server receive "backward" from client,car moves backward
tcpCliSock.send('2'.encode())
motor.motor_left(status, backward, left_spd * spd_ad)
motor.motor_right(status, forward, right_spd * spd_ad)
elif 'forward' in data: #When server receive "forward" from client,car moves forward
tcpCliSock.send('1'.encode())
motor.motor_left(status, forward, left_spd * spd_ad)
motor.motor_right(status, backward, right_spd * spd_ad)
elif 'l_up' in data: #Camera look up
if vtr_mid < look_up_max:
vtr_mid += turn_speed
turn.camera_turn(vtr_mid)
tcpCliSock.send('5'.encode())
elif 'l_do' in data: #Camera look down
if vtr_mid > look_down_max:
vtr_mid -= turn_speed
turn.camera_turn(vtr_mid)
tcpCliSock.send('6'.encode())
elif 'l_le' in data: #Camera look left
if hoz_mid < look_left_max:
hoz_mid += turn_speed
turn.ultra_turn(hoz_mid)
tcpCliSock.send('7'.encode())
elif 'l_ri' in data: #Camera look right
if hoz_mid > look_right_max:
hoz_mid -= turn_speed
turn.ultra_turn(hoz_mid)
tcpCliSock.send('8'.encode())
elif 'ahead' in data: #Camera look ahead
turn.ahead()
elif 'Stop' in data: #When server receive "Stop" from client,Auto Mode switches off
findline_mode = 0
auto_mode = 0
auto_status = 0
tcpCliSock.send('auto_status_off'.encode())
motor.motorStop()
led.both_off()
turn.middle()
time.sleep(0.1)
motor.motorStop()
led.both_off()
turn.middle()
elif 'auto' in data: #When server receive "auto" from client,start Auto Mode
if auto_status == 0:
tcpCliSock.send('0'.encode())
auto_status = 1
auto_mode = 1
dis_scan = 0
else:
pass
continue
elif 'findline' in data: #Find line mode start
if auto_status == 0:
tcpCliSock.send('findline'.encode())
auto_status = 1
findline_mode = 1
else:
pass
continue
elif 'backward' in v_command:
motor.motor_left(status, backward,left_spd)
motor.motor_right(status,forward,right_spd)
time.sleep(2)
motor.motorStop()
elif 'left' in v_command:
turn.left()
motor.motor_left(status, forward,left_spd*spd_ad_2)
motor.motor_right(status,backward,right_spd*spd_ad_2)
time.sleep(2)
motor.motorStop()
elif "right" in v_command:
turn.right()
motor.motor_left(status, forward,left_spd*spd_ad_2)
motor.motor_right(status,backward,right_spd*spd_ad_2)
time.sleep(2)
motor.motorStop()
elif 'stop' in v_command:
motor.motorStop()
else:
pass
try:
pass
except KeyboardInterrupt:
motor.motorStop()
def stopWhenObstacle():
m.motorStart(1, m.Dir_forward, 80)
while 1:
if u.checkdist() < dist:
m.motorStop()
break
def run(): #Main loop
global hoz_mid, vtr_mid, ip_con, led_status, auto_status, opencv_mode, findline_mode, speech_mode, auto_mode, data, addr, footage_socket, ap_status, turn_status, wifi_status
led.setup()
while True: #Connection
try:
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.connect(("1.1.1.1", 80))
ipaddr_check = s.getsockname()[0]
s.close()
print(ipaddr_check)
wifi_status = 1
except:
if ap_status == 0:
ap_threading = threading.Thread(
target=ap_thread) #Define a thread for data receiving
ap_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
ap_threading.start() #Thread starts
led.both_off()
led.yellow()
time.sleep(5)
wifi_status = 0
if wifi_status == 1:
print('waiting for connection...')
led.red()
tcpCliSock, addr = tcpSerSock.accept(
) #Determine whether to connect
led.both_off()
led.green()
print('...connected from :', addr)
#time.sleep(1)
tcpCliSock.send(
('SET %s' % vtr_mid + ' %s' % hoz_mid + ' %s' % left_spd +
' %s' % right_spd + ' %s' % look_up_max +
' %s' % look_down_max).encode())
print('SET %s' % vtr_mid + ' %s' % hoz_mid + ' %s' % left_spd +
' %s' % right_spd + ' %s' % left + ' %s' % right)
break
else:
led.both_off()
led.blue()
print('waiting for connection...')
tcpCliSock, addr = tcpSerSock.accept(
) #Determine whether to connect
led.both_off()
led.green()
print('...connected from :', addr)
#time.sleep(1)
tcpCliSock.send(
('SET %s' % vtr_mid + ' %s' % hoz_mid + ' %s' % left_spd +
' %s' % right_spd + ' %s' % look_up_max +
' %s' % look_down_max).encode())
print('SET %s' % vtr_mid + ' %s' % hoz_mid + ' %s' % left_spd +
' %s' % right_spd + ' %s' % left + ' %s' % right)
ap_status = 1
break
#FPV initialization
context = zmq.Context()
footage_socket = context.socket(zmq.PUB)
footage_socket.connect('tcp://%s:5555' % addr[0])
print(addr[0])
#Threads start
video_threading = threading.Thread(
target=opencv_thread) #Define a thread for FPV and OpenCV
video_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
video_threading.start() #Thread starts
ws2812_threading = threading.Thread(
target=ws2812_thread) #Define a thread for ws_2812 leds
ws2812_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
ws2812_threading.start() #Thread starts
findline_threading = threading.Thread(
target=findline_thread) #Define a thread for line tracking
findline_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
findline_threading.start() #Thread starts
speech_threading = threading.Thread(
target=speech_thread) #Define a thread for speech recognition
speech_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
speech_threading.start() #Thread starts
auto_threading = threading.Thread(
target=auto_thread) #Define a thread for ultrasonic tracking
auto_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
auto_threading.start() #Thread starts
scan_threading = threading.Thread(
target=dis_scan_thread) #Define a thread for ultrasonic scan
scan_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
scan_threading.start() #Thread starts
while True:
data = ''
data = tcpCliSock.recv(BUFSIZ).decode()
if not data:
continue
elif 'exit' in data:
os.system("sudo shutdown -h now\n")
elif 'spdset' in data:
global spd_ad
spd_ad = float((str(data))[7:]) #Speed Adjustment
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 'scan_rev' in data:
dis_can = scan_rev() #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 'EC1set' in data: #Camera Adjustment
new_EC1 = int((str(data))[7:])
turn.camera_turn(new_EC1)
replace_num('E_C1:', new_EC1)
elif 'EC2set' in data: #Ultrasonic Adjustment
new_EC2 = int((str(data))[7:])
replace_num('E_C2:', new_EC2)
turn.ultra_turn(new_EC2)
elif 'EM1set' in data: #Motor A Speed Adjustment
new_EM1 = int((str(data))[7:])
replace_num('E_M1:', new_EM1)
elif 'EM2set' in data: #Motor B Speed Adjustment
new_EM2 = int((str(data))[7:])
replace_num('E_M2:', new_EM2)
elif 'LUMset' in data: #Motor A Turningf Speed Adjustment
new_ET1 = int((str(data))[7:])
replace_num('look_up_max:', new_ET1)
turn.camera_turn(new_ET1)
elif 'LDMset' in data: #Motor B Turningf Speed Adjustment
new_ET2 = int((str(data))[7:])
replace_num('look_down_max:', new_ET2)
turn.camera_turn(new_ET2)
elif 'stop' in data: #When server receive "stop" from client,car stops moving
tcpCliSock.send('9'.encode())
setup()
motor.motorStop()
setup()
if led_status == 0:
led.setup()
led.both_off()
colorWipe(strip, Color(0, 0, 0))
continue
elif 'lightsON' in data: #Turn on the LEDs
led.both_on()
led_status = 1
tcpCliSock.send('lightsON'.encode())
elif 'lightsOFF' in data: #Turn off the LEDs
led.both_off()
led_status = 0
tcpCliSock.send('lightsOFF'.encode())
elif 'middle' in data: #Go straight
if led_status == 0:
led.side_color_off(left_R, left_G)
led.side_color_off(right_R, right_G)
else:
led.side_on(left_B)
led.side_on(right_B)
turn_status = 0
turn.middle()
elif 'Left' in data: #Turn left
if led_status == 0:
led.side_color_on(left_R, left_G)
else:
led.side_off(left_B)
turn.left()
turn_status = 1
tcpCliSock.send('3'.encode())
elif 'Right' in data: #Turn right
if led_status == 0:
led.side_color_on(right_R, right_G)
else:
led.side_off(right_B)
turn.right()
turn_status = 2
tcpCliSock.send('4'.encode())
elif 'backward' in data: #When server receive "backward" from client,car moves backward
tcpCliSock.send('2'.encode())
motor.motor_left(status, backward, left_spd * spd_ad)
motor.motor_right(status, forward, right_spd * spd_ad)
colorWipe(strip, Color(255, 0, 0))
elif 'forward' in data: #When server receive "forward" from client,car moves forward
tcpCliSock.send('1'.encode())
motor.motor_left(status, forward, left_spd * spd_ad)
motor.motor_right(status, backward, right_spd * spd_ad)
colorWipe(strip, Color(0, 0, 255))
elif 'l_up' in data: #Camera look up
if vtr_mid < look_up_max:
vtr_mid += turn_speed
turn.camera_turn(vtr_mid)
tcpCliSock.send('5'.encode())
elif 'l_do' in data: #Camera look down
if vtr_mid > look_down_max:
vtr_mid -= turn_speed
turn.camera_turn(vtr_mid)
print(vtr_mid)
tcpCliSock.send('6'.encode())
elif 'l_le' in data: #Camera look left
if hoz_mid < look_left_max:
hoz_mid += turn_speed
turn.ultra_turn(hoz_mid)
tcpCliSock.send('7'.encode())
elif 'l_ri' in data: #Camera look right
if hoz_mid > look_right_max:
hoz_mid -= turn_speed
turn.ultra_turn(hoz_mid)
tcpCliSock.send('8'.encode())
elif 'ahead' in data: #Camera look ahead
turn.ahead()
elif 'Stop' in data: #When server receive "Stop" from client,Auto Mode switches off
opencv_mode = 0
findline_mode = 0
speech_mode = 0
auto_mode = 0
auto_status = 0
dis_scan = 1
tcpCliSock.send('auto_status_off'.encode())
motor.motorStop()
led.both_off()
turn.middle()
time.sleep(0.1)
motor.motorStop()
led.both_off()
turn.middle()
elif 'auto' in data: #When server receive "auto" from client,start Auto Mode
if auto_status == 0:
tcpCliSock.send('0'.encode())
auto_status = 1
auto_mode = 1
dis_scan = 0
else:
pass
continue
elif 'opencv' in data: #When server receive "auto" from client,start Auto Mode
if auto_status == 0:
auto_status = 1
opencv_mode = 1
tcpCliSock.send('oncvon'.encode())
continue
elif 'findline' in data: #Find line mode start
if auto_status == 0:
tcpCliSock.send('findline'.encode())
auto_status = 1
findline_mode = 1
else:
pass
continue
elif 'voice_3' in data: #Speech recognition mode start
if auto_status == 0:
auto_status = 1
speech_mode = 1
tcpCliSock.send('voice_3'.encode())
else:
pass
continue
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 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
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(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'.encode())
motor.motor(status, 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(status, 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(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'.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 mainLoop(socket):
global led_status, auto_status, opencv_mode, findline_mode, speech_mode, \
auto_mode, command, ap_status, turn_status, blinkThreadStatus
BUFSIZ = 1024 #Define buffer size
while True:
command = socket.recv(BUFSIZ).decode()
print("mainLoop: received %s" % command)
if not command:
continue
# strip end-of-cmd off
if END_OF_CMD in command:
command = command[:-len(END_OF_CMD)]
# TODO: add code to handle receiving multiple commands delimited by end-of-cmd
if SOUND in command:
sounds.playSound(command[len(SOUND):])
elif SPEAK in command:
speak.say(command[len(SPEAK):])
elif VOLUME in command:
speak.changeVolume(command[len(VOLUME):])
elif RATE in command:
speak.changeRate(command[len(RATE):])
elif SHAKE in command:
servos.shakeHead()
elif NOD in command:
servos.nodHead()
elif 'exit' in command:
os.system("sudo shutdown -h now\n")
elif 'quit' in command:
print('shutting down')
return
elif 'spdset' in command:
global spd_adj
spd_adj = float((str(command))[7:]) #Speed Adjustment
print("speed adjustment set to %s" % str(spd_adj))
elif 'scan' in command:
dis_can = servos.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) + ' '
socket.sendall((str(str_send_1)).encode()) # Send Data
socket.send(
'finished'.encode()
) # Sending 'finished' tell the client to stop receiving the list of dis_can
elif 'EC1set' in command: # pitch Adjustment
newValue = int((str(command))[7:])
config.exportConfig('pitch_middle', newValue)
servos.HEAD_PITCH_MIDDLE = newValue
servos.headPitch(newValue)
elif 'LDMset' in command: # look down max Adjustment
newValue = int((str(command))[7:])
config.exportConfig('look_down_max', newValue)
servos.HEAD_PITCH_DOWN_MAX = newValue
servos.headPitch(newValue)
elif 'LUMset' in command: # look up max Adjustment
newValue = int((str(command))[7:])
config.exportConfig('look_up_max', newValue)
servos.HEAD_PITCH_UP_MAX = newValue
servos.headPitch(newValue)
elif 'EC2set' in command: # yaw Adjustment
newValue = int((str(command))[7:])
config.exportConfig('yaw_middle', newValue)
servos.HEAD_YAW_MIDDLE = newValue
servos.headYaw(newValue)
elif 'LLMset' in command: # look left max Adjustment
newValue = int((str(command))[7:])
config.exportConfig('look_left_max', newValue)
servos.HEAD_YAW_LEFT_MAX = newValue
servos.headYaw(newValue)
elif 'LRMset' in command: # look right max Adjustment
newValue = int((str(command))[7:])
config.exportConfig('look_right_max', newValue)
servos.HEAD_YAW_RIGHT_MAX = newValue
servos.headYaw(newValue)
elif 'STEERINGset' in command: #Motor Steering center Adjustment
newValue = int((str(command))[12:])
config.exportConfig('turn_middle', newValue)
servos.STEERING_MIDDLE = newValue
servos.steerMiddle()
elif 'SLMset' in command: # Steering left max Adjustment
newValue = int((str(command))[7:])
config.exportConfig('turn_left_max', newValue)
servos.STEERING_LEFT_MAX = newValue
servos.steerFullLeft()
elif 'SRMset' in command: # Steering right max Adjustment
newValue = int((str(command))[7:])
config.exportConfig('turn_right_max', newValue)
servos.STEERING_RIGHT_MAX = newValue
servos.steerFullRight()
elif 'EM1set' in command: #Motor A Speed Adjustment
newValue = int((str(command))[7:])
config.exportConfig('E_M1', newValue)
elif 'EM2set' in command: #Motor B Speed Adjustment
newValue = int((str(command))[7:])
config.exportConfig('E_M2', newValue)
elif 'stop' in command: #When server receive "stop" from client,car stops moving
socket.send('9'.encode())
motor.motorStop()
#setup()
if led_status == 0:
headlights.turn(headlights.BOTH, headlights.OFF)
colorWipe(ledStrip, rpi_ws281x.Color(0, 0, 0))
elif 'lightsON' in command: #Turn on the LEDs
headlights.turn(headlights.BOTH, headlights.WHITE)
led_status = 1
socket.send('lightsON'.encode())
elif 'lightsOFF' in command: #Turn off the LEDs
headlights.turn(headlights.BOTH, headlights.OFF)
led_status = 0
socket.send('lightsOFF'.encode())
elif 'SteerLeft' in command: #Turn more to the left
headlights.turn(headlights.RIGHT, headlights.OFF)
headlights.turn(headlights.LEFT, headlights.YELLOW)
servos.turnSteering(servos.LEFT)
turn_status = LEFT_TURN
elif 'SteerRight' in command: #Turn more to the Right
headlights.turn(headlights.LEFT, headlights.OFF)
headlights.turn(headlights.RIGHT, headlights.YELLOW)
servos.turnSteering(servos.RIGHT)
turn_status = RIGHT_TURN
elif 'middle' in command: #Go straight
headlights.turn(headlights.BOTH, headlights.BLUE)
turn_status = NO_TURN
servos.steerMiddle()
elif 'Left' in command: #Turn hard left
headlights.turn(headlights.RIGHT, headlights.OFF)
headlights.turn(headlights.LEFT, headlights.YELLOW)
servos.steerFullLeft()
turn_status = LEFT_TURN
socket.send('3'.encode())
elif 'Right' in command: #Turn hard right
headlights.turn(headlights.LEFT, headlights.OFF)
headlights.turn(headlights.RIGHT, headlights.YELLOW)
servos.steerFullRight()
turn_status = RIGHT_TURN
socket.send('4'.encode())
elif 'backward' in command: #When server receive "backward" from client,car moves backward
socket.send('2'.encode())
motor.move(motor.BACKWARD, right_spd * spd_adj)
elif 'forward' in command: #When server receive "forward" from client,car moves forward
socket.send('1'.encode())
motor.move(motor.FORWARD, right_spd * spd_adj)
elif 'l_up' in command: #Camera look up
servos.changePitch(servos.UP)
socket.send('5'.encode())
elif 'l_do' in command: #Camera look down
servos.changePitch(servos.DOWN)
socket.send('6'.encode())
elif 'l_le' in command: #Camera look left
servos.changeYaw(servos.LEFT)
socket.send('7'.encode())
elif 'l_ri' in command: #Camera look right
servos.changeYaw(servos.RIGHT)
socket.send('8'.encode())
elif 'ahead' in command: #Camera look ahead
servos.lookAhead()
elif 'Off' in command: #When server receive "Off" from client, Auto Mode switches off
opencv_mode = 0
findline_mode = 0
speech_mode = 0
auto_mode = 0
auto_status = 0
time.sleep(.3) # wait for threads to detect the off state
dis_scan = 1
socket.send('auto_status_off'.encode())
motor.motorStop()
headlights.turn(headlights.BOTH, headlights.OFF)
servos.steerMiddle()
turn_status = NO_TURN
elif 'auto' in command: #When server receive "auto" from client,start Auto Mode
if auto_status == 0:
socket.send('0'.encode())
auto_status = 1
auto_mode = 1
dis_scan = 0
elif 'opencv' in command: #When server receive "auto" from client,start Auto Mode
if auto_status == 0:
auto_status = 1
opencv_mode = 1
socket.send('oncvon'.encode())
elif 'findline' in command: #Find line mode start
if auto_status == 0:
socket.send('findline'.encode())
auto_status = 1
findline_mode = 1
elif 'voice_3' in command: #Speech recognition mode start
if auto_status == 0:
auto_status = 1
speech_mode = 1
socket.send('voice_3'.encode())
def opencv_thread(): #OpenCV and FPV video
hoz_mid_orig = servos.yawPosition
vtr_mid_orig = servos.pitchPosition
font = cv2.FONT_HERSHEY_SIMPLEX
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
image = frame.array
cv2.line(image, (300, 240), (340, 240), (128, 255, 128), 1)
cv2.line(image, (320, 220), (320, 260), (128, 255, 128), 1)
if opencv_mode == 1:
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, colorLower, colorUpper)
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
if len(cnts) > 0:
headlights.turn(headlights.BOTH, headlights.GREEN)
cv2.putText(image, 'Target Detected', (40, 60), font, 0.5,
(255, 255, 255), 1, cv2.LINE_AA)
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
X = int(x)
Y = int(y)
if radius > 10:
cv2.rectangle(image, (int(x - radius), int(y + radius)),
(int(x + radius), int(y - radius)),
(255, 255, 255), 1)
if X < 310:
mu1 = int((320 - X) / 3)
hoz_mid_orig += mu1
hoz_mid_orig = servos.headYaw(hoz_mid_orig)
#print('x=%d'%X)
elif X > 330:
mu1 = int((X - 330) / 3)
hoz_mid_orig -= mu1
hoz_mid_orig = servos.headYaw(hoz_mid_orig)
#print('x=%d'%X)
else:
servos.steerMiddle()
mu_t = 390 - (servos.STEERING_MIDDLE - hoz_mid_orig)
servos.steer(mu_t)
dis = dis_data
if dis < (distance_stay - 0.1):
headlights.turn(headlights.BOTH, headlights.RED)
motor.move(motor.BACKWARD, right_spd * spd_ad_u)
cv2.putText(image, 'Too Close', (40, 80), font, 0.5,
(128, 128, 255), 1, cv2.LINE_AA)
elif dis > (distance_stay + 0.1):
motor.move(motor.FORWARD, right_spd * spd_ad_2)
cv2.putText(image, 'OpenCV Tracking', (40, 80), font,
0.5, (128, 255, 128), 1, cv2.LINE_AA)
else:
motor.motorStop()
headlights.turn(headlights.BOTH, headlights.BLUE)
cv2.putText(image, 'In Position', (40, 80), font, 0.5,
(255, 128, 128), 1, cv2.LINE_AA)
if dis < 8:
cv2.putText(image, '%s m' % str(round(dis, 2)),
(40, 40), font, 0.5, (255, 255, 255), 1,
cv2.LINE_AA)
if Y < 230:
mu2 = int((240 - Y) / 5)
vtr_mid_orig += mu2
vtr_mid_orig = servos.headPitch(vtr_mid_orig)
elif Y > 250:
mu2 = int((Y - 240) / 5)
svtr_mid_orig = servos.headPitch(vtr_mid_orig)
if X > 280:
if X < 350:
cv2.line(image, (300, 240), (340, 240),
(64, 64, 255), 1)
cv2.line(image, (320, 220), (320, 260),
(64, 64, 255), 1)
cv2.rectangle(image,
(int(x - radius), int(y + radius)),
(int(x + radius), int(y - radius)),
(64, 64, 255), 1)
else:
headlights.turn(headlights.BOTH, headlights.YELLOW)
cv2.putText(image, 'Target Detecting', (40, 60), font, 0.5,
(255, 255, 255), 1, cv2.LINE_AA)
led_y = 1
motor.motorStop()
for i in range(1, len(pts)):
if pts[i - 1] is None or pts[i] is None:
continue
thickness = int(np.sqrt(args["buffer"] / float(i + 1)) * 2.5)
cv2.line(image, pts[i - 1], pts[i], (0, 0, 255), thickness)
else:
dis = dis_data
if dis < 8:
cv2.putText(image, '%s m' % str(round(dis, 2)), (40, 40), font,
0.5, (255, 255, 255), 1, cv2.LINE_AA)
encoded, buffer = cv2.imencode('.jpg', image)
jpg_as_text = base64.b64encode(buffer)
footage_socket.send(jpg_as_text)
rawCapture.truncate(0)
