# Edge Impulse - OpenMV Object Detection Example
import sensor, image, time,pyb,omv,math,utime,tf,lcd,gc
from pyb import UART,Pin,Timer,Servo
from machine import SPI
from umotor import UMOTOR
from pid import PID
from ultrasonic import ULTRASONIC
from button import BUTTON
from pycommon import PYCOMMON
common=PYCOMMON()
motor=UMOTOR()   #声明电机驱动，B0,B1,B4,B5
button=BUTTON()  #声明按键，梦飞openmv只有一个按键D8，因此直接内部指定了按键

lcd.init() # Initialize the lcd screen.
lcd.set_direction(2)
###################小车前后左右固定方向控制#####################
def run_forward():
    motor.run(60,60)
    time.sleep(2000) #前进2s后停止
    motor.run(0,0)

def run_back():
    motor.run(-60,-60)
    time.sleep(2000) #后退2s后停止
    motor.run(0,0)

def run_left():    #左转后前进
    motor.run(60,-60)
    time.sleep(500)  #转90度
    motor.run(60,60)
    time.sleep(2000)
    motor.run(0,0)

def run_right():   #右转后前进
    motor.run(-60,60)
    time.sleep(500)  #转90度
    motor.run(60,60)
    time.sleep(2000)
    motor.run(0,0)

def find_QR_control(img):
    objects=None
    object_s=None
    objects = img.find_qrcodes()
    if len(objects):
        object_s=common.find_max_object(objects)
        object_s=objects[0]
        pixels=int(object_s[2]*object_s[3])
        img.draw_rectangle(object_s.rect(), color = (255, 0, 0))
        img.draw_string(object_s[0],object_s[1]-10, "QR:%s"%(objects[0]).payload(),scale=1,color = (255, 0, 0))
        lcd.display(img)
        if (object_s.payload()=="1"):
            run_forward() #前进
            print("go forward")
        if (object_s.payload()=="2"):
            run_back()    #后退
            print("go back")
        if (object_s.payload()=="3"):
            run_left()    #左转
            print("go left")
        if (object_s.payload()=="4"):
            run_right()   #右转
            print("go right")
        else :
            motor.run(0,0)
##############################摄像头初始化部分#####################
sensor.reset()                         # Reset and initialize the sensor.
sensor.set_pixformat(sensor.GRAYSCALE)    # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QVGA)      # Set frame size to QVGA (320x240)
sensor.set_windowing((160, 160))       # Set 240x240 window.
clock = time.clock() # Tracks FPS. 设置一个定时器用来计算帧率
while True:
    clock.tick()
    img = sensor.snapshot()
    find_QR_control(img)
    lcd.display(img)
    print(clock.fps(), "fps", end="\n\n")
    if button.state():
        click_timer=time.ticks()          #开始计时
        while button.state():  pass       #等待按键抬起
        if time.ticks()-click_timer>2000: #按键时长超过2s
            break                         #循环退出，回到主界面
    else :
        click_timer=time.ticks()#计时更新