def run(self):
global hoz_mid, vtr_mid
while self.__running.isSet():
self.__flag.wait() # 为True时立即返回, 为False时阻塞直到内部的标识位为True后返回
if servo_command == 'lookleft':
if hoz_mid < look_left_max:
hoz_mid += turn_speed
turn.ultra_turn(hoz_mid)
elif servo_command == 'lookright':
if hoz_mid > look_right_max:
hoz_mid -= turn_speed
turn.ultra_turn(hoz_mid)
elif servo_command == 'up':
if vtr_mid < look_up_max:
vtr_mid += turn_speed
turn.camera_turn(vtr_mid)
elif servo_command == 'down':
if vtr_mid > look_down_max:
vtr_mid -= turn_speed
turn.camera_turn(vtr_mid)
time.sleep(0.07)
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 '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 loop'''
#??? Do these globals make sense
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: #??? only exits from breaks, seems wrong
#??? should this be if else rather than try except
#??? while not-connected
try: # try to ping to see if there is wifi
# checking self ip addr
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 not set up hotspot
if ap_status == 0: #??? it always is
#Define a thread for data receiving
ap_threading = threading.Thread(target=ap_thread)
ap_threading.setDaemon(True)
ap_threading.start()
led.both_off()
led.yellow()
time.sleep(5) #??? why sleep
wifi_status = 0 #??? seems unecessary
if wifi_status == 1:
print('waiting for connection...')
led.red()
#??? pause here and wait for connection?
tcpCliSock, addr = tcpSerSock.accept()
led.both_off()
led.green()
print('...connected from :', addr)
#time.sleep(1)
#TODO: change to format
#??? is this the best way to do this
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:
#??? now assuming ap-hotspot, but never explained
led.both_off()
led.blue()
#??? a lot of this is duplicated in the if statement
print('waiting for connection...')
# wait for connection
tcpCliSock, addr = tcpSerSock.accept()
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
#??? this is outside the while loop now
#FPV initialization
context = zmq.Context()
footage_socket = context.socket(zmq.PUB)
footage_socket.connect('tcp://%s:5555' % addr[0])
print(addr[0])
#??? why are we starting all these threads now
#Threads start
#Define a thread for FPV and OpenCV
video_threading = threading.Thread(target=opencv_thread)
video_threading.setDaemon(True)
video_threading.start()
#Define a thread for ws_2812 leds
ws2812_threading = threading.Thread(target=ws2812_thread)
ws2812_threading.setDaemon(True)
ws2812_threading.start()
#Define a thread for line tracking
findline_threading = threading.Thread(target=findline_thread)
findline_threading.setDaemon(True)
findline_threading.start()
#Define a thread for speech recognition
speech_threading = threading.Thread(target=speech_thread)
speech_threading.setDaemon(True)
speech_threading.start()
#Define a thread for ultrasonic tracking
auto_threading = threading.Thread(target=auto_thread)
auto_threading.setDaemon(True)
auto_threading.start()
#Define a thread for ultrasonic scan
scan_threading = threading.Thread(target=dis_scan_thread)
scan_threading.setDaemon(True)
scan_threading.start()
while True: #??? infinite loop
data = '' #??? why do we have to define early
data = tcpCliSock.recv(BUFSIZ).decode()
print(data)
#??? BUFSIZ is global
#??? this should be handled by a dictionary or interrupts
if not data:
continue
elif 'exit' in data:
quit()
#??? shutting down is a bad idea
#TODO: Make a shutdown function
#TODO: Include proper shutdowns
#os.system("sudo shutdown -h now\n")
elif 'spdset' in data:
#Speed Adjustment
global spd_ad
spd_ad = float((str(data))[7:])
elif 'scan' in data:
#Start Scanning
dis_can = scan()
#Divide the list to make it samller to send
str_list_1 = dis_can
#Separate the values by space
str_index = ' '
str_send_1 = str_index.join(str_list_1) + ' '
#??? this send is surprising
#Send Data
tcpCliSock.sendall((str(str_send_1)).encode())
#Sending 'finished'
#tell the client to stop receiving the list of dis_can
tcpCliSock.send('finished'.encode())
elif 'scan_rev' in data:
#Start Scanning
#??? what is scan_rev
#??? a lot of this is repeated
dis_can = scan_rev()
#Divide the list to make it samller to send
str_list_1 = dis_can
#Separate the values by space
str_index = ' '
str_send_1 = str_index.join(str_list_1) + ' '
#Send Data
tcpCliSock.sendall((str(str_send_1)).encode())
#Sending 'finished'
#tell the client to stop receiving the list of dis_can
tcpCliSock.send('finished'.encode())
#??? what are there names
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)
#TODO: fix the code for single motor
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)
#TODO: fix the code for single motor
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:
#??? what is the goal of stop
#??? what is this 9
tcpCliSock.send('9'.encode())
#??? why are we doing setup stop setup
setup()
motor.motorStop()
setup()
#??? why are we doing this
if led_status == 0:
led.setup()
led.both_off()
#??? what is the colorwipe
colorWipe(strip, Color(0, 0, 0))
continue
elif 'lightsON' in data:
# Turn on the LEDs
led.both_on()
#TODO statuses should be handled by a dictionary
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)
#TODO status handled better
turn_status = 0
turn.middle()
elif 'Left' in data:
# Turn left
#TODO: fix the capital case
if led_status == 0:
led.side_color_on(left_R, left_G)
else:
led.side_off(left_B)
turn.left()
turn_status = 1
#??? what do these numbers map too
#??? maybe use a dictionary
tcpCliSock.send('3'.encode())
elif 'Right' in data:
# Turn right
#TODO: fix the capital case
if led_status == 0:
#TODO: clarify variable names
led.side_color_on(right_R, right_G)
else:
led.side_off(right_B)
turn.right()
turn_status = 2
tcpCliSock.send('4'.encode())
#TODO: fix for single motor
elif 'backward' in data:
# Go backward
motor.motor_left(status, backward, left_spd * spd_ad)
motor.motor_right(status, forward, right_spd * spd_ad)
colorWipe(strip, Color(255, 0, 0))
tcpCliSock.send('2'.encode())
#TODO: fix for single motor
elif 'forward' in data:
# Go forward
motor.motor_left(status, forward, left_spd * spd_ad)
motor.motor_right(status, backward, right_spd * spd_ad)
colorWipe(strip, Color(0, 0, 255))
tcpCliSock.send('1'.encode())
#TODO: make better names
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: #TODO: fix capitalization
opencv_mode = 0
findline_mode = 0
speech_mode = 0
auto_mode = 0
auto_status = 0
dis_scan = 1 #??? why is this 1
tcpCliSock.send('auto_status_off'.encode())
motor.motorStop()
led.both_off()
turn.middle()
time.sleep(0.1) #??? why
#??? why do this twice
motor.motorStop()
led.both_off()
turn.middle()
elif 'auto' in data:
# start Auto Mode
#??? which automode is this
if auto_status == 0:
tcpCliSock.send('0'.encode())
auto_status = 1
auto_mode = 1 #?? dif from automode and autostatus
dis_scan = 0 #??? dis_scan
else:
pass
continue
elif 'opencv' in data:
# Start opencv mode
if auto_status == 0:
auto_status = 1
opencv_mode = 1
#??? what is this name
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
#??? why voice 3
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 opencv_thread():
'''OpenCV and FPV video'''
#??? why isn't opencv stuff in it's own module
#??? do we need these globals (maybe)
global hoz_mid_orig, vtr_mid_orig
font = cv2.FONT_HERSHEY_SIMPLEX #??? necessary?
#??? rawCapture is defined outside function but isn't global
for frame in camera.capture_continuous(
rawCapture,
format="bgr", #??? why bgr
use_video_port=True):
image = frame.array
# draw cross on image
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: #???
#TODO: make this a function
# searches for largest yellow object
#??? where are colorLower and colorUpper defined
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)
#??? how does findCountours work
cnts = cv2.findContours(
mask.copy(), #??? why use copy
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None #??? necessary
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) #??? line aa
#TODO: no single varible names
# c is the largest area contour
c = max(cnts, key=cv2.contourArea)
# location of moments
#TODO: make a function
((x, y), radius) = cv2.minEnclosingCircle(c)
X = int(x)
Y = int(y)
#TODO: make this a function
#??? what is this second attempt at finding center
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
if radius > 10: #TODO: make min size a setting
cv2.rectangle(
image, #??? why doesn't this pattern right
(int(x - radius), int(y + radius)),
(int(x + radius), int(y - radius)),
(255, 255, 255),
1)
#TODO: pan head is a function
#TODO: magic numbers in panning block
if X < 310:
mu1 = int((320 - X) / 3) #??? why 3 #??? what is mu1
hoz_mid_orig += mu1
if hoz_mid_orig < look_left_max:
pass #??? why is this not if > max
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 #??? why is this not if > max
else:
hoz_mid_orig = look_right_max
ultra_turn(hoz_mid_orig)
#print('x=%d'%X)
else:
turn.middle()
pass #??? why pass
#TODO: turn wheels as a function
#TODO: remove magic numbers
mu_t = 390 - (hoz_mid - hoz_mid_orig) #??? what is mu_t
v_mu_t = 390 + (hoz_mid + hoz_mid_orig
) #??? what is v_mu_t
turn.turn_ang(mu_t)
#TODO: seperate as a function
#??? why not use dis directly
dis = dis_data #??? dis_data could be a list
if dis < (distance_stay - 0.1): #TODO: .1 is magic number
led.both_off()
led.red()
turn.turn_ang(mu_t) #??? this is already done
#TODO: fix for 1 motor
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): #TODO: .1 is magic number
#TODO: fix for 1 motor
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()
#TODO: include both_off in all color change functions?
led.both_off()
led.blue()
#TODO: adjust colors with non-magic numbers
cv2.putText(image, 'In Position', (40, 80), font, 0.5,
(255, 128, 128), 1, cv2.LINE_AA)
if dis < 8: #TODO: 8 is magic number
#??? what is this
cv2.putText(image, '%s m' % str(round(dis, 2)),
(40, 40), font, 0.5, (255, 255, 255), 1,
cv2.LINE_AA)
#TODO: put in tilt function
#TODO: reposition to be with X
#TODO: fix magic numbers
if Y < 230:
mu2 = int((240 - Y) / 5) #??? what is mu2
vtr_mid_orig += mu2
if vtr_mid_orig < look_up_max:
pass #??? why is if like this
else:
vtr_mid_orig = look_up_max
turn.camera_turn(vtr_mid_orig) #TODO change to tilt
elif Y > 250: #TODO: see above
mu2 = int((Y - 240) / 5)
vtr_mid_orig -= mu2
if vtr_mid_orig > look_down_max:
pass #??? why is if like this
else:
vtr_mid_orig = look_down_max
turn.camera_turn(vtr_mid_orig)
if X > 280: #TODO: fix magic numbers
if X < 350: #??? what is happening here
# if X in some range
#print('looked') #??? what
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: # no contours found
led.both_off() #TODO: put this into color changes
led.yellow()
cv2.putText(image, 'Target Detecting', (40, 60), font, 0.5,
(255, 255, 255), 1, cv2.LINE_AA)
led_y = 1 #??? never used
#TODO: choose search patterns not just stop and wait
motor.motorStop()
#??? no idea what this block is doing
for i in range(1, len(pts)): #??? pts is global what is it?
if pts[i - 1] is None or pts[i] is None:
continue
#TODO: magic numbers
#??? what is args
thickness = int(np.sqrt(args["buffer"] / float(i + 1)) * 2.5)
cv2.line(image, pts[i - 1], pts[i], (0, 0, 255), thickness)
else: # not opencv_mode==1
#??? why not use directly and its a list
dis = dis_data
#Place the distance to closest object on screen
if dis < 8:
cv2.putText(image, '%s m' % str(round(dis, 2)), (40, 40), font,
0.5, (255, 255, 255), 1, cv2.LINE_AA)
# send image to client
encoded, buffer = cv2.imencode('.jpg', image)
jpg_as_text = base64.b64encode(buffer)
footage_socket.send(jpg_as_text)
rawCapture.truncate(0)
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