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

本文链接：https://blog.csdn.net/2403_87969572/article/details/149900066

文章目录

前言
一、硬件选择
二、原理介绍（UART）
三、硬件连线
三、软件代码

前言

这篇博客的代码是博主在备赛电赛的时候写来练手的，结果今年电赛的题目真的差不多，是一个自动瞄准追踪装置，因此在比赛结束之后也是用这些代码写一下这篇博客。方案是两个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 = 120

while 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_x
        dy = blob_y - center_y
        
        # 按要求处理偏差值（加500，确保传输为正数）
        send_dx = dx + 500
        send_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, ImageTk

class ThresholdEditor:
    def __init__(self, root):
        self.root = root
        self.root.title("OpenCV阈值编辑器")
        self.root.geometry("1200x800")
        self.root.minsize(1000, 700)
        
        # 初始化变量
        self.image = None
        self.video_capture = None
        self.is_camera_active = False
        self.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] = var
                
                tk.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] = var
                
                tk.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-255
            return 179 if channel == "H" else 255
        return 255
    
    def change_mode(self, mode):
        self.color_mode = mode
        self.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 = None
            self.is_camera_active = False
            self.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 = None
                return
                
            self.is_camera_active = True
            self.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("错误", "无法加载选中的图片")
                return
            self.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 = frame
                self.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 = 400
            display_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 img

if __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 没有数据会返回 FALSE
    fifo_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 的中断优先级为 0
	
	  uart_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方向PID
    pid_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;  // 前三位数字减128
              int dy = res.second_num - 128; // 后三位数字减128
							float 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到300
    int32_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 / 66
    float 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 范围（输入越小，输出越大）。
$\text{output} = 700 - \frac{\text{input} \times 550}{200}$
二、min_int
返回两个整数中的较小值

三、output_to_servo
将输入值output_x转换为伺服电机的角度偏移量（基于反正切计算）。
计算对边长度：
$output_x × 1.8 66.0 \text{numerator} = \frac{\text{output\_x} \times 1.8}{66.0}$
计算弧度（反正切）：
$\text{radian} = \text{atan2}(\text{numerator}, 15.0)$
弧度转角度（整数）：
$servo_dx = ⌊ radian × 180.0 3.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 控制器输出，包含积分限幅和输出限幅功能。
积分项累加：
$\text{integral} = \text{integral} + \text{error}$
积分限幅：
$max_limit if integral > max_limit − max_limit if integral < − max_limit integral otherwise \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}$
微分项计算：
$prev_error \text{derivative} = \text{error} - \text{prev\_error}$
PID输出：
$\text{output} = kp \times \text{error} + ki \times \text{integral} + kd \times \text{derivative}$
输出限幅：
$max_limit if output > max_limit − max_limit if output < − max_limit output otherwise \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秒左右，相对来说还是一个不错的成绩的。

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