from pyb import Pin, Timer
pwma = Pin('P7')
pwmb = Pin('P8')
inverse_left=False  #change it to True to inverse left wheel
inverse_right=False #change it to True to inverse right wheel
tim = Timer(4, freq=50)
ch1 = tim.channel(1, Timer.PWM, pin=pwma)
ch2 = tim.channel(2, Timer.PWM, pin=pwmb)
ch1.pulse_width_percent(7.5)
ch2.pulse_width_percent(7.5)
def run(left_speed, right_speed):
    if inverse_left==True:
        left_speed=(-left_speed)
    if inverse_right==True:
        right_speed=(-right_speed)
    if left_speed > 0:
        ch1.pulse_width_percent(abs(left_speed))
    if right_speed > 0:
        ch2.pulse_width_percent(abs(right_speed))

import sensor, image, time
import car
from pid import PID
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(10)
sensor.set_auto_whitebal(False)
clock = time.clock()
green_threshold   = (76, 96, -110, -30, 8, 66)
#size_threshold = 2000
x_pid = PID(p=0.25, i=1, imax=10)
#h_pid = PID(p=0.05, i=0.1, imax=50)
pwmm1=0
pwmm2=0
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() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.
    blobs = img.find_blobs([green_threshold])
    p_in = Pin('P0', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻
    value0 = p_in.value() # get value, 0 or 1#读入p_in引脚的值  stoPf  p60
    p_in = Pin('P1', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻
    value1 = p_in.value() # get value, 0 or 1#读入p_in引脚的值 unstop  p61
    p_in = Pin('P2', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻
    value2 = p_in.value() # get value, 0 or 1#读入p_in引脚的值  hand   p62
    p_in = Pin('P3', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻
    value3 = p_in.value() # get value, 0 or 1#读入p_in引脚的值  zidong p63
    p_in = Pin('P4', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻
    value4 = p_in.value() # get value, 0 or 1#读入p_in引脚的值  front  p66
    p_in = Pin('P5', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻
    value5 = p_in.value() # get value, 0 or 1#读入p_in引脚的值  left   p67
    p_in = Pin('P6', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻
    value6 = p_in.value() # get value, 0 or 1#读入p_in引脚的值  turn   p65
    if value1==1#不停止标志位
        if value3==1#自动标志位
            if blobs:
                max_blob = find_max(blobs)
                x_error = max_blob[5]-img.width()/2
                print("x error: ", x_error)
                '''
                for b in blobs:
                    # Draw a rect around the blob.
                    img.draw_rectangle(b[0:4]) # rect
                    img.draw_cross(b[5], b[6]) # cx, cy
                '''
                img.draw_rectangle(max_blob[0:4]) # rect
                img.draw_cross(max_blob[5], max_blob[6]) # cx, cy
                x_output=x_pid.get_pid(x_error,1)
                #print("h_output",h_output)
                pwmm1=11-x_output
                pwmm2=11+x_output
                if (pwmm1)<8:
                    pwmm1=8
                elif (pwmm1)>11:
                    pwmm1=11
                else:
                    pwmm1=pwmm1
                if pwmm2<8:
                    pwmm2=8
                elif pwmm2>11:
                    pwmm2=11
                else:
                    pwmm2=pwmm2
                else 
		    car.run(pwmm1,pwmm2)
        if value2==1#手动标志位
            if value4==1:#前进标志位
                car.run(8,8)
            if value5==1: #左转标志位
                car.run(8,0)
            if value6==1:#右转标志位
                car.run(0,8)
    if value0==1#停止标志位
        car.run(0,0)