import sensor, image, time
import pyb

from pid import PID
from pyb import Servo

pan_servo=Servo(1)
pan_servo.calibration(500,2500,500)
pan_pid = PID(p=0.04, i=0.01, imax=90) #脱机运行或者禁用图像传输，使用这个PID


pinADir0 = pyb.Pin('P3', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)
pinADir1 = pyb.Pin('P2', pyb.Pin.OUT_PP, pyb.Pin.PULL_NONE)


pinADir0.value(0)
pinADir1.value(1)


tim = pyb.Timer(4, freq=1000)

chA = tim.channel(1, pyb.Timer.PWM, pin=pyb.Pin("P8"))


sensor.reset() # Initialize the camera sensor.
sensor.set_contrast(3)
sensor.set_gainceiling(16)
sensor.set_pixformat(sensor.GRAYSCALE) # use RGB565.
sensor.set_framesize(sensor.QVGA) # use QQVGA for speed.
sensor.set_vflip(True)
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.
face_cascade = image.HaarCascade("frontalface", stages=25)

def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob


while(True):
    clock.tick()
    img = sensor.snapshot()
    faces = img.find_features(face_cascade, threshold=0.75, scale=1.25)
    if faces:

        face = find_max(faces)
        cx = int(face[0]+face[2]/2)
        cy = int(face[1]+face[3]/2)
        pan_error = cx-img.width()/2
        print("pan_error: ", pan_error)
        img.draw_rectangle(face) # rect
        img.draw_cross(cx, cy) # cx, cy
        pan_output=pan_pid.get_pid(pan_error,1)
        print("pan_output",pan_output)
        pan_servo.angle(int(pan_servo.angle()+pan_output))
        chA.pulse_width_percent(40)



    else:
        chA.pulse_width_percent(0)