def main(): global FACE_DETECTOR, stepper_index, CURRENT_FACE, DELAY_IMAGES, NEXT_TIME, IMAGE, DEFINITION_MULTIPLIER, TRACKING_SCREEN, CAPTURE, DELAY_DELETE_IMAGES, LOW_RES_STREAM, index, servo_index_x, gray # Init script STEPPER.start() STEPPER.enable(False) # LOCK = threading.Lock() # CONTROL_THREAD = Thread() # CONTROL_THREAD.setRunFunction(control_loop) # CONTROL_THREAD.start() # Global variables CURRENT_TIME = time.time() DELAY_IMAGES = 5 NEXT_TIME = CURRENT_TIME + DELAY_IMAGES # Camera IMAGE = None DEFINITION_MULTIPLIER = 1 CAMERA = PiCamera() CAMERA.resolution = (int(1024 * 1.5), int(768 * 1.5)) CAMERA.framerate = 32 TRACKING_SCREEN = [ int(160 * DEFINITION_MULTIPLIER), int(120 * DEFINITION_MULTIPLIER) ] CAPTURE = PiRGBArray(CAMERA, size=(int(160 * DEFINITION_MULTIPLIER), int(120 * DEFINITION_MULTIPLIER))) DELAY_DELETE_IMAGES = 60 # seconds -> 2min LOW_RES_STREAM = CAMERA.capture_continuous( CAPTURE, format="bgr", use_video_port=True, splitter_port=2, resize=(int(160 * DEFINITION_MULTIPLIER), int(120 * DEFINITION_MULTIPLIER))) time.sleep(2.0) print("done warming up") # Remove all old images CAN_TAKE_A_PHOTO = False SYNC_FOLDER_PATH = "/home/pi/Desktop/RPI_3_sync/" file_list = glob.glob(SYNC_FOLDER_PATH + "*.jpg") for image in file_list: os.remove(image) # Run the min loop run = True try: while run: # Update current time CURRENT_TIME = time.time() if FACES is not None: if len(FACES) > 0: #print("photo") CAN_TAKE_A_PHOTO = True # Take pictures every DELAY_IMAGES if CURRENT_TIME >= NEXT_TIME and CAN_TAKE_A_PHOTO: CAMERA.capture(SYNC_FOLDER_PATH + str(int(math.floor(time.time() * 1000))) + '.jpg') file_list = glob.glob(SYNC_FOLDER_PATH + "*.jpg") #print("Image taken: " + str(len(file_list))) for image in file_list: name = float( image.split("/")[len(image.split("/")) - 1].replace( ' ', '')[:-4].upper()) # Delete images that are older than the DELAY_DELETE_IMAGES if name < CURRENT_TIME - DELAY_DELETE_IMAGES: os.remove(image) for i in range(len(file_list), 0): if len(file_list) - i > 20: os.remove(file_list[i]) NEXT_TIME += DELAY_IMAGES CAN_TAKE_A_PHOTO = False low_res = LOW_RES_STREAM.next() IMAGE = low_res.array gray = cv2.cvtColor(IMAGE, cv2.COLOR_BGR2GRAY) # Run face detection as soon as one has ended if FACE_DETECTOR is None: FACE_DETECTOR = Thread() FACE_DETECTOR.setRunFunction(face_detection) FACE_DETECTOR.start() else: if FACE_DETECTOR.isAlive() is not True: FACE_DETECTOR = None if FACES is not None: if len(FACES) > 0: if CURRENT_FACE is None: CURRENT_FACE = FACES[0] # Always check for the closer face to the last one # in order not to jump from a face to another min_distance = float("inf") for face in FACES: distance = math.sqrt( math.pow(CURRENT_FACE[0] - face[0], 2) + math.pow(CURRENT_FACE[1] - face[1], 2)) if distance < min_distance: min_distance = distance CURRENT_FACE = face #print(CURRENT_FACE) cv2.circle(IMAGE, (CURRENT_FACE[0] + CURRENT_FACE[2] / 2, CURRENT_FACE[1] + CURRENT_FACE[3] / 2), int(CURRENT_FACE[2] / 3), (255, 255, 255), 1) else: CURRENT_FACE = None # Motors control #SERVO.set(0, 0.5 - math.sin(index) * 0.1) #SERVO.set(0, 0.5) stepper_index += 0.001 # Check if there is a current face to track if CURRENT_FACE is not None: # print("face") face_pos_x = CURRENT_FACE[0] + CURRENT_FACE[2] / 2 delta_x = (face_pos_x - TRACKING_SCREEN[0] * 0.5) / ( TRACKING_SCREEN[0] * 0.5) servo_index_x += delta_x * -0.02 STEPPER.enable(True) face_pos_y = CURRENT_FACE[1] + CURRENT_FACE[3] / 2 delta_y = (face_pos_y - TRACKING_SCREEN[1] * 0.5) / ( TRACKING_SCREEN[1] * 0.5) STEPPER.setVelocity(delta_y * 2) else: STEPPER.enable(False) index += 0.03 servo_target = 0.5 - math.sin(index) * 0.3 servo_index_x += (servo_target - servo_index_x) * 0.1 SERVO.set(0, servo_index_x) time.sleep(0.001) CAPTURE.truncate(0) # cv2.imshow("Frame", IMAGE) key = cv2.waitKey(1) & 0xFF if key == ord("q"): STEPPER.stop() STEPPER.enable(False) CONTROL_THREAD.stop() run = False break elif key == ord("r"): servo_index_x = 0.45 except KeyboardInterrupt: STEPPER.stop() STEPPER.enable(False) CONTROL_THREAD.stop() run = False
os.remove(file_list[i]) NEXT_TIME += DELAY_IMAGES CAN_TAKE_A_PHOTO = False low_res = LOW_RES_STREAM.next() IMAGE = low_res.array gray = cv2.cvtColor(IMAGE, cv2.COLOR_BGR2GRAY) # Run face detection as soon as one has ended if FACE_DETECTOR is None: FACE_DETECTOR = Thread() FACE_DETECTOR.setRunFunction(face_detection) FACE_DETECTOR.start() else: if FACE_DETECTOR.isAlive() is not True: FACE_DETECTOR = None if FACES is not None: if len(FACES) > 0: if CURRENT_FACE is None: CURRENT_FACE = FACES[0] # Always check for the closer face to the last one # in order not to jump from a face to another min_distance = float("inf") for face in FACES: distance = math.sqrt( math.pow(CURRENT_FACE[0] - face[0], 2) + math.pow(CURRENT_FACE[1] - face[1], 2)) if distance < min_distance: