def move_servos():
lt = joy.leftTrigger()
rt = joy.rightTrigger()
lb = joy.leftBumper()
rb = joy.rightBumper()
if lt:
arm.loosen()
if rt:
arm.clamp()
if lb:
arm.reach_out()
if rb:
arm.tuck_in()
reset_arm = joy.Y()
if reset_arm: # TODO should probably have this be a one time thing
arm.start_position()
camup = joy.dpadUp()
camdown = joy.dpadDown()
if camup:
servo.camera_ang('lookup')
if camdown:
servo.camera_ang('lookdown')
def FindColor(self, invar):
global FindColorMode
FindColorMode = invar
if not FindColorMode:
servo.camera_ang('home', 0)
if invar:
print('FindColorMode enabled')
if WatchDogMode:
self.WatchDog(0)
else:
print('FindColorMode disabled')
def capture_thread(self, IPinver):
ap = argparse.ArgumentParser() #OpenCV initialization
ap.add_argument("-b",
"--buffer",
type=int,
default=64,
help="max buffer size")
args = vars(ap.parse_args())
pts = deque(maxlen=args["buffer"])
font = cv2.FONT_HERSHEY_SIMPLEX
camera = picamera.PiCamera()
camera.resolution = (640, 480)
camera.framerate = 20
rawCapture = PiRGBArray(camera, size=(640, 480))
context = zmq.Context()
footage_socket = context.socket(zmq.PUB)
print(IPinver)
footage_socket.connect('tcp://%s:5555' % IPinver)
avg = None
motionCounter = 0
#time.sleep(4)
lastMovtionCaptured = datetime.datetime.now()
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
frame_image = frame.array
cv2.line(frame_image, (300, 240), (340, 240), (128, 255, 128), 1)
cv2.line(frame_image, (320, 220), (320, 260), (128, 255, 128), 1)
timestamp = datetime.datetime.now()
if FindColorMode:
####>>>OpenCV Start<<<####
hsv = cv2.cvtColor(frame_image, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, self.colorLower, self.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:
cv2.putText(frame_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(frame_image,
(int(x - radius), int(y + radius)),
(int(x + radius), int(y - radius)),
(255, 255, 255), 1)
if Y < (240 - tor):
error = (240 - Y) / 5
outv = int(round((pid.GenOut(error)), 0))
servo.camera_ang('lookup', outv)
Y_lock = 0
elif Y > (240 + tor):
error = (Y - 240) / 5
outv = int(round((pid.GenOut(error)), 0))
servo.camera_ang('lookdown', outv)
Y_lock = 0
else:
Y_lock = 1
if X < (320 - tor * 3):
move.move(70, 'no', 'left', 0.6)
#time.sleep(0.1)
#move.motorStop()
X_lock = 0
elif X > (330 + tor * 3):
move.move(70, 'no', 'right', 0.6)
#time.sleep(0.1)
#move.motorStop()
X_lock = 0
else:
move.motorStop()
X_lock = 1
if X_lock == 1 and Y_lock == 1:
if UltraData > 0.5:
move.move(70, 'forward', 'no', 0.6)
elif UltraData < 0.4:
move.move(70, 'backward', 'no', 0.6)
print(UltraData)
else:
move.motorStop()
else:
cv2.putText(frame_image, 'Target Detecting', (40, 60),
font, 0.5, (255, 255, 255), 1, cv2.LINE_AA)
move.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(frame_image, pts[i - 1], pts[i], (0, 0, 255),
thickness)
####>>>OpenCV Ends<<<####
if WatchDogMode:
gray = cv2.cvtColor(frame_image, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (21, 21), 0)
if avg is None:
print("[INFO] starting background model...")
avg = gray.copy().astype("float")
rawCapture.truncate(0)
continue
cv2.accumulateWeighted(gray, avg, 0.5)
frameDelta = cv2.absdiff(gray, cv2.convertScaleAbs(avg))
# threshold the delta image, dilate the thresholded image to fill
# in holes, then find contours on thresholded image
thresh = cv2.threshold(frameDelta, 5, 255,
cv2.THRESH_BINARY)[1]
thresh = cv2.dilate(thresh, None, iterations=2)
cnts = cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
# print('x')
# loop over the contours
for c in cnts:
# if the contour is too small, ignore it
if cv2.contourArea(c) < 5000:
continue
# compute the bounding box for the contour, draw it on the frame,
# and update the text
(x, y, w, h) = cv2.boundingRect(c)
cv2.rectangle(frame_image, (x, y), (x + w, y + h),
(128, 255, 0), 1)
text = "Occupied"
motionCounter += 1
#print(motionCounter)
#print(text)
LED.colorWipe(Color(255, 16, 0))
lastMovtionCaptured = timestamp
if (timestamp - lastMovtionCaptured).seconds >= 0.5:
LED.colorWipe(Color(255, 255, 0))
encoded, buffer = cv2.imencode('.jpg', frame_image)
jpg_as_text = base64.b64encode(buffer)
footage_socket.send(jpg_as_text)
rawCapture.truncate(0)
def FindColor(self, invar):
global FindColorMode
FindColorMode = invar
if not FindColorMode:
servo.camera_ang('home', 0)
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
def find_color_fnc(self, frame_image):
####>>>OpenCV Start<<<####
hsv = cv2.cvtColor(frame_image, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, self.colorLower, self.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:
cv2.putText(frame_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(frame_image, (int(x - radius), int(y + radius)),
(int(x + radius), int(y - radius)),
(255, 255, 255), 1)
if Y < (240 - tor):
error = (240 - Y) / 5
outv = int(round((pid.GenOut(error)), 0))
servo.camera_ang('lookup', outv)
Y_lock = 0
elif Y > (240 + tor):
error = (Y - 240) / 5
outv = int(round((pid.GenOut(error)), 0))
servo.camera_ang('lookdown', outv)
Y_lock = 0
else:
Y_lock = 1
if X < (320 - tor * 3):
move.move(70, 'no', 'left', 0.6)
time.sleep(0.1)
#move.motorStop()
X_lock = 0
elif X > (330 + tor * 3):
move.move(70, 'no', 'right', 0.6)
time.sleep(0.1)
#move.motorStop()
X_lock = 0
else:
move.motorStop()
X_lock = 1
if X_lock == 1: # and Y_lock == 1:
UltraData = ultra.checkdist()
if UltraData > 3:
move.motorStop()
elif UltraData > 0.5:
move.move(70, 'forward', 'no', 0.6)
elif UltraData < 0.4:
move.move(70, 'backward', 'no', 0.6)
print(UltraData)
else:
move.motorStop()
else:
cv2.putText(frame_image, 'Target Detecting', (40, 60), font, 0.5,
(255, 255, 255), 1, cv2.LINE_AA)
move.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(frame_image, pts[i - 1], pts[i], (0, 0, 255), thickness)
####>>>OpenCV Ends<<<####
return frame_image, mask
def run():
global direction_command, turn_command, pos_input, catch_input, cir_input, ultrasonicMode, FindLineMode, 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'
if SportModeOn:
move.move(speed_set, direction_command, turn_command, rad)
else:
move.move(SpeedBase, 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:
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 robotCtrl(command_input, response):
global direction_command, turn_command
if 'forward' == command_input:
direction_command = 'forward'
move.move(speed_set, 'forward', 'no', rad)
elif 'backward' == command_input:
direction_command = 'backward'
move.move(speed_set, 'backward', 'no', rad)
elif 'DS' in command_input:
direction_command = 'no'
move.move(speed_set, 'no', 'no', rad)
elif 'left' == command_input:
turn_command = 'left'
move.move(speed_set, 'no', 'left', rad)
elif 'right' == command_input:
turn_command = 'right'
move.move(speed_set, 'no', 'right', rad)
elif 'TS' in command_input:
turn_command = 'no'
if direction_command == 'no':
move.move(speed_set, 'no', 'no', rad)
else:
move.move(speed_set, direction_command, 'no', rad)
# elif 'lookleft' == command_input:
# P_sc.singleServo(0, 1, 3)
# elif 'lookright' == command_input:
# P_sc.singleServo(0, -1, 3)
# elif 'LRstop' in command_input:
# P_sc.stopWiggle()
elif 'up' == command_input:
# C_sc.singleServo(0, 1, 3)
servo.camera_ang('lookup','no')
elif 'down' == command_input:
# C_sc.singleServo(0, -1, 3)
servo.camera_ang('lookdown','no')
#elif 'UDstop'==command_input:
# #C_sc.stopWiggle()
# servo.camera_ang('home','no')
# time.sleep(0.2)
# servo.clean_all()
elif 'handup' == command_input:
H_sc.singleServo(2, 1, 3)
elif 'handdown' == command_input:
H_sc.singleServo(2, -1, 3)
elif 'HAstop' in command_input:
H_sc.stopWiggle()
elif 'grab' == command_input:
G_sc.singleServo(3, -1, 3)
elif 'loose' == command_input:
G_sc.singleServo(3, 1, 3)
elif 'stop' == command_input:
G_sc.stopWiggle()
elif 'home' == command_input:
P_sc.moveServoInit([0])
C_sc.moveServoInit([4])
T_sc.moveServoInit([1])
H_sc.moveServoInit([2])
G_sc.moveServoInit([3])
def head_home():
servo.camera_ang('home', 'no')
time.sleep(0.2)
servo.clean_all()
