mv————xunji 代码003

最新推荐文章于 2024-11-05 15:28:12 发布

ooolmf

最新推荐文章于 2024-11-05 15:28:12 发布

阅读量147

点赞数

分类专栏： mv 文章标签： python

本文链接：https://blog.csdn.net/lmf666/article/details/130775953

版权

mv 专栏收录该内容

12 篇文章 0 订阅

订阅专栏

该代码实现了一个PID控制器，用于处理从RGB565图像传感器获取的数据，通过检测ROI区域的像素值进行目标跟踪。PID算法计算控制值，通过UART通信接口发送到外部设备，同时限制输出在特定范围内。系统还包含了图像显示和Blob检测功能，用于识别和定位目标。

摘要由CSDN通过智能技术生成

import sensor, image, time
from pyb import UART, LED
import json
class PID:
    def __init__(self, limit, target, feedback, Kp, Ki, Kd, e_0, e_1, e_2):
        self.limit=limit
        self.target=target
        self.feedback=feedback
        self.Kp=Kp
        self.Ki=Ki
        self.Kd=Kd
        self.e_0=e_0
        self.e_1=e_1
        self.e_2=e_2
    def pid_calc(self):
        self.e_0 = self.target - self.feedback
        ep = self.e_0  - self.e_1
        ei = self.e_0
        ed = self.e_0 - 2*self.e_1 + self.e_2
        out = self.Kp*ep + self.Ki*ei + self.Kd*ed
        out = min(max(out, -self.limit), self.limit)
        self.e_2 = self.e_1
        self.e_1 = self.e_0
        return out
pid = PID(100, 0, 0,
    1.2, 0.002, 0,
    0, 0, 0)
led=LED(1)
led.on()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
clock = time.clock()
uart = UART(3, 115200)
control_val=0
control_val=0
MIN_LIMIT=-300
MAX_LIMIT=300
ans_x=0
ans_y=160
Roi=[ans_x,ans_y,320,50]
aaa=28
while(True):
    quan=0;
    quan_id=-1
    img = sensor.snapshot(1.8)
    blobs = img.find_blobs([(5, 32, -21, 11, -14, 24)],roi=Roi,pixels_threshold = 24,area_threshold = 5,merge = True)
    img.draw_rectangle(Roi)
    for b in blobs:
        if quan<b.pixels():
            quan=b.pixels()
            quan_id=b
    if quan > 0:
        x = quan_id[0]
        y = quan_id[1]
        width = quan_id[2]
        height = quan_id[3]
        img.draw_rectangle([x,y,width,height])
        img.draw_cross(quan_id[5], quan_id[6])
        pid.feedback=x
        pid.target=160
        control_val += int(pid.pid_calc())
        control_val = max(min(control_val, MAX_LIMIT), MIN_LIMIT)
        print(control_val)
        control_val+=556
        uart.write(aaa.to_bytes(1,'int')+control_val.to_bytes(2,'int'))
        control_val-=556