def grab():
try:
pos_input = 0
OUT = 1
while 1:
a=input()
if OUT == 1:
if pos_input < 13:
pos_input+=1
else:
print('MAX')
OUT = 0
else:
if pos_input > 1:
pos_input-=1
else:
print('MIN')
OUT = 1
servo.catch(pos_input)
print(pos_input)
pass
except KeyboardInterrupt:
servo.clean_all()
try:
tcpSerSock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
tcpSerSock.setsockopt(socket.SOL_SOCKET,socket.SO_REUSEADDR,1)
tcpSerSock.bind(ADDR)
tcpSerSock.listen(5) #Start server,waiting for client
print('waiting for connection...')
tcpCliSock, addr = tcpSerSock.accept()
print('...connected from :', addr)
fpv=FPV.FPV()
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
break
except:
LED.colorWipe(0,0,0)
try:
LED.colorWipe(0,80,255)
except:
pass
run()
try:
pwm.set_all_pwm(0,0)
run()
except:
LED.colorWipe(0,0,0)
servo.clean_all()
move.destroy()
def run():
global direction_command, turn_command, pos_input, catch_input, cir_input, ultrasonicMode, FindLineMode, FindColorMode
move.setup()
findline.setup()
info_threading = threading.Thread(
target=info_send_client) # Define a thread for FPV and OpenCV
info_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
info_threading.start() # Thread starts
ultra_threading = threading.Thread(
target=ultra_send_client) # Define a thread for FPV and OpenCV
ultra_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
ultra_threading.start() # Thread starts
findline_threading = threading.Thread(
target=findline_thread) # Define a thread for FPV and OpenCV
findline_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
findline_threading.start() # Thread starts
# move.stand()
ws_R = 0
ws_G = 0
ws_B = 0
Y_pitch = 0
Y_pitch_MAX = 200
Y_pitch_MIN = -200
while True:
data = ""
data = str(tcpCliSock.recv(BUFSIZ).decode())
if not data:
continue
elif "forward" == data:
direction_command = "forward"
move.move(speed_set, direction_command, turn_command, rad)
elif "backward" == data:
direction_command = "backward"
move.move(speed_set, direction_command, turn_command, rad)
elif "DS" in data:
direction_command = "no"
move.move(speed_set, direction_command, turn_command, rad)
elif "left" == data:
turn_command = "left"
move.move(speed_set, direction_command, turn_command, rad)
elif "right" == data:
turn_command = "right"
move.move(speed_set, direction_command, turn_command, rad)
elif "TS" in data:
turn_command = "no"
move.move(speed_set, direction_command, turn_command, rad)
elif "out" == data:
if pos_input < 17:
pos_input += 1
servo.hand_pos(pos_input)
elif "in" == data:
if pos_input > 1:
pos_input -= 1
servo.hand_pos(pos_input)
elif "headup" == data:
servo.camera_ang("lookup", 0)
elif "headdown" == data:
servo.camera_ang("lookdown", 0)
elif "headhome" == data:
servo.camera_ang("home", 0)
servo.hand("in")
servo.cir_pos(6)
pos_input = 1
catch_input = 1
cir_input = 6
servo.catch(catch_input)
time.sleep(0.5)
servo.clean_all()
elif "c_left" == data:
if cir_input < 12:
cir_input += 1
servo.cir_pos(cir_input)
elif "c_right" == data:
if cir_input > 1:
cir_input -= 1
servo.cir_pos(cir_input)
elif "catch" == data:
if catch_input < 13:
catch_input += 1
servo.catch(catch_input)
elif "loose" == data:
if catch_input > 1:
catch_input -= 1
servo.catch(catch_input)
elif "wsR" in data:
try:
set_R = data.split()
ws_R = int(set_R[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif "wsG" in data:
try:
set_G = data.split()
ws_G = int(set_G[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif "wsB" in data:
try:
set_B = data.split()
ws_B = int(set_B[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif "FindColor" in data:
FindColorMode = 0
tcpCliSock.send(("FunEnd").encode())
elif "WatchDog" in data:
tcpCliSock.send(("FunEnd").encode())
elif "steady" in data:
ultrasonicMode = 1
tcpCliSock.send(("steady").encode())
elif "FindLine" in data:
FindLineMode = 1
tcpCliSock.send(("FindLine").encode())
elif "funEnd" in data:
ultrasonicMode = 0
FindLineMode = 0
FindColorMode = 0
tcpCliSock.send(("FunEnd").encode())
move.motorStop()
else:
pass
def run():
global direction_command, turn_command, pos_input, catch_input, cir_input, ultrasonicMode, FindLineMode, FindColorMode
move.setup()
findline.setup()
info_threading = threading.Thread(
target=info_send_controller) #Define a thread for FPV and OpenCV
info_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
info_threading.start() #Thread starts
ultra_threading = threading.Thread(
target=ultra_send_controller) #Define a thread for FPV and OpenCV
ultra_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
ultra_threading.start() #Thread starts
findline_threading = threading.Thread(
target=findline_thread) #Define a thread for FPV and OpenCV
findline_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
findline_threading.start() #Thread starts
#move.stand()
ws_R = 0
ws_G = 0
ws_B = 0
Y_pitch = 0
Y_pitch_MAX = 200
Y_pitch_MIN = -200
while True:
data = ''
data = str(tcpCliSock.recv(BUFSIZ).decode())
if not data:
continue
elif 'forward' == data:
direction_command = 'forward'
move.move(speed_set, direction_command, turn_command, rad)
elif 'backward' == data:
direction_command = 'backward'
move.move(speed_set, direction_command, turn_command, rad)
elif 'DS' in data:
direction_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'left' == data:
turn_command = 'left'
move.move(speed_set, direction_command, turn_command, rad)
elif 'right' == data:
turn_command = 'right'
move.move(speed_set, direction_command, turn_command, rad)
elif 'TS' in data:
turn_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'out' == data:
if pos_input < 17:
pos_input += 1
servo.hand_pos(pos_input)
elif 'in' == data:
if pos_input > 1:
pos_input -= 1
servo.hand_pos(pos_input)
elif 'headup' == data:
servo.camera_ang('lookup', 0)
elif 'headdown' == data:
servo.camera_ang('lookdown', 0)
elif 'headhome' == data:
servo.camera_ang('home', 0)
servo.hand('in')
servo.cir_pos(6)
pos_input = 1
catch_input = 1
cir_input = 6
servo.catch(catch_input)
time.sleep(0.5)
servo.clean_all()
elif 'c_left' == data:
if cir_input < 12:
cir_input += 1
servo.cir_pos(cir_input)
elif 'c_right' == data:
if cir_input > 1:
cir_input -= 1
servo.cir_pos(cir_input)
elif 'catch' == data:
if catch_input < 13:
catch_input += 1
servo.catch(catch_input)
elif 'loose' == data:
if catch_input > 1:
catch_input -= 1
servo.catch(catch_input)
elif 'wsR' in data:
try:
set_R = data.split()
ws_R = int(set_R[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif 'wsG' in data:
try:
set_G = data.split()
ws_G = int(set_G[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif 'wsB' in data:
try:
set_B = data.split()
ws_B = int(set_B[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif 'FindColor' in data:
FindColorMode = 0
tcpCliSock.send(('FunEnd').encode())
elif 'WatchDog' in data:
tcpCliSock.send(('FunEnd').encode())
elif 'steady' in data:
ultrasonicMode = 1
tcpCliSock.send(('steady').encode())
elif 'FindLine' in data:
FindLineMode = 1
tcpCliSock.send(('FindLine').encode())
elif 'funEnd' in data:
ultrasonicMode = 0
FindLineMode = 0
FindColorMode = 0
tcpCliSock.send(('FunEnd').encode())
move.motorStop()
else:
pass
def run():
global direction_command, turn_command, pos_input, catch_input, cir_input, ultrasonicMode, FindLineMode, FindLine1Mode, FindLine2Mode, FindColorMode, SportModeOn
move.setup()
findline.setup()
info_threading = threading.Thread(
target=info_send_client) #Define a thread for FPV and OpenCV
info_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
info_threading.start() #Thread starts
ultra_threading = threading.Thread(
target=ultra_send_client) #Define a thread for FPV and OpenCV
ultra_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
ultra_threading.start() #Thread starts
findline_threading = threading.Thread(
target=findline_thread) #Define a thread for FPV and OpenCV
findline_threading.setDaemon(
True
) #'True' means it is a front thread,it would close when the mainloop() closes
findline_threading.start() #Thread starts
#move.stand()
ws_R = 0
ws_G = 0
ws_B = 0
Y_pitch = 0
Y_pitch_MAX = 200
Y_pitch_MIN = -200
while True:
data = ''
data = str(tcpCliSock.recv(BUFSIZ).decode())
if not data:
continue
elif 'SportModeOn' == data:
SportModeOn = 1
tcpCliSock.send(('SportModeOn').encode())
elif 'SportModeOff' == data:
SportModeOn = 0
tcpCliSock.send(('SportModeOff').encode())
elif 'forward' == data:
direction_command = 'forward'
if SportModeOn:
move.move(speed_set, direction_command, turn_command, rad)
else:
move.move(SpeedBase, direction_command, turn_command, rad)
elif 'backward' == data:
direction_command = 'backward'
if SportModeOn:
move.move(speed_set, direction_command, turn_command, rad)
else:
move.move(SpeedBase, direction_command, turn_command, rad)
elif 'DS' in data:
direction_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'left' == data:
turn_command = 'left'
if SportModeOn:
move.move(speed_set, direction_command, turn_command, rad)
else:
move.move(SpeedBase, direction_command, turn_command, rad)
elif 'right' == data:
turn_command = 'right'
if SportModeOn:
move.move(speed_set, direction_command, turn_command, rad)
else:
move.move(SpeedBase, direction_command, turn_command, rad)
elif 'TS' in data:
turn_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'headup' == data:
servo.camera_ang('lookup', 'no')
elif 'headdown' == data:
servo.camera_ang('lookdown', 'no')
elif 'headhome' == data:
servo.camera_ang('home', 'no')
time.sleep(0.2)
servo.clean_all()
elif 'wsR' in data:
try:
set_R = data.split()
ws_R = int(set_R[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif 'wsG' in data:
try:
set_G = data.split()
ws_G = int(set_G[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif 'wsB' in data:
try:
set_B = data.split()
ws_B = int(set_B[1])
LED.colorWipe(Color(ws_R, ws_G, ws_B))
except:
pass
elif 'FindColor' in data:
fpv.FindColor(1)
FindColorMode = 1
ultrasonicMode = 1
tcpCliSock.send(('FindColor').encode())
elif 'WatchDog' in data:
fpv.WatchDog(1)
tcpCliSock.send(('WatchDog').encode())
elif 'steady' in data:
ultrasonicMode = 1
tcpCliSock.send(('steady').encode())
elif 'FindLine' in data:
FindLine1Mode = 0
FindLine2Mode = 0
FindLineMode = 1
tcpCliSock.send(('FindLine').encode())
elif 'FindLine1' in data:
FindLine1Mode = 1
FindLineMode = 0
FindLine2Mode = 0
tcpCliSock.send(('FindLine1').encode())
elif 'FindLine2' in data:
FindLine2Mode = 1
FindLineMode = 0
FindLine1Mode = 0
tcpCliSock.send(('FindLine2').encode())
elif 'funEnd' in data:
fpv.FindColor(0)
fpv.WatchDog(0)
ultrasonicMode = 0
FindLineMode = 0
FindLine1Mode = 0
FindLine2Mode = 0
FindColorMode = 0
tcpCliSock.send(('FunEnd').encode())
move.motorStop()
else:
pass
def head_home():
servo.camera_ang('home', 'no')
time.sleep(0.2)
servo.clean_all()
