THRESHOLD = (1, 28, -21, 5, -20, 35)
import sensor,image,time,gc
from pyb import LED
from pid import PID
from pyb import UART
uart = UART(3,115200,bits=8, parity=None, stop=1, timeout_char=2000, read_buf_len=1024)
def sending_data(data1):
global uart
data = bytearray([0x23, data1, 0x40])
uart.write(data);
rho_pid = PID(p=0.6, i=0, d=0.15)
theta_pid = PID(p=0.003, i=0, d=0.002)
LED(1).on()
LED(2).on()
LED(3).on()
sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQQVGA)
sensor.skip_frames(time = 2000)
clock = time.clock()
while(True):
clock.tick()
img = sensor.snapshot().binary([THRESHOLD])
line = img.get_regression([(100,100)], robust = True)
if (line):
rho_err = abs(line.rho())-img.width()/2
if line.theta()>90:
theta_err = line.theta()-180
else:
theta_err = line.theta()
img.draw_line(line.line(), color = 127)
if line.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
sending_data(int(output))
print(int(output))
pass
gc.collect()