# import the necessary packages
from picamera.array import PiRGBArray
from picamera import PiCamera
import RPi.GPIO as GPIO
import time
import cv2
# initialize the camera and grab a reference to the raw camera capture

GPIO.setmode(GPIO.BOARD)
INT1 = 11
INT2 = 12
INT3 = 13
INT4 = 15
ENA = 16
ENB = 18

GPIO.setup(INT1,GPIO.OUT)
GPIO.setup(INT2,GPIO.OUT)
GPIO.setup(INT3,GPIO.OUT)
GPIO.setup(INT4,GPIO.OUT)
GPIO.setup(ENA,GPIO.OUT)
GPIO.setup(ENB,GPIO.OUT)

pwma = GPIO.PWM(16,80)
pwmb = GPIO.PWM(18,80)
pwma.start(90)
pwmb.start(90)
GPIO.output(INT1,GPIO.HIGH)
GPIO.output(INT2,GPIO.LOW)
GPIO.output(INT3,GPIO.HIGH)
GPIO.output(INT4,GPIO.LOW)

def right():
    pwma.ChangeDutyCycle(90)
    pwmb.ChangeDutyCycle(20)

def left():
    pwma.ChangeDutyCycle(20)
    pwmb.ChangeDutyCycle(90)

def stop():
    GPIO.output(INT1,GPIO.LOW)
    GPIO.output(INT2,GPIO.LOW)
    GPIO.output(ENA,GPIO.HIGH)
    time.sleep(1)
    GPIO.output(INT3,GPIO.LOW)
    GPIO.output(INT4,GPIO.LOW)
    GPIO.output(ENB,GPIO.HIGH)
    GPIO.cleanup()

camera = PiCamera()
camera.resolution = (640, 480)
camera.framerate = 32
camera.hflip = True
camera.vflip = True
rawCapture = PiRGBArray(camera, size=(640, 480))
# allow the camera to warmup
time.sleep(0.1)
# capture frames from the camera
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
    # grab the raw NumPy array representing the image, then initialize the timestamp
    # and occupied/unoccupied text
    crop_img = frame.array
    gray = cv2.cvtColor(crop_img, cv2.COLOR_BGR2GRAY)
    blur = cv2.GaussianBlur(gray, (15, 15), 0)
    ret, thresh1 = cv2.threshold(blur, 100, 255, cv2.THRESH_BINARY)
    mask = cv2.erode(thresh1, None, iterations=2)
    mask = cv2.dilate(mask, None, iterations=2)
    
    cnts = cv2.findContours(mask.copy(),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
    contours = cnts[0]
    if len(contours) > 0:
        c = max(contours, key=cv2.contourArea)
        M = cv2.moments(c)
        # 求取中心点坐标（cx,cy）
        cx = int(M['m10'] / M['m00'])
        cy = int(M['m01'] / M['m00'])
        cv2.line(crop_img, (cx, 0), (cx, 720), (255, 0, 0), 1)
        cv2.line(crop_img, (0, cy), (1280, cy), (255, 0, 0), 1)
        # 绘制轮廓图
        cv2.drawContours(crop_img, contours, -1, (0, 255, 0), 1)
        
        if cx>360:
            print("right")
            right()
        elif cx<360:
            print("left")
            left()
        print(cx)
    # show the frame
    cv2.imshow("Frame", crop_img)
    key = cv2.waitKey(1) & 0xFF
    # clear the stream in preparation for the next frame
    rawCapture.truncate(0)
    # if the `q` key was pressed, break from the loop
    if key == ord("q"):
        stop()
        break