MSPM0开发学习笔记：二维云台结合openmv实现小球追踪

文章目录

前言
一、硬件选择
二、原理介绍（UART）
三、硬件连线
三、软件代码
- 1、视觉部分代码（Openart）
- 2、控制部分代码（MSPM0）
- - （1） UART部分
  - （2）计算函数部分
  - （3）控制部分

前言

这篇博客的代码是博主在备赛电赛的时候写来练手的，结果今年电赛的题目真的差不多，是一个自动瞄准追踪装置，因此在比赛结束之后也是用这些代码写一下这篇博客。方案是两个42步进电机采用同一个驱动模块进行驱动（D36A）,主控肯定采用MSPM0G3507。然后3D打印了一个二维云台的结构并进行组装，视觉方面采用openart (后面比赛的时候还是选了树莓派。openart的帧率实在太低了，最多才10-20帧)。本章博客主要是讲这个结合openart和云台进行小球追踪的思路以及代码。
其实按赛题的要求，云台的控制端是可以不使用mspm0系列的芯片进行控制的，完全可以采用openart或者其他模块直接进行控制，但是由于之前写的代码都是mspm0的，所以博主这边还是采用了mspm0的芯片进行控制（由于还有小车的循迹方面，一块mspm0的引脚甚至不够用，最后用了两块mspm0，一块控制小车一块控制云台）

如果无法很好的复现博客里的代码，可以私信作者博取源代码

一、硬件选择

主控：MSPM0G3507
驱动：D36A双路步进电机驱动
电机：42步进电机*2
视觉：Openart

二、原理介绍（UART）

这边主要讲UART的通信原理，UART 是短距离设备间常用的异步串行通信协议，无需同步时钟，仅通过 TX（发送）和 RX（接收）两根信号线实现双向通信，本装置中 Openart 与 MSPM0G3507 即通过它传输小球位置信息。
其系统含发送器与接收器：发送器将并行数据转为串行数据经 TX 发送，接收器通过 RX 接收串行数据并还原为并行数据。
数据以帧传输，含起始位（1 位低电平，表传输开始）、数据位（5-9 位，常为 8 位有效信息）、校验位（可选，用于校验准确性）、停止位（1-2 位高电平，表传输结束）。
波特率（单位 bps）是关键参数，表每秒传输的二进制位数，通信双方需一致（如本装置约定 115200bps），否则会出错。
实际应用中，Openart 识别小球后，按约定帧格式经 TX 发送位置信息；MSPM0G3507 通过 RX 接收并解析，计算云台转动角度，控制步进电机完成追踪。

三、硬件连线

云台部分硬件连线部分在前几篇篇博客里面已经说过了，可以直接去上一篇里面看，这边附上链接
MSPM0开发学习笔记：D36A驱动的42步进电机二维云台（2025电赛附源代码及引脚配置）
这边讲一下openart和mspm0之间的连线，两个设备之间采用uart进行通讯，openart上面是有专门用于通信的一个4p接口的。
在这里插入图片描述
从左到右分别是5V GND TX RX；我们采用的方案是openart只负责传递偏差距离，在mspm0中进行pid计算以及云台的控制（这个是为了之后如果openart不适用的话换视觉模块更加方便，事实证明确实应该这样处理，同样的条件下openart只能跑到15帧左右，但是树莓派连接摄像头可以跑到100帧，并且通讯的延迟也更低）

用4p的线接出来之后直接连到mspm0上面的对应引脚（注意TX要接RX，RX接TX）就好了（需要选用使用与UART通信的GPIO引脚，引脚之后在代码中初始化为GPIO引脚的RX和TX）

然后连在mspm0上的线有一点要注意就是，在引脚充足的情况下，尽量避免共用的引脚（下图左边有连线的这些），因为说不准会出什么问题导致后续排查很久。
在这里插入图片描述

三、软件代码

1、视觉部分代码（Openart）

视觉部分采用Python语言实现，IDE采用Openmv
具体代码如下

import sensor
import image
import time
from machine import UART# 初始化UART，波特率115200，对应设备端口可根据实际调整
uart = UART(2, 115200)# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)  # 320x240分辨率
sensor.skip_frames(time=2000)
sensor.set_auto_gain(False)  # 关闭自动增益，避免颜色识别受光强影响
sensor.set_auto_whitebal(False)  # 关闭自动白平衡# 红色HSV阈值范围（可根据实际环境调整）
red_threshold = (30, 100, 15, 127, 15, 127)# 图像中心点坐标
center_x = 160
center_y = 120while True:img = sensor.snapshot()# 查找红色色块blobs = img.find_blobs([red_threshold], pixels_threshold=200, area_threshold=200)if blobs:# 取最大的色块作为目标largest_blob = max(blobs, key=lambda b: b.area())# 计算色块中心坐标blob_x = largest_blob.cx()blob_y = largest_blob.cy()# 计算与中心点的偏差dx = blob_x - center_xdy = blob_y - center_y# 按要求处理偏差值（加500，确保传输为正数）send_dx = dx + 500send_dy = dy + 500# 格式化发送字符串data_str = "X{}{}Y".format(send_dx, send_dy)# 通过UART发送数据uart.write(data_str + "\r\n")print("发送数据：", data_str)  # 调试用time.sleep(0.05)  # 控制发送频率

代码解释：

red_threshold = (30, 100, 15, 127, 15, 127）这边是颜色阈值，需要根据实际情况进行调整，稳定性不高说实话，因此也可以考虑yolo算法的识别，之后有机会的话也出一期，难度是相对来说比较低的，就是过程会比较麻烦一些。
颜色阈值可以通过openmv IDE自带的阈值编辑器来得到
在这里插入图片描述
在工具这边选机器视觉，然后里面有一个阈值编辑器，打开界面如下

白色的是被跟踪的像素，通过调整滑块让需要的颜色变成白色就可以。
也可以选用灰度编辑器（上图用的是LAB）但是灰度编辑器一般来说用于筛选黑白会更合适（比如这次电赛E题的白色靶子和黑色边框）
在这里插入图片描述
选完之后吧阈值复制一下黏贴到代码的对应位置就可以了
不过openmvIDE只提供LAB和灰度这两种阈值编辑器，如果有需要别的比如HSV或者BRG这些的是可以自己用opencv写一个程序来进行筛选的。
这边也是写了一个简单的阈值编辑器，完整代码如下：

import cv2
import numpy as np
import tkinter as tk
from tkinter import filedialog
from PIL import Image, ImageTkclass ThresholdEditor:def __init__(self, root):self.root = rootself.root.title("OpenCV阈值编辑器")self.root.geometry("1200x800")self.root.minsize(1000, 700)# 初始化变量self.image = Noneself.video_capture = Noneself.is_camera_active = Falseself.color_mode = "BGR"  # 默认模式# 创建UI组件self.create_widgets()# 初始化滑块值self.init_slider_values()def create_widgets(self):# 创建顶部控制区control_frame = tk.Frame(self.root, padx=10, pady=5)control_frame.pack(fill=tk.X)# 输入源选择input_frame = tk.Frame(control_frame)input_frame.pack(side=tk.LEFT, padx=10)tk.Label(input_frame, text="输入源:", font=("Arial", 10, "bold")).pack(side=tk.LEFT, padx=5)self.camera_btn = tk.Button(input_frame, text="启动摄像头", command=self.toggle_camera,bg="#4CAF50", fg="white", padx=8)self.camera_btn.pack(side=tk.LEFT, padx=5)self.image_btn = tk.Button(input_frame, text="选择图片", command=self.load_image,bg="#2196F3", fg="white", padx=8)self.image_btn.pack(side=tk.LEFT, padx=5)# 颜色模式选择mode_frame = tk.Frame(control_frame)mode_frame.pack(side=tk.LEFT, padx=20)tk.Label(mode_frame, text="颜色模式:", font=("Arial", 10, "bold")).pack(side=tk.LEFT, padx=5)self.mode_var = tk.StringVar(value="BGR")modes = ["BGR", "HSV", "灰度", "LAB"]mode_menu = tk.OptionMenu(mode_frame, self.mode_var, *modes, command=self.change_mode)mode_menu.config(width=8)mode_menu.pack(side=tk.LEFT, padx=5)# 创建滑块区域（带滚动条）slider_container = tk.Frame(self.root)slider_container.pack(fill=tk.X, padx=10, pady=5)self.slider_canvas = tk.Canvas(slider_container)scrollbar = tk.Scrollbar(slider_container, orient="horizontal", command=self.slider_canvas.xview)self.slider_frame = tk.Frame(self.slider_canvas)self.slider_frame.bind("<Configure>",lambda e: self.slider_canvas.configure(scrollregion=self.slider_canvas.bbox("all")))self.slider_canvas.create_window((0, 0), window=self.slider_frame, anchor="nw")self.slider_canvas.configure(xscrollcommand=scrollbar.set)self.slider_canvas.pack(side="left", fill="x", expand=True)scrollbar.pack(side="bottom", fill="x")# 创建图像显示区域display_frame = tk.Frame(self.root)display_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=5)self.original_frame = tk.Frame(display_frame)self.original_frame.pack(side=tk.LEFT, fill=tk.BOTH, expand=True, padx=5)tk.Label(self.original_frame, text="原图", font=("Arial", 10, "bold")).pack()self.original_label = tk.Label(self.original_frame, bg="#f0f0f0")self.original_label.pack(fill=tk.BOTH, expand=True)self.processed_frame = tk.Frame(display_frame)self.processed_frame.pack(side=tk.RIGHT, fill=tk.BOTH, expand=True, padx=5)tk.Label(self.processed_frame, text="阈值处理后", font=("Arial", 10, "bold")).pack()self.processed_label = tk.Label(self.processed_frame, bg="#f0f0f0")self.processed_label.pack(fill=tk.BOTH, expand=True)def init_slider_values(self):# 清除现有滑块for widget in self.slider_frame.winfo_children():widget.destroy()# 根据颜色模式创建滑块if self.color_mode in ["BGR", "HSV", "LAB"]:# 三个通道的低阈值self.low_vars = {}for channel in self.get_channels():frame = tk.Frame(self.slider_frame)frame.pack(side=tk.LEFT, padx=10)var = tk.IntVar(value=0)self.low_vars[channel] = vartk.Label(frame, text=f"低{channel}:", width=8).pack(anchor=tk.W)slider = tk.Scale(frame, from_=0, to=self.get_max_value(channel), variable=var, orient=tk.HORIZONTAL, length=200,command=lambda _: self.update_thresholds())slider.pack()tk.Label(frame, textvariable=var, width=5, borderwidth=2, relief="sunken").pack(pady=5)# 三个通道的高阈值self.high_vars = {}for channel in self.get_channels():frame = tk.Frame(self.slider_frame)frame.pack(side=tk.LEFT, padx=10)max_val = self.get_max_value(channel)var = tk.IntVar(value=max_val)self.high_vars[channel] = vartk.Label(frame, text=f"高{channel}:", width=8).pack(anchor=tk.W)slider = tk.Scale(frame, from_=0, to=max_val, variable=var, orient=tk.HORIZONTAL, length=200,command=lambda _: self.update_thresholds())slider.pack()tk.Label(frame, textvariable=var, width=5, borderwidth=2, relief="sunken").pack(pady=5)elif self.color_mode == "灰度":# 灰度模式只有一个通道self.gray_low_var = tk.IntVar(value=0)self.gray_high_var = tk.IntVar(value=255)# 低阈值frame = tk.Frame(self.slider_frame)frame.pack(side=tk.LEFT, padx=10)tk.Label(frame, text="低阈值:", width=8).pack(anchor=tk.W)slider = tk.Scale(frame, from_=0, to=255, variable=self.gray_low_var, orient=tk.HORIZONTAL, length=200,command=lambda _: self.update_thresholds())slider.pack()tk.Label(frame, textvariable=self.gray_low_var, width=5, borderwidth=2, relief="sunken").pack(pady=5)# 高阈值frame = tk.Frame(self.slider_frame)frame.pack(side=tk.LEFT, padx=10)tk.Label(frame, text="高阈值:", width=8).pack(anchor=tk.W)slider = tk.Scale(frame, from_=0, to=255, variable=self.gray_high_var, orient=tk.HORIZONTAL, length=200,command=lambda _: self.update_thresholds())slider.pack()tk.Label(frame, textvariable=self.gray_high_var, width=5, borderwidth=2, relief="sunken").pack(pady=5)def get_channels(self):if self.color_mode == "BGR":return ["B", "G", "R"]elif self.color_mode == "HSV":return ["H", "S", "V"]elif self.color_mode == "LAB":return ["L", "A", "B"]return []def get_max_value(self, channel=None):if self.color_mode == "HSV":# H通道范围是0-179，S和V是0-255return 179 if channel == "H" else 255return 255def change_mode(self, mode):self.color_mode = modeself.init_slider_values()self.update_thresholds()def toggle_camera(self):if self.is_camera_active:# 关闭摄像头if self.video_capture:self.video_capture.release()self.video_capture = Noneself.is_camera_active = Falseself.camera_btn.config(text="启动摄像头", bg="#4CAF50")else:# 打开摄像头self.video_capture = cv2.VideoCapture(0)if not self.video_capture.isOpened():tk.messagebox.showerror("错误", "无法打开摄像头，请检查设备是否正常")self.video_capture = Nonereturnself.is_camera_active = Trueself.camera_btn.config(text="关闭摄像头", bg="#f44336")self.image = None  # 清除已加载的图像self.update_frame()  # 开始更新帧def load_image(self):# 关闭摄像头（如果开启）if self.is_camera_active:self.toggle_camera()# 选择并加载图片file_path = filedialog.askopenfilename(filetypes=[("图像文件", "*.png;*.jpg;*.jpeg;*.bmp;*.gif")])if file_path:self.image = cv2.imread(file_path)if self.image is None:tk.messagebox.showerror("错误", "无法加载选中的图片")returnself.update_thresholds()def update_frame(self):if self.is_camera_active and self.video_capture.isOpened():ret, frame = self.video_capture.read()if ret:self.image = frameself.update_thresholds()# 继续更新帧self.root.after(30, self.update_frame)def update_thresholds(self):if self.image is None:return# 复制原图用于显示original = self.image.copy()# 根据颜色模式转换图像并应用阈值if self.color_mode == "HSV":processed = cv2.cvtColor(self.image, cv2.COLOR_BGR2HSV)low = np.array([self.low_vars["H"].get(),self.low_vars["S"].get(),self.low_vars["V"].get()])high = np.array([self.high_vars["H"].get(),self.high_vars["S"].get(),self.high_vars["V"].get()])mask = cv2.inRange(processed, low, high)result = cv2.bitwise_and(original, original, mask=mask)elif self.color_mode == "灰度":processed = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY)low = self.gray_low_var.get()high = self.gray_high_var.get()_, result = cv2.threshold(processed, low, high, cv2.THRESH_BINARY)# 转换回BGR以便与原图格式一致result = cv2.cvtColor(result, cv2.COLOR_GRAY2BGR)elif self.color_mode == "LAB":processed = cv2.cvtColor(self.image, cv2.COLOR_BGR2LAB)low = np.array([self.low_vars["L"].get(),self.low_vars["A"].get(),self.low_vars["B"].get()])high = np.array([self.high_vars["L"].get(),self.high_vars["A"].get(),self.high_vars["B"].get()])mask = cv2.inRange(processed, low, high)result = cv2.bitwise_and(original, original, mask=mask)else:  # BGR模式low = np.array([self.low_vars["B"].get(),self.low_vars["G"].get(),self.low_vars["R"].get()])high = np.array([self.high_vars["B"].get(),self.high_vars["G"].get(),self.high_vars["R"].get()])mask = cv2.inRange(self.image, low, high)result = cv2.bitwise_and(original, original, mask=mask)# 显示图像self.display_image(original, self.original_label)self.display_image(result, self.processed_label)def display_image(self, img, label):# 调整图像大小以适应窗口img = self.resize_image(img, label)# 转换OpenCV图像格式为Tkinter可用格式img_rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)pil_img = Image.fromarray(img_rgb)tk_img = ImageTk.PhotoImage(image=pil_img)# 更新标签图像label.config(image=tk_img)label.image = tk_img  # 保持引用，防止被垃圾回收def resize_image(self, img, label):# 获取显示区域大小display_width = label.winfo_width()display_height = label.winfo_height()# 如果窗口还没初始化，使用默认大小if display_width <= 1 or display_height <= 1:display_width = 400display_height = 300# 计算调整比例h, w = img.shape[:2]ratio = min(display_width / w, display_height / h)# 调整大小if ratio < 1:new_size = (int(w * ratio), int(h * ratio))return cv2.resize(img, new_size, interpolation=cv2.INTER_AREA)return imgif __name__ == "__main__":root = tk.Tk()# 确保中文显示正常app = ThresholdEditor(root)root.mainloop()

注意选取图片的时候，图片路径不要有中文。具体效果如下，退出之前记得记下调好的阈值
在这里插入图片描述
然后还有一个注意的点就是，把所有的误差值都加上五百，是为了防止负号或十位数个位数的误差导致UART传输过程或传输后的解码读取过程出错，保证是三位正数，之后在MSPM0端的代码解析之后再减去500就好。

2、控制部分代码（MSPM0）

（1） UART部分

初始化部分

#define UART_INDEX              (UART_2   )                           // 默认 UART_1
#define UART_BAUDRATE           (DEBUG_UART_BAUDRATE)                           // 默认 115200
#define UART_TX_PIN             (UART2_TX_B15  )                           // 默认 UART0_TX_A10
#define UART_RX_PIN             (UART2_RX_B16  )                           // 默认 UART1_RX_A11#define UART_PRIORITY           (UART0_INT_IRQn)                                // 对应串口中断的中断编号 在 MIMXRT1064.h 头文件中查看 IRQn_Type 枚举体uint8 uart_get_data[64];                                                        // 串口接收数据缓冲区
uint8 fifo_get_data[64];                                                        // fifo 输出读出缓冲区uint8 get_data = 0;                                                             // 接收数据变量
uint32 fifo_data_count = 0;                                                     // fifo 数据个数fifo_struct uart_data_fifo;

相关函数定义部分

void uart_rx_interrupt_handler (uint32 state, void *ptr)
{ 
//    get_data = uart_read_byte(UART_INDEX);                                    // 接收数据 while 等待式 不建议在中断使用uart_query_byte(UART_INDEX, &get_data);                                     // 接收数据 查询式 有数据会返回 TRUE 没有数据会返回 FALSEfifo_write_buffer(&uart_data_fifo, &get_data, 1);                           // 将数据写入 fifo 中
}// 解析结果结构体
typedef struct {bool valid;           // 数据是否有效int first_num;        // 前三位数字int second_num;       // 后三位数字
} ParseResult;// 解析格式为XnnnnnnY的数据（X开头，Y结尾，中间6位数字）
ParseResult parse_xy_data(const char *data) {ParseResult result = {false, 0, 0};// 检查数据长度是否正确（X + 6位数字 + Y 共8个字符）if (strlen(data) != 8) {return result;}// 检查开头是否为'X'，结尾是否为'Y'if (data[0] != 'X' || data[7] != 'Y') {return result;}// 提取中间6位数字并检查是否都是数字for (int i = 1; i <= 6; i++) {if (data[i] < '0' || data[i] > '9') {return result;}}// 提取前三位数字char first_str[4] = {0};strncpy(first_str, &data[1], 3);result.first_num = atoi(first_str);// 提取后三位数字char second_str[4] = {0};strncpy(second_str, &data[4], 3);result.second_num = atoi(second_str);// 标记为有效数据result.valid = true;return result;
}

主函数部分

uint8 gpio_status;
int main (void)
{//SYSCFG_DL_init();clock_init(SYSTEM_CLOCK_80M);   // 时钟配置及系统初始化<务必保留>d36a_init();//debug_init();                   // 调试端口初始化// 此处编写用户代码 例如外设初始化代码等fifo_init(&uart_data_fifo, FIFO_DATA_8BIT, uart_get_data, 64);              // 初始化 fifo 挂载缓冲区uart_init(UART_INDEX, UART_BAUDRATE, UART_TX_PIN, UART_RX_PIN);             // 初始化串口uart_set_interrupt_config(UART_INDEX, UART_INTERRUPT_CONFIG_RX_ENABLE);		// 使能串口接收中断interrupt_set_priority(UART_PRIORITY, 0);                                   // 设置对应 UART_INDEX 的中断优先级为 0uart_set_callback(UART_INDEX, uart_rx_interrupt_handler, NULL);			    // 定义中断接收函数// 参数说明：通道、模式、kp（比例）、kpp（二次项）、ki（积分）、kd（微分）、kdd（额外项）、最大输出限制pid_init(PID_CH_X, PID_POSITIONAL, 0.6f, 0.0f, 0.02f, 0.2f, 0.0f, 200.0f);  // X方向PIDpid_init(PID_CH_Y, PID_POSITIONAL, 0.4f, 0.0f, 0.15f, 0.0f, 0.0f, 100.0f);  // Y方向PID// 注：实际参数（0.5f等）需要根据硬件调试，max_limit是输出最大值（如舵机角度范围）uart_write_string(UART_INDEX, "UART Text.");                                // 输出测试信息uart_write_byte(UART_INDEX, '\r');                                          // 输出回车uart_write_byte(UART_INDEX, '\n');                                          // 输出换行// 此处编写用户代码 例如外设初始化代码等while(true){fifo_data_count = fifo_used(&uart_data_fifo);                   // 查看FIFO是否有数据
if(fifo_data_count != 0)                                        // 有数据可读
{// 从FIFO读取数据（最多读取31字节，留1字节给终止符）uint32_t read_len = (fifo_data_count > 31) ? 31 : fifo_data_count;fifo_read_buffer(&uart_data_fifo, fifo_get_data, &read_len, FIFO_READ_AND_CLEAN);// 添加字符串终止符（确保parse_xy_data能正确识别字符串结尾）fifo_get_data[read_len] = '\0';// 解析接收到的数据ParseResult res = parse_xy_data((const char*)fifo_get_data);// 定义发送缓冲区（避免栈溢出，固定长度足够存储响应）char response[50];if (res.valid){// 解析成功：格式化响应（例如"138,118"）//sprintf(response, "%d,%d\r\n", res.first_num, res.second_num);//uart_write_string(UART_INDEX, response);int dx = res.first_num - 128;  // 前三位数字减128int dy = res.second_num - 128; // 后三位数字减128float pid_out_x = pid_calculate(PID_CH_X, (float)dx);  // X方向PID输出float pid_out_y = pid_calculate(PID_CH_Y, (float)dy);  // Y方向PID输出// 示例：计算output_x=10时的舵机控制量int ddx = output_to_servo(dx);int ddy = output_to_servo(dy);				// 5. 使用PID输出（示例：发送到串口查看结果）float servo_x=ddx*16;float servo_y=ddy*-16;int speed_x=map_0_200_to_1000_300(servo_x);int speed_y=map_0_200_to_1000_300(servo_y);int speed=min_int(speed_x,speed_y);d36a_set_angle_both(servo_y,servo_x,speed);//d36a_set_angle(D36A_MOTOR_B,servo_x,300);sprintf(response, "dx=%d, dy=%d | sex=%d, sey=%d \r\n",dx, dy, (int)servo_x, (int)servo_y);uart_write_string(UART_INDEX, response);}}system_delay_ms(10);}
}

（2）计算函数部分

int32_t map_0_200_to_1000_300(int32_t input) {// 限制输入值在0-200范围内if (input < 0) {input = 0;} else if (input > 200) {input = 200;}// 线性映射公式：output = (input - in_min) * (out_max - out_min) / (in_max - in_min) + out_min// 这里是反向映射，1000到300int32_t output = 700 - (input * 550) / 200;return output;
}int min_int(int a, int b) {if (a < b) {return a;} else {return b;}
}int output_to_servo(float output_x)
{// 1. 计算atan2的第一个参数：output_x * 1.8 / 66float numerator = output_x * 1.8f / 66.0f;// 2. 计算反正切（atan2(对边, 邻边)），结果为弧度float radian = atan2f(numerator, 15.0f);  // 第二个参数固定为15（与Python一致）// 3. 将弧度转换为角度（乘以180/π），并转换为整数int servo_dx = (int)(radian * 180.0f / 3.1415926f);  // 用3.1415926提高精度return servo_dx;
}void pid_init(uint8_t ch, PID_Mode mode, float kp, float kpp, float ki, float kd, float kdd, float max_limit)
{if (ch >= PID_MAX_CHANNEL) return;PID_Controller* pid = &pid_controllers[ch];memset(pid, 0, sizeof(PID_Controller));pid->mode = mode;pid->kp = kp;pid->kpp = kpp;pid->ki = ki;pid->kd = kd;pid->kdd = kdd;pid->max_limit = max_limit;
}float pid_calculate(uint8_t ch, float error)
{if (ch >= PID_MAX_CHANNEL) return 0.0f;PID_Controller* pid = &pid_controllers[ch];// »ý·ÖÀÛ¼Ópid->integral += error;// »ý·ÖÏÞ·ù£¬·ÀÖ¹»ý·Ö±¥ºÍif (pid->integral > pid->max_limit) pid->integral = pid->max_limit;if (pid->integral < -pid->max_limit) pid->integral = -pid->max_limit;// Îó²îÎ¢·Öfloat derivative = error - pid->prev_error;// ¼ÆËãÊä³öfloat output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative ;// ¸üÐÂÉÏÒ»´ÎÎó²îpid->prev_error = error;// ÏÞ·ùÊä³öif (output > pid->max_limit) return pid->max_limit;if (output < -pid->max_limit) return -pid->max_limit;return output;
}#define PID_CH_X                0  // X方向PID通道
#define PID_CH_Y                1  // Y方向PID通道

函数解释
一、map_0_200_to_1000_300
将输入值限制在 0~200 范围内，再线性反向映射到 1000~300 范围（输入越小，输出越大）。
$output=700−input×550200\text{output} = 700 - \frac{\text{input} \times 550}{200}$
二、min_int
返回两个整数中的较小值

三、output_to_servo
将输入值output_x转换为伺服电机的角度偏移量（基于反正切计算）。
计算对边长度：
$numerator=output_x×1.866.0\text{numerator} = \frac{\text{output\_x} \times 1.8}{66.0}$
计算弧度（反正切）：
$radian=atan2(numerator,15.0)\text{radian} = \text{atan2}(\text{numerator}, 15.0)$
弧度转角度（整数）：
$servo_dx=⌊radian×180.03.1415926⌉(取整)\text{servo\_dx} = \left\lfloor \text{radian} \times \frac{180.0}{3.1415926} \right\rceil \quad (\text{取整})$
四、pid_init
初始化指定通道的 PID 控制器，设置控制模式、比例系数（kp、kpp）、积分系数（ki）、微分系数（kd、kdd）及输出最大值限制。

五、pid_calculate
计算指定通道的 PID 控制器输出，包含积分限幅和输出限幅功能。
积分项累加：
$integral=integral+error\text{integral} = \text{integral} + \text{error}$
积分限幅：
$integral={max_limitif integral>max_limit−max_limitif integral<−max_limitintegralotherwise\text{integral} = \begin{cases} \text{max\_limit} & \text{if } \text{integral} > \text{max\_limit} \\ -\text{max\_limit} & \text{if } \text{integral} < -\text{max\_limit} \\ \text{integral} & \text{otherwise} \end{cases}$
微分项计算：
$derivative=error−prev_error\text{derivative} = \text{error} - \text{prev\_error}$
PID输出：
$output=kp×error+ki×integral+kd×derivative\text{output} = kp \times \text{error} + ki \times \text{integral} + kd \times \text{derivative}$
输出限幅：
$output={max_limitif output>max_limit−max_limitif output<−max_limitoutputotherwise\text{output} = \begin{cases} \text{max\_limit} & \text{if } \text{output} > \text{max\_limit} \\ -\text{max\_limit} & \text{if } \text{output} < -\text{max\_limit} \\ \text{output} & \text{otherwise} \end{cases}$

这些函数里面的很多值都是需要根据实际设备调整的，比如*16是因为42电机对角度进行了16细分，需要乘于16才是正常的值，然后pid不用说肯定是要自己调的，output_to_servo中的值也需要根据实际情况进行调整

（3）控制部分

控制部分的函数只有一个就是d36a_set_angle_both，具体的内容在前两章都讲过这边就不赘述了，可以直接参考之前的博客
MSPM0开发学习笔记：D36A驱动的42步进电机二维云台（2025电赛附源代码及引脚配置）
值得注意的一点是这个控制是阻塞型的，就是电机需要运动完这个指令才会从openart那边接收到新的误差参数进行下一步调整，并且在接受信息的这一瞬间电机的速度直接就是0，而不是根据误差实时调整频率以及方向，所以效果并不是非常准并且电机会有较大的抖动，虽然大体效果是还可以的但是仍需要精进。后续的博客会发在电赛期间写的非阻塞控制代码，基础部分定位第二题一秒不到第二题2.6秒左右，相对来说还是一个不错的成绩的。

如果无法很好的复现博客里的代码，可以私信作者博取源代码