def run(self): global goal_pos, servo_command, init_get, functionMode while self.__running.isSet(): self.__flag.wait() if functionMode != 6: if servo_command == 'lookleft': servo.lookleft(servo_speed) elif servo_command == 'lookright': servo.lookright(servo_speed) elif servo_command == 'up': servo.up(servo_speed) elif servo_command == 'down': servo.down(servo_speed) else: pass if functionMode == 4: servo.ahead() findline.run() if not functionMode: move.motorStop() elif functionMode == 5: autoDect(50) if not functionMode: move.motorStop() elif functionMode == 6: if MPU_connection: accelerometer_data = sensor.get_accel_data() X_get = accelerometer_data['x'] if not init_get: goal_pos = X_get init_get = 1 if servo_command == 'up': servo.up(servo_speed) time.sleep(0.2) accelerometer_data = sensor.get_accel_data() X_get = accelerometer_data['x'] goal_pos = X_get elif servo_command == 'down': servo.down(servo_speed) time.sleep(0.2) accelerometer_data = sensor.get_accel_data() X_get = accelerometer_data['x'] goal_pos = X_get if abs(X_get - goal_pos) > tor_pos: if X_get > goal_pos: servo.down(int(mpu_speed * abs(X_get - goal_pos))) elif X_get < goal_pos: servo.up(int(mpu_speed * abs(X_get - goal_pos))) time.sleep(0.03) continue else: functionMode = 0 try: self.pause() except: pass time.sleep(0.03)
def findLineCtrl(posInput, setCenter): if posInput: if posInput > (setCenter + findLineError): move.motorStop() #turnRight error = (posInput - 320) / 5 outv = int(round((pid.GenOut(error)), 0)) servo.lookright(outv) # servo.turnRight(coe_Genout(error, 64)) servo.turnRight() X_lock = 0 elif posInput < (setCenter - findLineError): move.motorStop() #turnLeft error = (320 - posInput) / 5 outv = int(round((pid.GenOut(error)), 0)) servo.lookleft(outv) # servo.turnLeft(coe_Genout(error, 64)) servo.turnLeft() else: if CVrun: move.move(speed_set, 'forward') #forward else: if CVrun: move.move(speed_set, 'backward')
def moveCtrl(distanceInput, backRange, forwardRange): if CVrun: if distanceInput > forwardRange: move.move(speed_set, 'forward') elif distanceInput < backRange: move.move(speed_set, 'backward') else: move.motorStop()
def capture_thread(self, IPinver): global frame_image, camera 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 lastMovtionCaptured = datetime.datetime.now() for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True): frame_image = frame.array timestamp = datetime.datetime.now() if FindColorMode: ####>>>OpenCV Start<<<#### 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: 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.up(outv) Y_lock = 0 elif Y > (240 + tor): error = (Y - 240) / 5 outv = int(round((pid.GenOut(error)), 0)) servo.down(outv) Y_lock = 0 else: Y_lock = 1 if X < (320 - tor * 3): error = (320 - X) / 5 outv = int(round((pid.GenOut(error)), 0)) servo.lookleft(outv) servo.turnLeft(coe_Genout(error, 64)) X_lock = 0 elif X > (330 + tor * 3): error = (X - 240) / 5 outv = int(round((pid.GenOut(error)), 0)) servo.lookright(outv) servo.turnRight(coe_Genout(error, 64)) X_lock = 0 else: move.motorStop() X_lock = 1 if X_lock == 1 and Y_lock == 1: switch.switch(1, 1) switch.switch(2, 1) switch.switch(3, 1) moveCtrl(ultra.checkdist(), back_R, forward_R) else: move.motorStop() switch.switch(1, 0) switch.switch(2, 0) switch.switch(3, 0) else: cv2.putText(frame_image, 'Target Detecting', (40, 60), font, 0.5, (255, 255, 255), 1, cv2.LINE_AA) move.motorStop() ####>>>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(255, 16, 0) lastMovtionCaptured = timestamp switch.switch(1, 1) switch.switch(2, 1) switch.switch(3, 1) if (timestamp - lastMovtionCaptured).seconds >= 0.5: LED.colorWipe(255, 255, 0) switch.switch(1, 0) switch.switch(2, 0) switch.switch(3, 0) if FindLineMode: cvFindLine() cv2.line(frame_image, (300, 240), (340, 240), (128, 255, 128), 1) cv2.line(frame_image, (320, 220), (320, 260), (128, 255, 128), 1) if FindLineMode and not frameRender: encoded, buffer = cv2.imencode('.jpg', frame_findline) else: encoded, buffer = cv2.imencode('.jpg', frame_image) jpg_as_text = base64.b64encode(buffer) footage_socket.send(jpg_as_text) rawCapture.truncate(0)
def run(): global turn_status, speed, angle_rate, color_select, led, check_true_out, backing, last_turn status_right = GPIO.input(line_pin_right) status_middle = GPIO.input(line_pin_middle) status_left = GPIO.input(line_pin_left) #print('R%d M%d L%d'%(status_right,status_middle,status_left)) if status_right == color_select: check_true_out = 0 backing = 0 print('left') led.colorWipe(0, 255, 0) turn_status = -1 last_turn = -1 servo.turnLeft(angle_rate) move.move(speed, 'forward') elif status_left == color_select: check_true_out = 0 backing = 0 print('right') turn_status = 1 last_turn = 1 led.colorWipe(0, 0, 255) servo.turnRight(angle_rate) move.move(speed, 'forward') elif status_middle == color_select: check_true_out = 0 backing = 0 print('middle') led.colorWipe(255, 255, 255) turn_status = 0 servo.turnMiddle() move.move(speed, 'forward') # time.sleep(0.2) else: print('none') led.colorWipe(255, 0, 0) if not backing == 1: if check_true_out == 1: check_true_out = 0 if turn_status == 0: if last_turn == 1: servo.turnRight(angle_rate) else: servo.turnLeft(angle_rate) move.move(speed, 'backward') time.sleep(0.3) elif turn_status == 1: time.sleep(0.3) servo.turnLeft(angle_rate) else: time.sleep(0.3) servo.turnRight(angle_rate) move.move(speed, 'backward') backing = 1 # time.sleep(0.2) else: time.sleep(0.1) check_true_out = 1
servo.turnRight(angle_rate) else: servo.turnLeft(angle_rate) move.move(speed, 'backward') time.sleep(0.3) elif turn_status == 1: time.sleep(0.3) servo.turnLeft(angle_rate) else: time.sleep(0.3) servo.turnRight(angle_rate) move.move(speed, 'backward') backing = 1 # time.sleep(0.2) else: time.sleep(0.1) check_true_out = 1 if __name__ == '__main__': try: setup() move.setup() while 1: run() pass except KeyboardInterrupt: move.destroy()
def autoDect(speed): move.motorStop() servo.ahead() time.sleep(0.3) getMiddle = ultra.checkdist() print('M%f' % getMiddle) servo.ahead() servo.lookleft(100) time.sleep(0.3) getLeft = ultra.checkdist() print('L%f' % getLeft) servo.ahead() servo.lookright(100) time.sleep(0.3) getRight = ultra.checkdist() print('R%f' % getRight) if getMiddle < range_min and min(getLeft, getRight) > range_min: if random.randint(0, 1): servo.turnLeft() else: servo.turnRight() move.move(speed, 'forward') time.sleep(0.5) move.motorStop() elif getLeft < range_min and min(getMiddle, getRight) > range_min: servo.turnRight(0.7) move.move(speed, 'forward') time.sleep(0.5) move.motorStop() elif getRight < range_min and min(getMiddle, getLeft) > range_min: servo.turnLeft(0.7) move.move(speed, 'forward') time.sleep(0.5) move.motorStop() elif max(getLeft, getMiddle) < range_min and getRight > range_min: servo.turnRight() move.move(speed, 'forward') time.sleep(0.5) move.motorStop() elif max(getMiddle, getRight) < range_min and getLeft > range_min: servo.turnLeft() move.move(speed, 'forward') time.sleep(0.5) move.motorStop() elif max(getLeft, getMiddle, getRight) < range_min: move.move(speed, 'backward') time.sleep(0.5) move.motorStop() else: servo.turnMiddle() move.move(speed, 'forward') time.sleep(0.5) move.motorStop()
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)
def run(self): global goal_pos, servo_command, init_get, functionMode while self.__running.isSet(): self.__flag.wait() if SR_mode: voice_command = SR.run() if voice_command == 'forward': turn.turnMiddle() move.move(speed_set, 'forward') time.sleep(1) move.motorStop() elif voice_command == 'backward': turn.turnMiddle() move.move(speed_set, 'backward') time.sleep(1) move.motorStop() elif voice_command == 'left': servo.turnLeft() move.move(speed_set, 'forward') time.sleep(1) turn.turnMiddle() move.motorStop() elif voice_command == 'right': servo.turnRight() move.move(speed_set, 'forward') time.sleep(1) turn.turnMiddle() move.motorStop() elif voice_command == 'stop': turn.turnMiddle() move.motorStop() else: self.pause()
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 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 run() try: run() except: servo_move.stop() led.colorWipe(0, 0, 0) servo.clean_all() move.destroy()