# Untitled - By: mengfan_zheng - 周五 4月 15 2022
import sensor, image, time,pyb,omv,math,utime,tf,lcd,gc
from pyb import UART,Pin,Timer,Servo
from machine import SPI
from ubluetooth import CARSTATE,UBLUETOOTH
from umotor import UMOTOR
from pid import PID
from ultrasonic import ULTRASONIC
from button import BUTTON
BINARY_VISIBLE = True
# 选择跟踪色块的颜色
#THRESHOLD  = (94, 55, 15, -10, 9, 25)#黄色
##THRESHOLD  = (50, 34, 16, 39, 21, 0)#红色
#THRESHOLD  = (31, 90, -26, 2, -34, -5)#蓝色
#THRESHOLD  = (27, 9, -69, 31, 76, -31)#黑色线
#用户需调整PID参数和颜色阈值参数。可适应不同得场景
global THRESHOLD
THRESHOLD  =(10, 83)  #灰度阈值 黑线
rho_pid = PID(p=0.05, i=0.2)
theta_pid = PID(p=0.001, i=0.15)
motor=UMOTOR()   #声明电机驱动，B0,B1,B4,B5
button=BUTTON()  #声明按键，梦飞openmv只有一个按键D8，因此直接内部指定了按键

lcd.init()
lcd.set_direction(2)

def traceline_regression(img):
    car_speed=30
    line = img.get_regression([(255,255) if BINARY_VISIBLE else THRESHOLD])
    if (line):
        img.draw_line(line.line(), color = 127)
        rho_err = abs(line.rho())-img.width()/2
        if line.theta()>90:
            theta_err = abs(line.theta())-180
        else:
            theta_err = line.theta()
        print("line.magnitude",line.magnitude())
        print("line.theta",line.theta())
        print("line.rho",line.rho())
        if line.magnitude()>=2:#匹配线宽
            rho_output = rho_pid.get_pid(rho_err,1)
            theta_output = theta_pid.get_pid(theta_err,1)
            output = rho_output+theta_output
            print("output",output)
            motor.run(car_speed-output, car_speed+output)
        else:
           motor.run(0,0)
           pass
    else:
        motor.run(0,0)
        pass



#镜头配置配置
# 初始化摄像头传感器
sensor.reset() # Initialize the camera sensor.
# 设置颜色编码RGB565(每个像素用16比特位表示,占2个字节,RGB分量分别使用5位、6位、5位。)
sensor.set_pixformat(sensor.GRAYSCALE) # use RGB565.
# 设置摄像头帧分辨率为QVGA，即320*240
sensor.set_framesize(sensor.HQVGA) # use QQVGA for speed.
# 跳过50帧画面，等待设置生效（包括分辨率设置和舵机转动）
sensor.skip_frames(50) # Let new settings take affect.
# 关闭自动白平衡
#sensor.set_auto_whitebal(False) #turn this off.
#sensor.set_hmirror(True)
#sensor.set_vflip((True))
clock = time.clock() # Tracks FPS. 设置一个定时器用来计算帧率

click_timer=time.ticks() #计时参数
while(True):
    clock.tick()
    #img = sensor.snapshot().binary([THRESHOLD])
    #line = img.get_regression([(100,100,0,0,0,0)], robust = True)
    img = sensor.snapshot().binary([THRESHOLD]) if BINARY_VISIBLE else sensor.snapshot()
    traceline_regression(img)
    lcd.display(img) # Take a picture and display the image.
    print(clock.fps())
    #长按按键退出
    if button.state():
        click_timer=time.ticks()          #开始计时
        while button.state():  pass       #等待按键抬起
        if time.ticks()-click_timer>2000: #按键时长超过2s
            break                         #循环退出，回到主界面
    else :
        click_timer=time.ticks()#计时更新