【K230+stm32】二维追踪云台_stm32 k230模块例程

cubeide配置：

K230代码：

import time, os, gc, sys, mathfrom media.sensor import *from media.display import *from media.media import *from machine import UARTfrom machine import FPIOA# 检测分辨率DETECT_WIDTH = 320DETECT_HEIGHT = 240# 颜色阈值thresholds = [ (3, 78, 45, 7, -52, 51) # 红色阈值]def find_largest_blob(blobs): \"\"\"寻找面积最大的色块\"\"\" if not blobs: return None largest_blob = blobs[0] for blob in blobs[1:]: if blob.area() > largest_blob.area(): largest_blob = blob return largest_blobdef uartinit(): # 配置引脚 fpioa = FPIOA() fpioa.set_function(11, FPIOA.UART2_TXD) fpioa.set_function(12, FPIOA.UART2_RXD) # 初始化UART2，波特率115200，8位数据位，无校验，1位停止位 uart = UART(UART.UART2, baudrate=115200, bits=UART.EIGHTBITS, parity=UART.PARITY_NONE, stop=UART.STOPBITS_ONE) return uartdef send_blob_data(uart, blob): \"\"\"通过串口发送色块数据\"\"\" if blob is None: return x, y = blob.cx(), blob.cy() data = bytearray([ 0xFF, x & 0xFF, (x >> 8) & 0xFF, y & 0xFF, (y >> 8) & 0xFF, 0xFE ]) print(x) print(y) uart.write(data)def draw_blob_info(img, blob): \"\"\"绘制色块信息\"\"\" if blob is None: return # 绘制矩形框 img.draw_rectangle(blob.rect()) # 绘制中心十字 img.draw_cross(blob.cx(), blob.cy()) # 绘制旋转角度 img.draw_keypoints([(blob.cx(), blob.cy(), int(math.degrees(blob.rotation())))], size=20) def initialize_sensor(): sensor = Sensor(width=DETECT_WIDTH, height=DETECT_HEIGHT) sensor.reset() sensor.set_framesize(width=DETECT_WIDTH, height=DETECT_HEIGHT) sensor.set_pixformat(Sensor.RGB565) return sensordef main_loop(sensor): fps = time.clock() while True: fps.tick() os.exitpoint() # 检测 IDE 中断 # 拍摄图像 img = sensor.snapshot() blobs=img.find_blobs(thresholds, pixels_threshold=100, area_threshold=100, merge=True) largest_blob = find_largest_blob(blobs) send_blob_data(uart, largest_blob) draw_blob_info(img, largest_blob) # 显示图像 Display.show_image(img) # 垃圾回收 gc.collect()def cleanup(sensor): if isinstance(sensor, Sensor): sensor.stop() Display.deinit() os.exitpoint(os.EXITPOINT_ENABLE_SLEEP) time.sleep_ms(100) MediaManager.deinit()if __name__ == \"__main__\": try: sensor = initialize_sensor() uart=uartinit() Display.init(Display.VIRT, width=DETECT_WIDTH, height=DETECT_HEIGHT, fps=100) MediaManager.init() sensor.run() main_loop(sensor) except KeyboardInterrupt: print(\"User stop\") except BaseException as e: print(f\"Exception: {e}\") finally: cleanup(sensor)

stm32串口部分：

// muart.c#include \"muart.h\"#include // 模块私有变量static UART_HandleTypeDef *muart_handle;uint8_t rx_buffer[1]; // 单字节接收缓冲区static uint8_t packet_buffer[MUART_PACKET_SIZE];static uint8_t packet_index = 0;static uint8_t receiving = 0;// 数据包存储static MUART_DataPacketTypeDef packet = {0};char display_str[MUART_DISPLAY_STR_SIZE] = \"No Data\";// 私有函数声明static void MUART_ParsePacket(void);void MUART_Init(UART_HandleTypeDef *huart){ muart_handle = huart; memset(&packet, 0, sizeof(packet)); HAL_UART_Receive_IT(muart_handle, rx_buffer, 1);}void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart){ if (huart == muart_handle) { uint8_t data = rx_buffer[0]; if(!receiving && data == 0xFF) { receiving = 1; packet_index = 0; packet_buffer[packet_index++] = data; } else if(receiving) { packet_buffer[packet_index++] = data; if(packet_index >= MUART_PACKET_SIZE) { if(packet_buffer[MUART_PACKET_SIZE-1] == 0xFE) {  MUART_ParsePacket(); } receiving = 0; } } HAL_UART_Receive_IT(muart_handle, rx_buffer, 1); }}static void MUART_ParsePacket(void){ packet.x = packet_buffer[1] | (packet_buffer[2] << 8); packet.y = packet_buffer[3] | (packet_buffer[4] << 8); packet.updated = 1; snprintf(display_str, MUART_DISPLAY_STR_SIZE, \"X:%4d Y:%4d\", packet.x, packet.y);}void MUART_ProcessData(void){ // 预留数据处理接口 // 可在此添加滤波算法等}MUART_DataPacketTypeDef MUART_GetPacket(void){ MUART_DataPacketTypeDef ret = packet; packet.updated = 0; // 清除更新标志 return ret;}const char* MUART_GetDisplayString(void){ return display_str;}

PID部分:

#include \"pid.h\"void PID_Init(PID_Controller *pid, float kp, float ki, float kd,  float integral_limit, float output_limit){ pid->kp = kp; pid->ki = ki; pid->kd = kd; pid->integral_limit = integral_limit; pid->output_limit = output_limit; PID_Reset(pid);}float PID_Calculate(PID_Controller *pid, float error){ // 积分项计算 (自动限幅) pid->integral += error; if(pid->integral > pid->integral_limit) pid->integral = pid->integral_limit; if(pid->integral integral_limit) pid->integral = -pid->integral_limit; // 微分项计算 float derivative = error - pid->last_error; // PID输出计算 float output = pid->kp * error  + pid->ki * pid->integral  + pid->kd * derivative; // 输出限幅 if(output > pid->output_limit) output = pid->output_limit; if(output output_limit) output = -pid->output_limit; // 更新上次误差 pid->last_error = error; return output;}void PID_Reset(PID_Controller *pid) { pid->integral = 0; pid->last_error = 0;}

主程序:

int main(void) { /* 初始化串口 */ MUART_Init(&huart2); /* 初始化硬件 */ HAL_Init(); SystemClock_Config(); MX_GPIO_Init(); MX_TIM2_Init(); MX_TIM3_Init(); MX_USART2_UART_Init(); MX_I2C2_Init(); /* 初始化PWM和PID控制器 */ HAL_TIM_PWM_Init(&htim2); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2); PID_Controller pid_x, pid_y; PID_Init(&pid_x, 0.5f, 0.04f, 0.02f, 1.0f, 100.0f); PID_Init(&pid_y, 0.5f, 0.04f, 0.02f, 1.0f, 100.0f); int x = 160, y = 120; float x_n = 1, y_n = 1; float pwmx = 0, pwmy = 0; /* 初始化OLED */ OLED_Init(); HAL_UART_Receive_IT(&huart2, rx_buffer, 1); char pwmm[20]; char pidm[20]; float pidx = 0, pidy = 0; /* 主循环 */ while (1) { /* 新建空白缓冲区 */ OLED_NewFrame(); /* 获取串口数据包 */ MUART_DataPacketTypeDef packet = MUART_GetPacket(); if (packet.updated == 1) { float px = PID_Calculate(&pid_x, (x - packet.x)) * 1; float py = PID_Calculate(&pid_y, (y - packet.y)) * 1; pidx += px; pidy += py; /* 限制PID输出范围 */ if (pidx >= 1000) { pidx = 1000; } if (pidx = 1000) { pidy = 1000; } if (pidy <= -1000) { pidy = -1000; } /* 计算PWM值 */ pwmx = (1500 + pidx) * x_n; pwmy = (1500 + pidy) * y_n; /* 设置PWM占空比 */ __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, pwmx); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, pwmy); /* 打印调试信息 */ snprintf(pwmm, sizeof(pwmm), \"pwmx:%.0lf,pwmy:%.0lf\", pwmx, pwmy); snprintf(pidm, sizeof(pidm), \"pidx:%.0lf,pidy:%.0lf\", px, py); } /* 在OLED上显示信息 */ OLED_PrintASCIIString(0, 22, display_str, &afont8x6, OLED_COLOR_NORMAL); OLED_PrintASCIIString(0, 33, pidm, &afont8x6, OLED_COLOR_NORMAL); OLED_PrintASCIIString(0, 44, pwmm, &afont8x6, OLED_COLOR_NORMAL); OLED_ShowFrame(); }}

其中OLED部分代码并不通用，请替换成自己的OLED函数，或删去此部分。