网站导航
> 技术文档 > k210红色小球跟踪云台开源，解决问题k210识别代码，stm32与k210通信，舵机pid调试。_舵机云台模型开源

k210红色小球跟踪云台开源，解决问题k210识别代码，stm32与k210通信，舵机pid调试。_舵机云台模型开源

网友: 技术文档 2025-07-30 20:36:46

一、实现效果

云台可以稳定跟踪红色小球，为了方便展示，使用电脑屏幕生成小球展示。

k210小球跟踪云台

二、stm32代码实现

1.stm32与k210端串口通信

代码展示

#include \"serial_port.h\"#include #include 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 // 垂直方向参数#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  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）调整舵机角度，使目标保持在屏幕指定区域。以下是代码的思路解析：

  1. 死区检测：若目标在死区（DEAD_ZONE_LOW到DEAD_ZONE_HIGH）内，重置积分项并返回，避免不必要的调整。

  2. 时间间隔计算：使用HAL_GetTick()获取当前时间，计算与上次计算的时间差dt，确保控制周期至少为10ms。

  3. 动态目标设定：根据目标位置cy，将目标拉到死区边界（DEAD_ZONE_LOW或DEAD_ZONE_HIGH），计算误差error。

  4. PID计算：

    • 比例项：根据误差和比例系数Kp计算比例项P。

    • 积分项：累加误差并限制范围，计算积分项I。

    • 微分项：根据误差变化率计算微分项D，减少振荡。

  5. 合成输出与限制：将P、I、D相加得到输出output，并限制其在最大步长范围内。

  6. 更新角度：根据output更新舵机当前角度currentAngle，并限制其在最小和最大角度范围内。

  7. 应用角度：调用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 和 cyupdateServoPosition(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_uartimport sensorimport lcd # 确保 lcd 模块在全局范围内导入import timeserial = hw_uart()# 定义帧头和帧尾FRAME_HEADER_1 = 0xAAFRAME_HEADER_2 = 0x55FRAME_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最小系统板。

网络标签：

相关问题