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()
def appCommand(data_input):
global direction_command, turn_command, pos_input, catch_input, cir_input
if data_input == "forwardStart\n":
direction_command = "forward"
move.move(speed_set, direction_command, turn_command, rad)
elif data_input == "backwardStart\n":
direction_command = "backward"
move.move(speed_set, direction_command, turn_command, rad)
elif data_input == "leftStart\n":
turn_command = "left"
move.move(speed_set, direction_command, turn_command, rad)
elif data_input == "rightStart\n":
turn_command = "right"
move.move(speed_set, direction_command, turn_command, rad)
elif "forwardStop" in data_input:
direction_command = "no"
move.move(speed_set, direction_command, turn_command, rad)
elif "backwardStop" in data_input:
direction_command = "no"
move.move(speed_set, direction_command, turn_command, rad)
elif "leftStop" in data_input:
turn_command = "no"
move.move(speed_set, direction_command, turn_command, rad)
elif "rightStop" in data_input:
turn_command = "no"
move.move(speed_set, direction_command, turn_command, rad)
if data_input == "lookLeftStart\n":
if cir_input < 12:
cir_input += 1
servo.cir_pos(cir_input)
elif data_input == "lookRightStart\n":
if cir_input > 1:
cir_input -= 1
servo.cir_pos(cir_input)
elif data_input == "downStart\n":
servo.camera_ang("lookdown", 10)
elif data_input == "upStart\n":
servo.camera_ang("lookup", 10)
elif "lookLeftStop" in data_input:
pass
elif "lookRightStop" in data_input:
pass
elif "downStop" in data_input:
pass
elif "upStop" in data_input:
pass
if data_input == "aStart\n":
if pos_input < 17:
pos_input += 1
servo.hand_pos(pos_input)
elif data_input == "bStart\n":
if pos_input > 1:
pos_input -= 1
servo.hand_pos(pos_input)
elif data_input == "cStart\n":
if catch_input < 13:
catch_input += 3
servo.catch(catch_input)
elif data_input == "dStart\n":
if catch_input > 1:
catch_input -= 3
servo.catch(catch_input)
elif "aStop" in data_input:
pass
elif "bStop" in data_input:
pass
elif "cStop" in data_input:
pass
elif "dStop" in data_input:
pass
print(data_input)
def appCommand(data_input):
global direction_command, turn_command, pos_input, catch_input, cir_input
if data_input == 'forwardStart\n':
direction_command = 'forward'
move.move(speed_set, direction_command, turn_command, rad)
elif data_input == 'backwardStart\n':
direction_command = 'backward'
move.move(speed_set, direction_command, turn_command, rad)
elif data_input == 'leftStart\n':
turn_command = 'left'
move.move(speed_set, direction_command, turn_command, rad)
elif data_input == 'rightStart\n':
turn_command = 'right'
move.move(speed_set, direction_command, turn_command, rad)
elif 'forwardStop' in data_input:
direction_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'backwardStop' in data_input:
direction_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'leftStop' in data_input:
turn_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
elif 'rightStop' in data_input:
turn_command = 'no'
move.move(speed_set, direction_command, turn_command, rad)
if data_input == 'lookLeftStart\n':
if cir_input < 12:
cir_input+=1
servo.cir_pos(cir_input)
elif data_input == 'lookRightStart\n':
if cir_input > 1:
cir_input-=1
servo.cir_pos(cir_input)
elif data_input == 'downStart\n':
servo.camera_ang('lookdown',10)
elif data_input == 'upStart\n':
servo.camera_ang('lookup',10)
elif 'lookLeftStop' in data_input:
pass
elif 'lookRightStop' in data_input:
pass
elif 'downStop' in data_input:
pass
elif 'upStop' in data_input:
pass
if data_input == 'aStart\n':
if pos_input < 17:
pos_input+=1
servo.hand_pos(pos_input)
elif data_input == 'bStart\n':
if pos_input > 1:
pos_input-=1
servo.hand_pos(pos_input)
elif data_input == 'cStart\n':
if catch_input < 13:
catch_input+=3
servo.catch(catch_input)
elif data_input == 'dStart\n':
if catch_input > 1:
catch_input-=3
servo.catch(catch_input)
elif 'aStop' in data_input:
pass
elif 'bStop' in data_input:
pass
elif 'cStop' in data_input:
pass
elif 'dStop' in data_input:
pass
print(data_input)
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.initPosAll()
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(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(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(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:
FindLineMode = 1
tcpCliSock.send(('FindLine').encode())
elif 'funEnd' in data:
fpv.FindColor(0)
fpv.WatchDog(0)
ultrasonicMode = 0
FindLineMode = 0
FindColorMode = 0
tcpCliSock.send(('FunEnd').encode())
move.motorStop()
time.sleep(0.3)
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_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
