23电赛E题激光追踪练习

这个练习是在暑假电赛集训期间做的，尽力而为了，但是结果还是有很大误差，仅供参考。

算是对自己学习的记录。

先看一下题目要求：

首先分析

云台分为两个，一个是红激光云台，一个是绿激光云台。分成两个看：

红激光云台需要做两件事：

第一件：绕背景大框运动，这个矩形大框的识别要求太高，所以最后选择了开环手动定点获取坐标，然后再执行，对于复位，也是如此。

第二件：绕黑色胶带矩形运动，激光在胶带上识别的效果很差，通过改变曝光度等，最后做到在上面能够识别，但是一旦运动起来，在画面中完全看不到激光的影子，所以最后选择了半开环，先识别矩形的四个角坐标，然后传给32，以pid进行激光位置的计算运动。（在这里将任务3和任务4结合了，如果第三位自己手动写死，那么精度自然更高）

绿激光云台只需要做一件事：

识别红绿激光的坐标，将绿的当作输入，红的当作目标，不断进行pid运算，其难点依然是在胶带上的识别。

下面说我的处理方法：

对于红绿激光的识别，一开始我是在正常亮度下进行，能够识别到，但是发现，从白天到晚上，开灯或不开灯，环境光线的变化会导致识别不正常。所以后面大胆将曝光度直接拉低到画面全黑，只能看到红色和激光，识别效果反而非常稳定。

import sensor, image, time
from machine import UART
from pyb import UART
import json
redblack_threshold   =(10, 63, 17, 65, 9, 50) #胶带
red_threshold =(11, 100, 16, 127, 1, 68)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing(100,40,140,140)
sensor.set_vflip(True)##上下
#sensor.set_hmirror(True)##左右
sensor.skip_frames(10)
clock = time.clock()
sensor.set_auto_whitebal(False)#禁用白平衡
#sensor.set_auto_gain(False)#关闭自动增益
t=0
p=0
d = {}
i=0
mode=0
rx_buff=[]
state = 0
f=0
uart = UART(3, 115200, timeout_char=1000)
#串口接收函数
def Receive_Prepare(da):
    global state
    if state==0:
        if da == 0x0d:#帧头
            state = 1
        else:
            state = 0
            rx_buff.clear()
    elif state==1:
        rx_buff.append(da)
        mode=da
    else:
        state = 0
        rx_buff.clear()

while(True):
    img = sensor.snapshot()
    red_blobs = img.find_blobs([red_threshold])
    redblack_blobs=img.find_blobs([redblack_threshold])

    if(uart.any()>0):
       f=uart.readchar()
       Receive_Prepare(f)
       p=mode
       mode=f

    if mode==0:
        sensor.set_auto_exposure(False,10000)#设置曝光度

        if red_blobs:
            for a in red_blobs:
                img.draw_cross(a[5], a[6])
                print(a[5],a[6],mode)
                data = bytearray([0xb3,0xb3,0,0,0,0,0,0,0,0,a[5],a[6],0x0d,0x0a])
                uart.write(data)
    if p<mode:
        t=0
    if mode>=1:

        if t==0:
            sensor.set_auto_exposure(False,94000)#设置曝光度
        rects=img.find_rects(threshold = 10000)
        if rects:
            t=1
            for b in rects:
                img.draw_rectangle(b.rect())
                i=0
                for p in b.corners():
                    d[i]=p[0]
                    d[i+1]=p[1]
                    i+=2
                print(d[0],d[1],d[2],d[3],d[4],d[5],d[6],d[7],mode)
                data = bytearray([0xb3,0xb3,d[0],d[1],d[2],d[3],d[4],d[5],d[6],d[7],70, 70,0x0d,0x0a])
                uart.write(data)
        sensor.set_auto_exposure(False,120000)#设置曝光度
        if redblack_blobs or red_blobs:
            for a in redblack_blobs or red_blobs:
                img.draw_cross(a[5], a[6])
                data = bytearray([0xb3,0xb3,0,0,0,0,0,0,0,0,a[5], a[6],0x0d,0x0a])
                uart.write(data)

对于舵机运动，进行PID控制，这是keil里的控制代码，输入量和目标量都是位置参数，x轴舵机和y轴舵机是独立的。

//PID函数
uint16_t Motorx_PID_Cal(int input_cx,int setx)
{
	static int pwmout=0,last_error=0,last_last_error=0;
	int error =setx - input_cx;	// 误差
	int d_error=error-last_error;
	int dd_error = -2*last_error+error+last_last_error; 
	 
	pwmout+=(cmd_P-1)*d_error/10 +(cmd_I-1.1)*error/10+cmd_D*dd_error/10; // 输出pwm信号
	last_last_error=last_error;
	last_error = error;
	if(pwmout > 970) pwmout=970;
	if(pwmout < 550) pwmout=550;
	comx_Set(pwmout);
	return pwmout;
}

uint16_t Motory_PID_Cal(int input_cy,int sety)
{
	static int pwmout=0,last_error=0,last_last_error=0;
	int error =sety - input_cy;	// 误差
	int d_error=error-last_error;
	int dd_error = -2*last_error+error+last_last_error; 
	 
	pwmout+=cmd_P*d_error/10 +cmd_I*error/10+cmd_D*dd_error/10; // 输出pwm信号
	last_last_error=last_error;
	last_error = error;
	if(pwmout > 1660) pwmout=1660;
	if(pwmout < 1450) pwmout=1450;
	comy_Set(pwmout);
	return pwmout;
}

最后结果：

集训电赛2023e题 激光追踪

其他的可以详见工程

23电赛e: 2023电赛e题激光追踪 (gitee.com)https://gitee.com/lin-zhiquan11/23-electric-racing-e