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
def waitUntilBoot():
print("Waiting 20 sec")
time.sleep(20)
print("Start")
main()
waitThread = Thread()
waitThread.setRunFunction(waitUntilBoot)
waitThread.start()
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
LOCK.release()
SERVO.set(0, servo_index_x)
time.sleep(0.001)
except KeyboardInterrupt:
print("Control thread ended!")
# 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),
