嵌入式激光追踪

效果同动态，响应跟踪速度太慢了，优化了半天，又是降分辨率又是改算法的又是调pid的值，结果捣鼓半天，发现不管是对图像处理，还是色块识别，k210都有对其对应的库函数，改完后发现流程了不止一点半点😂😂😂，手搓的一些垃圾算法还是记录一下

import sensor

import image

import lcd

import math

from machine import Timer,PWM

import time

olderrx=0

olderry=0

x=0

y=0

# 初始化摄像头

sensor.reset()

sensor.set_pixformat(sensor.GRAYSCALE)#灰度图像

sensor.set_framesize(sensor.QVGA)

sensor.run(1)

sensor.skip_frames()

sensor.skip_frames(time=2000) # 跳过不稳定画面

 

lcd.init(type=1,freq=15000000)

lcd.rotation(0)

#初始化PWM

tim0=Timer(Timer.TIMER0,Timer.CHANNEL0,mode=Timer.MODE_PWM)

tim1=Timer(Timer.TIMER1,Timer.CHANNEL0,mode=Timer.MODE_PWM)

pwm0=PWM(tim0,freq=50,duty=7.5,pin=15,enable=True)#原点上下舵机都为90度

pwm1=PWM(tim1,freq=50,duty=7.5,pin=16,enable=True)

 

 

#获取光斑中心点像素坐标函数

def set_img(Image):

    max_brightness = 240

    arrx=[]

    arry=[]

    for x in range(Image.width()):

        for y in range(Image.height()):

         pixel_value = Image.get_pixel(x, y)

         if pixel_value > max_brightness:

            Image.set_pixel(x,y,255)

            arrx.append(x)

            arry.append(y)

         else:

            Image.set_pixel(x,y,0)

    x_len=len(arrx)

    y_len=len(arry)

    getx=int(sum(arrx))

    gety=int(sum(arry))

    arrx.clear()

    arry.clear()

    return getx,gety,x_len,y_len

 

#PID算法

def My_PIDx(target,origin):

    global olderrx

    Kp=0.07

    Kd=0.001

    err=target-origin #误差值

    cerrx=err-olderrx

    olderrx=err

    return Kp*err+Kd*cerrx

 

 

def My_PIDy(target,origin):

        global olderry

        Kp=0.07

        Kd=0.001

        err=origin-target #误差值

        cerry=err-olderry

        olderry=err

        return Kp*err+Kd*cerry

 

 

while(True):

    img_origin = sensor.snapshot() # 获取灰度图像,初始化坐标原点为红色激光点

    get_origin=set_img(img_origin)#返回元组，set_img_init中：0=x原点sum,1=y原点sum

    if get_origin[2]==0 or get_origin[3]==0:

        continue

    x_o=int(get_origin[0]/get_origin[2])

    y_o=int(get_origin[1]/get_origin[3])

    print(x_o)#原点x

    print(y_o)#原点y

    while(True):

         img = sensor.snapshot() # 获取灰度图像

         get_target=set_img(img)

         if get_target[2]-get_origin[2]!=0 and get_target[3]-get_origin[3]!=0:

            x_t=int((get_target[0]-get_origin[0])/(get_target[2]-get_origin[2]))#目标点x的像素坐标

            y_t=int((get_target[1]-get_origin[1])/(get_target[3]-get_origin[3]))#目标点y的像素坐标

            if abs(x_t-x_o)<1 and abs(y_t-y_o)<1:

                x=0

                y=0

            else:

                x+=My_PIDx(x_t,x_o)

                y+=My_PIDy(y_t,y_o)

                if x>=-135 and y>=-135:

                    pwm1.duty((x+90)/180*10+2.5)

                    pwm0.duty((y+90)/180*10+2.5)

         lcd.display(img)