main.py
import pyb, sensor, time, image
from pyb import Pin,Timer
LED_b = pyb.LED(3)
LED_g = pyb.LED(2)
Servo_PWM = Pin('P7')
pwma = Pin('P8')
pwmb = Pin('P9')
tim1 = Timer(4, freq=400)
ch1 = tim1.channel(1, Timer.PWM, pin=Servo_PWM)
ch2 = tim1.channel(2, Timer.PWM, pin=pwma)
ch3 = tim1.channel(3, Timer.PWM, pin=pwmb)
ch1.pulse_width_percent(60)
ch2.pulse_width_percent(0)
ch3.pulse_width_percent(0)
thresholds = [(90, 100, -128, 127, -128, 127)]
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 50)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False, 1400)
clock = time.clock()
def DrawImg():
	img = sensor.snapshot()
	blobs = img.find_blobs([thresholds[0]])
	if len(blobs)==1:
		b = blobs[0]
		img.draw_rectangle(b[0:4])
		img.draw_cross(b[5], b[6])
		return b
K = 290
def DistenceMeasure(b):
	Lm = (b[2]+b[3])/2
	Distence = K/Lm
	return Distence
def Angle(b):
	delta_x = b[5] - 160
	Sita= delta_x*180/320+135
	return Sita
def MY_Angle(Sita):
	Servo_ht = Sita/270*2+0.5
	Servo_pwm = Servo_ht/2.5*100
	ch1.pulse_width_percent(int(Servo_pwm))
while True:
	clock.tick()
	b1 = DrawImg()
	if(b1 == None):
		LED_g.off()
		LED_b.on()
	else:
		LED_b.off()
		LED_g.on()
		D = DistenceMeasure(b1)
		Sita = Angle(b1)
		Servo_ht = Sita/270*2+0.5
		Servo_pwm = Servo_ht/2.5*100
		ch1.pulse_width_percent(int(Servo_pwm))
		print(Servo_pwm)

encoder2 = Pin('P3', Pin.OUT_PP)
extint1 = pyb.ExtInt(encoder1,pyb.ExtInt.IRQ_RISING_FALLING,pyb.Pin.PULL_UP, EncoderCount)