#THRESHOLD =(0, 23, -12, 5, -5, 13)
THRESHOLD_day=(0, 30, -47, 20, -28, 19)
import sensor, image, time,math,pyb
from pyb import UART,Pin,Timer
from pyb import LED
from pid import PID
output_str=''
uart = UART(3,9600)
sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_windowing((0,60,120,60))
sensor.skip_frames(time = 2000)
clock = time.clock()
LED(1).on()
LED(2).on()
LED(3).on()
roi_X=[30,0,30,30]
roi_stra=[50,0,30,30]
rho_pid = PID(p=0.7, i=0)
theta_pid = PID(p=0.3, i=0.5,d=0.1)
while(True):
img = sensor.snapshot()
img.draw_rectangle(roi_X, color = (255, 0, 0), thickness = 2, fill = False)
img.draw_rectangle(roi_stra, color = (255, 0, 0), thickness = 2, fill = False)
blobs = img.find_blobs([THRESHOLD_day], roi=roi_X,x_stride=1, y_stride=1,pixels_threshold=10)
for blob in blobs:
img.draw_rectangle(blob.rect())
img.draw_cross(blob.cx(), blob.cy())
if blob:
blobsss = img.find_blobs([THRESHOLD_day],roi=roi_stra,x_stride=1, y_stride=1,pixels_threshold=10)
for blob in blobs:
img.draw_rectangle(blob.rect())
img.draw_cross(blob.cx(), blob.cy())
if blob:
print('go!')
output_str =bytearray([0x2C,0x12,128,128,0x5B])
#output_str='&0%d%d*' % (300, 300)
uart.write(output_str)
print(output_str)
pyb.delay(200)
img = sensor.snapshot().binary([THRESHOLD_day])
line1 = img.get_regression([(100,100)], robust = True)
if (line1):
rho_err = abs(line1.rho())-img.width()/2
if line1.theta()>90:
theta_err = line1.theta()-180
else:
theta_err = line1.theta()
img.draw_line(line1.line(), color = 127)
if line1.magnitude()>8:
rho_output = rho_pid.get_pid(rho_err,1)
theta_output = theta_pid.get_pid(theta_err,1)
output = rho_output+theta_output
output1=128-output
output2=128+output
int(output1)
int(output2)
output_str =bytearray([0x2C,0x12,output1,output2,0x5B])
#output_str='&0%d%d*' % (300-output, 300+output)
uart.write(output_str)
print(output_str)
else:
output_str =bytearray([0x2C,0x12,0,128,0x5B])
#output_str='&0000300*'
uart.write(output_str)
print(output_str)
pass
