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()