def main(): global FREQ1, FREQ2 range_init() pwm1, pwm2 = servo.servo_init(FREQ1, FREQ2) while (1): try: dist = get_dist() print dist if dist < 100: call(['./take_pic.sh']) vect = image_capture.get_feature_vector('image.jpg') print vect clss = classify(vect) # Move laundry into correct bin print 'Moving laundry to ' + str(clss) + '\'s bin' servo.moveLaundry(clss, pwm1, pwm2) except KeyboardInterrupt: GPIO.cleanup()
# File name : server.py # Production : Gtank # Website : www.gewbot.com # E-mail : [email protected] # Author : William # Date : 2019/10/28 import socket import threading import time import os import LED import move import servo import switch servo.servo_init() switch.switchSetup() switch.set_all_switch_off() LED = LED.LED() LED.colorWipe(80, 255, 0) step_set = 1 speed_set = 100 rad = 0.6 direction_command = 'no' turn_command = 'no' servo_command = 'no' pos_input = 1 catch_input = 1 cir_input = 6
def run(): global servo_move, speed_set, servo_command, functionMode, init_get, R_set, G_set, B_set, SR_mode servo.servo_init() move.setup() findline.setup() direction_command = 'no' turn_command = 'no' servo_command = 'no' speed_set = 100 rad = 0.5 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 servo_move = Servo_ctrl() servo_move.start() servo_move.pause() findline.setup() 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) elif 'backward' == data: direction_command = 'backward' move.move(speed_set, direction_command) elif 'DS' in data: direction_command = 'no' move.move(speed_set, direction_command) elif 'left' == data: # turn_command = 'left' servo.turnLeft() elif 'right' == data: # turn_command = 'right' servo.turnRight() elif 'TS' in data: # turn_command = 'no' servo.turnMiddle() elif 'Switch_1_on' in data: fpv.Sounds(1) tcpCliSock.send(('Switch_1_on').encode()) elif 'Switch_1_off' in data: fpv.Sounds(0) tcpCliSock.send(('Switch_1_off').encode()) elif 'Switch_2_on' in data: #switch.switch(2,1) fpv.Radar(1) tcpCliSock.send(('Switch_2_on').encode()) elif 'Switch_2_off' in data: #switch.switch(2,0) fpv.Radar(0) tcpCliSock.send(('Switch_2_off').encode()) elif 'Switch_3_on' in data: switch.switch(3, 1) tcpCliSock.send(('Switch_3_on').encode()) elif 'Switch_3_off' in data: switch.switch(3, 0) tcpCliSock.send(('Switch_3_off').encode()) elif 'Switch_A_on' in data: tcpCliSock.send(('Switch_A_on').encode()) elif 'Switch_A_off' in data: tcpCliSock.send(('Switch_A_off').encode()) elif 'Switch_B_on' in data: tcpCliSock.send(('Switch_B_on').encode()) elif 'Switch_B_off' in data: tcpCliSock.send(('Switch_B_off').encode()) elif 'Switch_C_on' in data: fpv.HaarJJ(1) tcpCliSock.send(('Switch_C_on').encode()) elif 'Switch_C_off' in data: fpv.HaarJJ(0) tcpCliSock.send(('Switch_C_off').encode()) elif 'Face_Track_on' in data: # functionMode = 3 fpv.FaceTrack(1) tcpCliSock.send(('Face_Track_on').encode()) elif 'Face_Track_off' in data: # functionMode = 0 fpv.FaceTrack(0) tcpCliSock.send(('Face_Track_off').encode()) elif 'function_1_on' in data: servo.ahead() time.sleep(0.2) tcpCliSock.send(('function_1_on').encode()) radar_send = servo.radar_scan() tcpCliSock.sendall(radar_send.encode()) print(radar_send) time.sleep(0.3) tcpCliSock.send(('function_1_off').encode()) elif 'function_2_on' in data: functionMode = 2 fpv.FindColor(1) tcpCliSock.send(('function_2_on').encode()) elif 'function_2_off' in data: functionMode = 0 fpv.FindColor(0) switch.switch(1, 0) switch.switch(2, 0) switch.switch(3, 0) tcpCliSock.send(('function_2_off').encode()) elif 'function_3_on' in data: functionMode = 3 fpv.WatchDog(1) tcpCliSock.send(('function_3_on').encode()) elif 'function_3_off' in data: functionMode = 0 fpv.WatchDog(0) tcpCliSock.send(('function_3_off').encode()) elif 'function_4_on' in data: functionMode = 4 servo_move.resume() tcpCliSock.send(('function_4_on').encode()) elif 'function_4_off' in data: functionMode = 0 servo_move.pause() move.motorStop() tcpCliSock.send(('function_4_off').encode()) elif 'function_5_on' in data: functionMode = 5 servo_move.resume() tcpCliSock.send(('function_5_on').encode()) elif 'function_5_off' in data: functionMode = 0 servo_move.pause() move.motorStop() tcpCliSock.send(('function_5_off').encode()) elif 'function_6_on' in data: if MPU_connection: functionMode = 6 servo_move.resume() tcpCliSock.send(('function_6_on').encode()) elif 'function_6_off' in data: functionMode = 0 servo_move.pause() move.motorStop() init_get = 0 tcpCliSock.send(('function_6_off').encode()) #elif 'function_1_off' in data: # tcpCliSock.send(('function_1_off').encode()) elif 'lookleft' == data: servo_command = 'lookleft' servo_move.resume() elif 'lookright' == data: servo_command = 'lookright' servo_move.resume() elif 'up' == data: servo_command = 'up' servo_move.resume() elif 'down' == data: servo_command = 'down' servo_move.resume() elif 'stop' == data: if not functionMode: servo_move.pause() servo_command = 'no' pass elif 'home' == data: servo.ahead() elif 'CVrun' == data: if not FPV.CVrun: FPV.CVrun = 1 tcpCliSock.send(('CVrun_on').encode()) else: FPV.CVrun = 0 tcpCliSock.send(('CVrun_off').encode()) elif 'wsR' in data: try: set_R = data.split() R_set = int(set_R[1]) led.colorWipe(R_set, G_set, B_set) except: pass elif 'wsG' in data: try: set_G = data.split() G_set = int(set_G[1]) led.colorWipe(R_set, G_set, B_set) except: pass elif 'wsB' in data: try: set_B = data.split() B_set = int(set_B[1]) led.colorWipe(R_set, G_set, B_set) except: pass elif 'pwm0' in data: try: set_pwm0 = data.split() pwm0_set = int(set_pwm0[1]) servo.setPWM(0, pwm0_set) except: pass elif 'pwm1' in data: try: set_pwm1 = data.split() pwm1_set = int(set_pwm1[1]) servo.setPWM(1, pwm1_set) except: pass elif 'pwm2' in data: try: set_pwm2 = data.split() pwm2_set = int(set_pwm2[1]) servo.setPWM(2, pwm2_set) except: pass elif 'Speed' in data: try: set_speed = data.split() speed_set = int(set_speed[1]) except: pass elif 'Save' in data: try: servo.saveConfig() except: pass elif 'CVFL' in data: if not FPV.FindLineMode: FPV.FindLineMode = 1 tcpCliSock.send(('CVFL_on').encode()) else: move.motorStop() FPV.FindLineMode = 0 tcpCliSock.send(('CVFL_off').encode()) elif 'Render' in data: if FPV.frameRender: FPV.frameRender = 0 else: FPV.frameRender = 1 elif 'WBswitch' in data: if FPV.lineColorSet == 255: FPV.lineColorSet = 0 else: FPV.lineColorSet = 255 elif 'lip1' in data: try: set_lip1 = data.split() lip1_set = int(set_lip1[1]) FPV.linePos_1 = lip1_set except: pass elif 'lip2' in data: try: set_lip2 = data.split() lip2_set = int(set_lip2[1]) FPV.linePos_2 = lip2_set except: pass elif 'err' in data: try: set_err = data.split() err_set = int(set_err[1]) FPV.findLineError = err_set except: pass elif 'FCSET' in data: FCSET = data.split() fpv.colorFindSet(int(FCSET[1]), int(FCSET[2]), int(FCSET[3])) elif 'setEC' in data: #Z ECset = data.split() try: fpv.setExpCom(int(ECset[1])) except: pass elif 'defEC' in data: #Z fpv.defaultExpCom() elif 'police' in data: if LED.ledfunc != 'police': tcpCliSock.send(('rainbow_off').encode()) LED.ledfunc = 'police' ledthread.resume() tcpCliSock.send(('police_on').encode()) elif LED.ledfunc == 'police': LED.ledfunc = '' ledthread.pause() tcpCliSock.send(('police_off').encode()) elif 'rainbow' in data: if LED.ledfunc != 'rainbow': tcpCliSock.send(('police_off').encode()) LED.ledfunc = 'rainbow' ledthread.resume() tcpCliSock.send(('rainbow_on').encode()) elif LED.ledfunc == 'rainbow': LED.ledfunc = '' ledthread.pause() tcpCliSock.send(('rainbow_off').encode()) elif 'sr' in data: if not SR_mode: if SR_dect: SR_mode = 1 tcpCliSock.send(('sr_on').encode()) sr.resume() elif SR_mode: SR_mode = 0 sr.pause() move.motorStop() tcpCliSock.send(('sr_off').encode()) else: pass print(data)
def run(): global servo_move, speed_set, servo_command, functionMode, init_get, R_set, G_set, B_set, SR_mode servo.servo_init() move.setup() findline.setup() direction_command = "no" turn_command = "no" servo_command = "no" speed_set = 100 rad = 0.5 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 servo_move = Servo_ctrl() servo_move.start() servo_move.pause() findline.setup() 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) elif "backward" == data: direction_command = "backward" move.move(speed_set, direction_command) elif "DS" in data: direction_command = "no" move.move(speed_set, direction_command) elif "left" == data: # turn_command = 'left' servo.turnLeft() elif "right" == data: # turn_command = 'right' servo.turnRight() elif "TS" in data: # turn_command = 'no' servo.turnMiddle() elif "Switch_1_on" in data: switch.switch(1, 1) tcpCliSock.send(("Switch_1_on").encode()) elif "Switch_1_off" in data: switch.switch(1, 0) tcpCliSock.send(("Switch_1_off").encode()) elif "Switch_2_on" in data: switch.switch(2, 1) tcpCliSock.send(("Switch_2_on").encode()) elif "Switch_2_off" in data: switch.switch(2, 0) tcpCliSock.send(("Switch_2_off").encode()) elif "Switch_3_on" in data: switch.switch(3, 1) tcpCliSock.send(("Switch_3_on").encode()) elif "Switch_3_off" in data: switch.switch(3, 0) tcpCliSock.send(("Switch_3_off").encode()) elif "function_1_on" in data: servo.ahead() time.sleep(0.2) tcpCliSock.send(("function_1_on").encode()) radar_send = servo.radar_scan() tcpCliSock.sendall(radar_send.encode()) print(radar_send) time.sleep(0.3) tcpCliSock.send(("function_1_off").encode()) elif "function_2_on" in data: functionMode = 2 fpv.FindColor(1) tcpCliSock.send(("function_2_on").encode()) elif "function_3_on" in data: functionMode = 3 fpv.WatchDog(1) tcpCliSock.send(("function_3_on").encode()) elif "function_4_on" in data: functionMode = 4 servo_move.resume() tcpCliSock.send(("function_4_on").encode()) elif "function_5_on" in data: functionMode = 5 servo_move.resume() tcpCliSock.send(("function_5_on").encode()) elif "function_6_on" in data: if MPU_connection: functionMode = 6 servo_move.resume() tcpCliSock.send(("function_6_on").encode()) # elif 'function_1_off' in data: # tcpCliSock.send(('function_1_off').encode()) elif "function_2_off" in data: functionMode = 0 fpv.FindColor(0) switch.switch(1, 0) switch.switch(2, 0) switch.switch(3, 0) tcpCliSock.send(("function_2_off").encode()) elif "function_3_off" in data: functionMode = 0 fpv.WatchDog(0) tcpCliSock.send(("function_3_off").encode()) elif "function_4_off" in data: functionMode = 0 servo_move.pause() move.motorStop() tcpCliSock.send(("function_4_off").encode()) elif "function_5_off" in data: functionMode = 0 servo_move.pause() move.motorStop() tcpCliSock.send(("function_5_off").encode()) elif "function_6_off" in data: functionMode = 0 servo_move.pause() move.motorStop() init_get = 0 tcpCliSock.send(("function_6_off").encode()) elif "lookleft" == data: servo_command = "lookleft" servo_move.resume() elif "lookright" == data: servo_command = "lookright" servo_move.resume() elif "up" == data: servo_command = "up" servo_move.resume() elif "down" == data: servo_command = "down" servo_move.resume() elif "stop" == data: if not functionMode: servo_move.pause() servo_command = "no" pass elif "home" == data: servo.ahead() elif "CVrun" == data: if not FPV.CVrun: FPV.CVrun = 1 tcpCliSock.send(("CVrun_on").encode()) else: FPV.CVrun = 0 tcpCliSock.send(("CVrun_off").encode()) elif "wsR" in data: try: set_R = data.split() R_set = int(set_R[1]) led.colorWipe(R_set, G_set, B_set) except: pass elif "wsG" in data: try: set_G = data.split() G_set = int(set_G[1]) led.colorWipe(R_set, G_set, B_set) except: pass elif "wsB" in data: try: set_B = data.split() B_set = int(set_B[1]) led.colorWipe(R_set, G_set, B_set) except: pass elif "pwm0" in data: try: set_pwm0 = data.split() pwm0_set = int(set_pwm0[1]) servo.setPWM(0, pwm0_set) except: pass elif "pwm1" in data: try: set_pwm1 = data.split() pwm1_set = int(set_pwm1[1]) servo.setPWM(1, pwm1_set) except: pass elif "pwm2" in data: try: set_pwm2 = data.split() pwm2_set = int(set_pwm2[1]) servo.setPWM(2, pwm2_set) except: pass elif "Speed" in data: try: set_speed = data.split() speed_set = int(set_speed[1]) except: pass elif "Save" in data: try: servo.saveConfig() except: pass elif "CVFL" in data: if not FPV.FindLineMode: FPV.FindLineMode = 1 tcpCliSock.send(("CVFL_on").encode()) else: move.motorStop() FPV.FindLineMode = 0 tcpCliSock.send(("CVFL_off").encode()) elif "Render" in data: if FPV.frameRender: FPV.frameRender = 0 else: FPV.frameRender = 1 elif "WBswitch" in data: if FPV.lineColorSet == 255: FPV.lineColorSet = 0 else: FPV.lineColorSet = 255 elif "lip1" in data: try: set_lip1 = data.split() lip1_set = int(set_lip1[1]) FPV.linePos_1 = lip1_set except: pass elif "lip2" in data: try: set_lip2 = data.split() lip2_set = int(set_lip2[1]) FPV.linePos_2 = lip2_set except: pass elif "err" in data: try: set_err = data.split() err_set = int(set_err[1]) FPV.findLineError = err_set except: pass elif "FCSET" in data: FCSET = data.split() fpv.colorFindSet(int(FCSET[1]), int(FCSET[2]), int(FCSET[3])) elif "setEC" in data: # Z ECset = data.split() try: fpv.setExpCom(int(ECset[1])) except: pass elif "defEC" in data: # Z fpv.defaultExpCom() elif "police" in data: if LED.ledfunc != "police": tcpCliSock.send(("rainbow_off").encode()) LED.ledfunc = "police" ledthread.resume() tcpCliSock.send(("police_on").encode()) elif LED.ledfunc == "police": LED.ledfunc = "" ledthread.pause() tcpCliSock.send(("police_off").encode()) elif "rainbow" in data: if LED.ledfunc != "rainbow": tcpCliSock.send(("police_off").encode()) LED.ledfunc = "rainbow" ledthread.resume() tcpCliSock.send(("rainbow_on").encode()) elif LED.ledfunc == "rainbow": LED.ledfunc = "" ledthread.pause() tcpCliSock.send(("rainbow_off").encode()) elif "sr" in data: if not SR_mode: if SR_dect: SR_mode = 1 tcpCliSock.send(("sr_on").encode()) sr.resume() elif SR_mode: SR_mode = 0 sr.pause() move.motorStop() tcpCliSock.send(("sr_off").encode()) else: pass print(data)
class CVThread(threading.Thread): font = cv2.FONT_HERSHEY_SIMPLEX kalman_filter_X = Kalman_filter.Kalman_filter(0.01, 0.1) kalman_filter_Y = Kalman_filter.Kalman_filter(0.01, 0.1) P_direction = -1 T_direction = -1 P_servo = 11 T_servo = 11 P_anglePos = 0 T_anglePos = 0 cameraDiagonalW = 64 cameraDiagonalH = 48 videoW = 640 videoH = 480 Y_lock = 0 X_lock = 0 tor = 17 scGear = servo.servo_init() #move.setup() switch.switchSetup() def __init__(self, *args, **kwargs): self.CVThreading = 0 self.CVMode = 'none' self.imgCV = None self.mov_x = None self.mov_y = None self.mov_w = None self.mov_h = None self.radius = 0 self.box_x = None self.box_y = None self.drawing = 0 self.findColorDetection = 0 self.left_Pos1 = None self.right_Pos1 = None self.center_Pos1 = None self.left_Pos2 = None self.right_Pos2 = None self.center_Pos2 = None self.center = None super(CVThread, self).__init__(*args, **kwargs) self.__flag = threading.Event() self.__flag.clear() self.avg = None self.motionCounter = 0 self.lastMovtionCaptured = datetime.datetime.now() self.frameDelta = None self.thresh = None self.cnts = None def mode(self, invar, imgInput): self.CVMode = invar self.imgCV = imgInput self.resume() def elementDraw(self, imgInput): if self.CVMode == 'none': pass elif self.CVMode == 'findColor': if self.findColorDetection: cv2.putText(imgInput, 'Target Detected', (40, 60), CVThread.font, 0.5, (255, 255, 255), 1, cv2.LINE_AA) self.drawing = 1 else: cv2.putText(imgInput, 'Target Detecting', (40, 60), CVThread.font, 0.5, (255, 255, 255), 1, cv2.LINE_AA) self.drawing = 0 if self.radius > 10 and self.drawing: cv2.rectangle(imgInput, (int(self.box_x - self.radius), int(self.box_y + self.radius)), (int(self.box_x + self.radius), int(self.box_y - self.radius)), (255, 255, 255), 1) elif self.CVMode == 'findlineCV': if frameRender: imgInput = cv2.cvtColor(imgInput, cv2.COLOR_BGR2GRAY) retval_bw, imgInput = cv2.threshold(imgInput, 0, 255, cv2.THRESH_OTSU) imgInput = cv2.erode(imgInput, None, iterations=6) try: if lineColorSet == 255: cv2.putText(imgInput, ('Following White Line'), (30, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (128, 255, 128), 1, cv2.LINE_AA) else: cv2.putText(imgInput, ('Following Black Line'), (30, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (128, 255, 128), 1, cv2.LINE_AA) cv2.line(imgInput, (self.left_Pos1, (linePos_1 + 30)), (self.left_Pos1, (linePos_1 - 30)), (255, 128, 64), 1) cv2.line( imgInput, (self.right_Pos1, (linePos_1 + 30)), (self.right_Pos1, (linePos_1 - 30)), (64, 128, 255), ) cv2.line(imgInput, (0, linePos_1), (640, linePos_1), (255, 255, 64), 1) cv2.line(imgInput, (self.left_Pos2, (linePos_2 + 30)), (self.left_Pos2, (linePos_2 - 30)), (255, 128, 64), 1) cv2.line(imgInput, (self.right_Pos2, (linePos_2 + 30)), (self.right_Pos2, (linePos_2 - 30)), (64, 128, 255), 1) cv2.line(imgInput, (0, linePos_2), (640, linePos_2), (255, 255, 64), 1) cv2.line(imgInput, ((self.center - 20), int( (linePos_1 + linePos_2) / 2)), ((self.center + 20), int( (linePos_1 + linePos_2) / 2)), (0, 0, 0), 1) cv2.line( imgInput, ((self.center), int((linePos_1 + linePos_2) / 2 + 20)), ((self.center), int((linePos_1 + linePos_2) / 2 - 20)), (0, 0, 0), 1) except: pass elif self.CVMode == 'watchDog': if self.drawing: cv2.rectangle( imgInput, (self.mov_x, self.mov_y), (self.mov_x + self.mov_w, self.mov_y + self.mov_h), (128, 255, 0), 1) return imgInput def watchDog(self, imgInput): timestamp = datetime.datetime.now() gray = cv2.cvtColor(imgInput, cv2.COLOR_BGR2GRAY) gray = cv2.GaussianBlur(gray, (21, 21), 0) if self.avg is None: print("[INFO] starting background model...") self.avg = gray.copy().astype("float") return 'background model' cv2.accumulateWeighted(gray, self.avg, 0.5) self.frameDelta = cv2.absdiff(gray, cv2.convertScaleAbs(self.avg)) # threshold the delta image, dilate the thresholded image to fill # in holes, then find contours on thresholded image self.thresh = cv2.threshold(self.frameDelta, 5, 255, cv2.THRESH_BINARY)[1] self.thresh = cv2.dilate(self.thresh, None, iterations=2) self.cnts = cv2.findContours(self.thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) self.cnts = imutils.grab_contours(self.cnts) # print('x') # loop over the contours for c in self.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 (self.mov_x, self.mov_y, self.mov_w, self.mov_h) = cv2.boundingRect(c) self.drawing = 1 self.motionCounter += 1 #print(motionCounter) #print(text) self.lastMovtionCaptured = timestamp switch.switch(1, 1) switch.switch(2, 1) switch.switch(3, 1) if (timestamp - self.lastMovtionCaptured).seconds >= 0.5: self.drawing = 0 switch.switch(1, 0) switch.switch(2, 0) switch.switch(3, 0) self.pause() def findLineCtrl(self, posInput, setCenter): #2 # if posInput: if posInput > (setCenter + findLineError): # move.motorStop() #turnRight error = (posInput - 320) / 3 outv = int(round((pid.GenOut(error)), 0)) CVThread.scGear.moveAngle(0, -outv) if CVRun: move.move(80, 'no', 'right', 0.5) else: move.move(80, 'no', 'no', 0.5) pass elif posInput < (setCenter - findLineError): # move.motorStop() #turnLeft error = (320 - posInput) / 3 outv = int(round((pid.GenOut(error)), 0)) CVThread.scGear.moveAngle(0, outv) if CVRun: move.move(80, 'no', 'left', 0.5) else: move.move(80, 'no', 'no', 0.5) pass else: if CVRun: move.move(80, 'forward', 'no', 0.5) else: move.move(80, 'no', 'no', 0.5) #forward pass # else: # pass def findlineCV(self, frame_image): frame_findline = cv2.cvtColor(frame_image, cv2.COLOR_BGR2GRAY) retval, frame_findline = cv2.threshold(frame_findline, 0, 255, cv2.THRESH_OTSU) frame_findline = cv2.erode(frame_findline, None, iterations=6) colorPos_1 = frame_findline[linePos_1] colorPos_2 = frame_findline[linePos_2] try: lineColorCount_Pos1 = np.sum(colorPos_1 == lineColorSet) lineColorCount_Pos2 = np.sum(colorPos_2 == lineColorSet) lineIndex_Pos1 = np.where(colorPos_1 == lineColorSet) lineIndex_Pos2 = np.where(colorPos_2 == lineColorSet) if lineColorCount_Pos1 == 0: lineColorCount_Pos1 = 1 if lineColorCount_Pos2 == 0: lineColorCount_Pos2 = 1 self.left_Pos1 = lineIndex_Pos1[0][lineColorCount_Pos1 - 1] self.right_Pos1 = lineIndex_Pos1[0][0] self.center_Pos1 = int((self.left_Pos1 + self.right_Pos1) / 2) self.left_Pos2 = lineIndex_Pos2[0][lineColorCount_Pos2 - 1] self.right_Pos2 = lineIndex_Pos2[0][0] self.center_Pos2 = int((self.left_Pos2 + self.right_Pos2) / 2) self.center = int((self.center_Pos1 + self.center_Pos2) / 2) except: center = None pass self.findLineCtrl(self.center, 320) self.pause() def servoMove(ID, Dir, errorInput): if ID == 1: errorGenOut = CVThread.kalman_filter_X.kalman(errorInput) CVThread.P_anglePos += 0.15 * ( errorGenOut * Dir) * CVThread.cameraDiagonalW / CVThread.videoW if abs(errorInput) > CVThread.tor: CVThread.scGear.moveAngle(ID, CVThread.P_anglePos) CVThread.X_lock = 0 else: CVThread.X_lock = 1 elif ID == 11: errorGenOut = CVThread.kalman_filter_Y.kalman(errorInput) CVThread.T_anglePos += 0.25 * ( errorGenOut * Dir) * CVThread.cameraDiagonalH / CVThread.videoH if abs(errorInput) > CVThread.tor: CVThread.scGear.moveAngle(ID, CVThread.T_anglePos) CVThread.Y_lock = 0 else: CVThread.Y_lock = 1 else: print('No servoPort %d assigned.' % ID) def findColor(self, frame_image): hsv = cv2.cvtColor(frame_image, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv, colorLower, colorUpper) #1 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: self.findColorDetection = 1 c = max(cnts, key=cv2.contourArea) ((self.box_x, self.box_y), self.radius) = cv2.minEnclosingCircle(c) M = cv2.moments(c) center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) X = int(self.box_x) Y = int(self.box_y) error_Y = 240 - Y error_X = 320 - X # CVThread.servoMove(CVThread.P_servo, CVThread.P_direction, error_X) CVThread.servoMove(CVThread.T_servo, CVThread.T_direction, error_Y) if CVThread.X_lock == 1 and CVThread.Y_lock == 1: switch.switch(1, 1) switch.switch(2, 1) switch.switch(3, 1) else: switch.switch(1, 0) switch.switch(2, 0) switch.switch(3, 0) else: self.findColorDetection = 0 move.motorStop() self.pause() def pause(self): self.__flag.clear() def resume(self): self.__flag.set() def run(self): while 1: self.__flag.wait() if self.CVMode == 'none': continue elif self.CVMode == 'findColor': self.CVThreading = 1 self.findColor(self.imgCV) self.CVThreading = 0 elif self.CVMode == 'findlineCV': self.CVThreading = 1 self.findlineCV(self.imgCV) self.CVThreading = 0 elif self.CVMode == 'watchDog': self.CVThreading = 1 self.watchDog(self.imgCV) self.CVThreading = 0 pass
from BaseHTTPServer import BaseHTTPRequestHandler, HTTPServer import sys, os import signal import subprocess import threading import time from subprocess import call import servo import RPi as GPIO FREQ = 50 pwm1, pwm2, pwm3 = servo.servo_init(FREQ) class ServerHandler(BaseHTTPRequestHandler): global pwm1, pwm2, pwm3 def _set_headers(self, content_type="text/plain"): self.send_response(200) self.end_headers() def do_HEAD(self): self._set_headers() def do_GET(self): print("in do get") action = self.path.split("/") action = action[1] print("here is the action",action) if(action == "waterup"): servo.moveMotor(1,pwm1,pwm2,pwm3)
def run(): global servo_move, speed_set, servo_command, functionMode, init_get servo.servo_init() move.setup() findline.setup() direction_command = 'no' turn_command = 'no' servo_command = 'no' speed_set = 100 rad = 0.5 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 servo_move = Servo_ctrl() servo_move.start() servo_move.pause() findline.setup() 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 'Switch_1_on' in data: switch.switch(1, 1) tcpCliSock.send(('Switch_1_on').encode()) elif 'Switch_1_off' in data: switch.switch(1, 0) tcpCliSock.send(('Switch_1_off').encode()) elif 'Switch_2_on' in data: switch.switch(2, 1) tcpCliSock.send(('Switch_2_on').encode()) elif 'Switch_2_off' in data: switch.switch(2, 0) tcpCliSock.send(('Switch_2_off').encode()) elif 'Switch_3_on' in data: switch.switch(3, 1) tcpCliSock.send(('Switch_3_on').encode()) elif 'Switch_3_off' in data: switch.switch(3, 0) tcpCliSock.send(('Switch_3_off').encode()) elif 'function_1_on' in data: servo.ahead() time.sleep(0.2) tcpCliSock.send(('function_1_on').encode()) radar_send = servo.radar_scan() tcpCliSock.sendall(radar_send.encode()) print(radar_send) time.sleep(0.3) tcpCliSock.send(('function_1_off').encode()) elif 'function_2_on' in data: functionMode = 2 fpv.FindColor(1) tcpCliSock.send(('function_2_on').encode()) elif 'function_3_on' in data: functionMode = 3 fpv.WatchDog(1) tcpCliSock.send(('function_3_on').encode()) elif 'function_4_on' in data: functionMode = 4 servo_move.resume() tcpCliSock.send(('function_4_on').encode()) elif 'function_5_on' in data: functionMode = 5 servo_move.resume() tcpCliSock.send(('function_5_on').encode()) elif 'function_6_on' in data: if MPU_connection: functionMode = 6 servo_move.resume() tcpCliSock.send(('function_6_on').encode()) #elif 'function_1_off' in data: # tcpCliSock.send(('function_1_off').encode()) elif 'function_2_off' in data: functionMode = 0 fpv.FindColor(0) switch.switch(1, 0) switch.switch(2, 0) switch.switch(3, 0) tcpCliSock.send(('function_2_off').encode()) elif 'function_3_off' in data: functionMode = 0 fpv.WatchDog(0) tcpCliSock.send(('function_3_off').encode()) elif 'function_4_off' in data: functionMode = 0 servo_move.pause() move.motorStop() tcpCliSock.send(('function_4_off').encode()) elif 'function_5_off' in data: functionMode = 0 servo_move.pause() move.motorStop() tcpCliSock.send(('function_5_off').encode()) elif 'function_6_off' in data: functionMode = 0 servo_move.pause() move.motorStop() init_get = 0 tcpCliSock.send(('function_6_off').encode()) elif 'lookleft' == data: servo_command = 'lookleft' servo_move.resume() elif 'lookright' == data: servo_command = 'lookright' servo_move.resume() elif 'up' == data: servo_command = 'up' servo_move.resume() elif 'down' == data: servo_command = 'down' servo_move.resume() elif 'lookup' == data: servo_command = 'lookup' servo_move.resume() elif 'lookdown' == data: servo_command = 'lookdown' servo_move.resume() elif 'grab' == data: servo_command = 'grab' servo_move.resume() elif 'loose' == data: servo_command = 'loose' servo_move.resume() elif 'stop' == data: if not functionMode: servo_move.pause() servo_command = 'no' pass elif 'home' == data: servo.ahead() elif 'wsB' in data: try: set_B = data.split() speed_set = int(set_B[1]) except: pass elif 'CVFL' in data: if not FPV.FindLineMode: FPV.FindLineMode = 1 tcpCliSock.send(('CVFL_on').encode()) else: move.motorStop() FPV.FindLineMode = 0 tcpCliSock.send(('CVFL_off').encode()) elif 'Render' in data: if FPV.frameRender: FPV.frameRender = 0 else: FPV.frameRender = 1 elif 'WBswitch' in data: if FPV.lineColorSet == 255: FPV.lineColorSet = 0 else: FPV.lineColorSet = 255 elif 'lip1' in data: try: set_lip1 = data.split() lip1_set = int(set_lip1[1]) FPV.linePos_1 = lip1_set except: pass elif 'lip2' in data: try: set_lip2 = data.split() lip2_set = int(set_lip2[1]) FPV.linePos_2 = lip2_set except: pass elif 'err' in data: try: set_err = data.split() err_set = int(set_err[1]) FPV.findLineError = err_set except: pass elif 'FCSET' in data: FCSET = data.split() fpv.colorFindSet(int(FCSET[1]), int(FCSET[2]), int(FCSET[3])) elif 'setEC' in data: #Z ECset = data.split() try: fpv.setExpCom(int(ECset[1])) except: pass elif 'defEC' in data: #Z fpv.defaultExpCom() else: pass print(data)