代码收藏家 K210红色小球跟踪云台全面开源，解决识别代码疑难问题，STM32与K210通信及舵机PID调试指南。
一、实现效果
云台可以稳定跟踪红色小球，为了方便展示，使用电脑屏幕生成小球展示。
k210小球跟踪云台
二、stm32代码实现
1.stm32与k210端串口通信
代码展示
#include "serial_port.h"
#include <stdio.h>
#include <string.h>
uint8_t rx_buffer[SERIAL_BUFFER_SIZE]; // 环形缓冲区
uint16_t rx_head = 0; // 缓冲区写入位置
uint16_t rx_tail = 0; // 缓冲区读取位置
uint8_t data_ready = 0; // 数据接收完成标志
uint8_t cx = 0; // X坐标
uint8_t cy = 0; // Y坐标
UART_HandleTypeDef *huart_serial;
void Serial_Init(UART_HandleTypeDef *huart) {
huart_serial = huart;
rx_head = 0;
rx_tail = 0;
data_ready = 0;
HAL_UART_Receive_IT(huart_serial, &rx_buffer[rx_head], 1); // 启动接收中断
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
if (huart->Instance == huart_serial->Instance) {
rx_head = (rx_head + 1) % SERIAL_BUFFER_SIZE; // 更新写入位置
data_ready = 1; // 设置数据接收标志
HAL_UART_Receive_IT(huart_serial, &rx_buffer[rx_head], 1); // 重新启动接收中断
}
}
void Serial_ProcessData(void) {
while (rx_tail != rx_head) { // 缓冲区不为空
// 检查头帧
if (rx_buffer[rx_tail] == 0xAA && rx_buffer[(rx_tail + 1) % SERIAL_BUFFER_SIZE] == 0x55) {
// 检查尾帧
uint16_t frame_end = (rx_tail + 5) % SERIAL_BUFFER_SIZE;
if (rx_buffer[frame_end] == 0xFF) {
// 提取X坐标
cx = rx_buffer[(rx_tail + 3) % SERIAL_BUFFER_SIZE];
// 提取Y坐标
cy = rx_buffer[(rx_tail + 4) % SERIAL_BUFFER_SIZE];
// 更新读取位置
rx_tail = (frame_end + 1) % SERIAL_BUFFER_SIZE;
return; // 处理完一帧数据后退出
}
}
rx_tail = (rx_tail + 1) % SERIAL_BUFFER_SIZE; // 未找到完整帧，继续查找
}
}
uint8_t Serial_IsDataReady(void) {
return data_ready;
}
void Serial_PrintCoordinates(UART_HandleTypeDef *huart) {
char buffer[50];
int length = snprintf(buffer, sizeof(buffer), "CX: %d, CY: %d\n", cx, cy);
HAL_UART_Transmit(huart, (uint8_t *)buffer, length, HAL_MAX_DELAY);
}
关键是数据处理函数，使用中断接收，接收数据储存在rx_buffer[SERIAL_BUFFER_SIZE];，接收完进入数据处理函数，关键是K210端设置的帧头是0XAA和0X55，帧尾是0XFF，只有这些正确才会被解析数据，K210端传输6个数据，AA 55 0C x坐标 y坐标 FF。xy位置储存在cx和cy，cx并设置为全局变量，通过串口打印检查。
使用 while 循环检查缓冲区是否为空（即读取位置是否等于写入位置）。
检查当前读取位置的数据是否为帧头标志（0xAA 和 0x55）。
如果找到帧头，计算帧尾位置并检查是否为帧尾标志（0xFF）。
如果找到完整的数据帧，提取X坐标和Y坐标数据，并更新缓冲区的读取位置。
如果未找到完整的数据帧，移动读取位置继续查找。
2.舵机调试代码
为了后期使用freertos，选择两个舵机分开写pid调试
舵机代码
#include "servo.h"
void SetServoAngle(TIM_HandleTypeDef *htim, uint32_t Channel, float angle) {
uint32_t pulse = (uint32_t)(500 + (2000.0 / 270.0) * angle); // 绾挎?ф槧灏?
__HAL_TIM_SET_COMPARE(htim, Channel, pulse);
}
舵机pid调试代码
// gimbal_tracking.c
#include "gimbal_tracking.h"
#include "servo.h"
#include <math.h>
// 垂直方向参数
#define DEAD_ZONE_LOW 55 // 垂直死区下限（原需求是55-65）
#define DEAD_ZONE_HIGH 65 // 垂直死区上限
#define MIN_ANGLE 30.0f // 垂直舵机最小角度
#define MAX_ANGLE 170.0f
#define MAX_STEP 2.0f // 垂直方向步长更小
static float currentAngle = 50.0f; // 垂直初始角度
//static PID_Controller1 pid_vertical = {0.015, 0.000, 0.02, 0.0, 0.0, 0}; // 独立PID参数
static PID_Controller1 pid_vertical = {0.013, 0.0, 0.013, 0.0, 0.0, 0};
void shangServoPosition(uint8_t cy)
{
// 死区检测：目标在中间区域时不动作
if (cy >= DEAD_ZONE_LOW && cy <= DEAD_ZONE_HIGH) {
pid_vertical.integral = 0; // 重置积分项
return;
}
// 计算时间间隔（毫秒）
uint32_t now = HAL_GetTick();
float dt = (now - pid_vertical.last_time) / 1000.0f;
if (dt < 0.01f) return; // 10ms最小控制周期
pid_vertical.last_time = now;
// 动态目标设定：将目标拉到死区边界
float target = (cy < DEAD_ZONE_LOW) ? DEAD_ZONE_LOW : DEAD_ZONE_HIGH;
float error = target - cy;
// PID计算 ---------------------------------------------------
// 比例项
float P = pid_vertical.Kp * error;
// 积分项（带抗饱和）
pid_vertical.integral += error * dt;
pid_vertical.integral = fminf(fmaxf(pid_vertical.integral, -30.0f), 30.0f);
float I = pid_vertical.Ki * pid_vertical.integral;
// 微分项
float D = 0;
if (dt > 0) {
D = pid_vertical.Kd * (error - pid_vertical.prev_error) / dt;
}
pid_vertical.prev_error = error;
// 合成输出并限制幅度
float output = P + I + D;
output = fminf(fmaxf(output, -MAX_STEP), MAX_STEP);
// 更新角度 ---------------------------------------------------
currentAngle += output;
currentAngle = fminf(fmaxf(currentAngle, MIN_ANGLE), MAX_ANGLE);
// 应用角度到舵机（控制通道1）
SetServoAngle(&htim2, TIM_CHANNEL_1, currentAngle);
/* 调试输出 */
// printf("CY:%d Err:%.1f Out:%.2f Ang:%.1f\n", cy, error, output, currentAngle);
}
调节思路：
根据目标物体的垂直位置（cy）调整舵机角度，使目标保持在屏幕指定区域。以下是代码的思路解析：
-
死区检测：若目标在死区（DEAD_ZONE_LOW到DEAD_ZONE_HIGH）内，重置积分项并返回，避免不必要的调整。
-
时间间隔计算：使用HAL_GetTick()获取当前时间，计算与上次计算的时间差dt，确保控制周期至少为10ms。
-
动态目标设定：根据目标位置cy，将目标拉到死区边界（DEAD_ZONE_LOW或DEAD_ZONE_HIGH），计算误差error。
-
PID计算：
-
比例项：根据误差和比例系数Kp计算比例项P。
-
积分项：累加误差并限制范围，计算积分项I。
-
微分项：根据误差变化率计算微分项D，减少振荡。
-
合成输出与限制：将P、I、D相加得到输出output，并限制其在最大步长范围内。
-
更新角度：根据output更新舵机当前角度currentAngle，并限制其在最小和最大角度范围内。
-
应用角度：调用SetServoAngle()函数，将计算得到的角度应用到舵机。
3.主函数代码
简单调用串口接收，和舵机函数
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_TIM2_Init();
MX_USART1_UART_Init();
MX_USART3_UART_Init();
/* USER CODE BEGIN 2 */
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
Serial_Init(&huart1);
// 初始化跟踪模块
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
// SetServoAngle(&htim2, TIM_CHANNEL_2, 170.0f);
// SetServoAngle(&htim2, TIM_CHANNEL_1, 50.0f);
if (Serial_IsDataReady()) {
Serial_ProcessData(); // 处理数据，更新 cx 和 cy
updateServoPosition(cx);
shangServoPosition(cy);
// 打印坐标
// SetServoAngle(&htim2, TIM_CHANNEL_1, 10.0f);
// SetServoAngle(&htim2, TIM_CHANNEL_2, 10.0f);
Serial_PrintCoordinates(&huart3);
}
// 可选：添加短延时避免 CPU 占用率过高
HAL_Delay(10); // 10ms 延时
}
/* USER CODE END 3 */
}
三、K210端代码
K210端代码是参考原教案小球跟踪代码，但是他的数据传输格式太过复杂，进行简化格式；
# 定义串口对象
from hiwonder import hw_uart
import sensor
import lcd # 确保 lcd 模块在全局范围内导入
import time
serial = hw_uart()
# 定义帧头和帧尾
FRAME_HEADER_1 = 0xAA
FRAME_HEADER_2 = 0x55
FRAME_TAIL = 0xFF
# 功能号
FUNC_NUM = 0x0C
# 发送数据函数
def send_data(x, y):
'''
发送数据格式：
0xAA, 0x55, 功能号, x, y, 帧尾
'''
tx_buffer = [
FRAME_HEADER_1, # 帧头1
FRAME_HEADER_2, # 帧头2
FUNC_NUM, # 功能号
x, # x 坐标
y, # y 坐标
FRAME_TAIL # 帧尾
]
serial.send_bytearray(tx_buffer) # 发送数据
# 初始化 LCD 和传感器
def init_sensor():
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time=100)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
lcd.init() # 初始化 LCD
# 主程序
def main():
init_sensor()
# 储存红色的 LAB 阈值
color_thresholds = [(20, 80, 20, 62, 20, 35)] # Red
print("Start Color Recognition...")
clock = time.clock()
while True:
clock.tick()
img = sensor.snapshot()
fps = clock.fps()
for threshold in color_thresholds:
blobs = img.find_blobs([threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10)
if blobs:
blob_max = max(blobs, key=lambda b: b.area())
if blob_max.area() < 200:
continue
# 画方框和中心点
img.draw_cross(blob_max.cx(), blob_max.cy())
img.draw_string(blob_max.cx() + 10, blob_max.cy() - 10, 'Red', color=(255, 255, 255))
# 发送 x 和 y 坐标
send_x = int(blob_max.cx() / 2) # 缩放到 0-255
send_y = int(blob_max.cy() / 2) # 缩放到 0-255
send_data(send_x, send_y)
img.draw_string(0, 0, "%2.1ffps" % fps, color=(0, 60, 255), scale=2.0)
lcd.display(img) # 显示在 LCD 上
if __name__ == "__main__":
main()
实现红色物体检测与坐标发送功能，通过串口通信模块定义数据帧格式，将检测到的物体中心坐标发送给STM32；图像处理模块初始化传感器和LCD，用于图像获取与显示；主程序模块记录帧率，不断获取图像，检测红色物体，绘制标记并显示帧率。
在串口通信模块中，数据帧格式被定义为包含帧头、功能号、坐标数据和帧尾的结构。具体格式如下：
帧头：由两个字节组成，分别是 0xAA 和 0x55，用于标识数据帧的开始。
功能号：一个字节，值为 0x0C，用于标识数据帧的功能或用途。
坐标数据：包含两个字节，分别是目标物体的 X 坐标和 Y 坐标，这些坐标值被缩放到 0-255 的范围内。
帧尾：一个字节，值为 0xFF，用于标识数据帧的结束。
关键就是数据帧格式，需要细心检查，调试时可以通过ttl先串口打印k210发出的数据是否准确，再接通stm32端，本次使用3d打印底座，两个270度舵机，一个K210模块，stm32c8t6最小系统板。
作者：不过 江东
