main.py

import VideoDrone as vd
import time

drone = vd.VideoDrone()
last_check = time.time()
drone.start_stream()
drone.takeoff()

# Constants (cm/s)
MAX_SPEED_LR = 25
MAX_SPEED_FB = 50
MAX_SPEED_UD = 50
DEPTH_SCALAR = 1
EXP = 1

try:
    start = time.time()
    while True:
    
        # if time.time() - last_check > 1:
        #     #drone.check_safety()
        #     last_check = time.time()

        # horizontal/vertical Movement (no rotation)
        z = 1
        
        # nose coordinate
        points = drone.detect_face()
        if points is not None:
            x = points['nose'].x
            y = points['nose'].y
            z = drone.get_distance()

            # Protect against rare instance where x is negative (this can output imaginary numbers with fractional exponents)
            if x < 0:
                x = 0       
            lr_velocity = round((MAX_SPEED_LR/(2*z**DEPTH_SCALAR))*(((2*x)-1)**EXP))

            # Protect against rare instance where y is negative (this can output imaginary numbers with fractional exponents)
            if y < 0:
                y = 0
            ud_velocity = round((-MAX_SPEED_UD/(2*z**DEPTH_SCALAR))*(((2*y)-0.75)**EXP))

            fb_velocity = round((-MAX_SPEED_FB)*(((2*z)-1)**EXP))

        else:
            lr_velocity = 0
            ud_velocity = 0
            fb_velocity = 0

        # velocities
        #fb_velocity = round((-MAX_SPEED_FB)*(((2*z)-1)**EXP))
        #fb_velocity = 0
    
        print("Velocity (lr, ud, fb) : " + str((lr_velocity, ud_velocity, fb_velocity)))
        drone.send_rc_control(lr_velocity, fb_velocity, ud_velocity, 0)

        #print(drone.get_distance())

        if time.time() - start > 30:
            print("times up")
            drone.land()
            drone.streamoff()
            break

except KeyboardInterrupt:
    drone.land()
    drone.streamoff()
    pass