來源：https://blog.csdn.net/2403_87969572/article/details/149900066

抓取時間（ISO本地）：2026-05-18 05:17:03

---

文章目錄

• 前言
• 一、硬件選擇
• 二、原理介紹（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 = 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
將輸入值限制在 0200 範圍內，再線性反向映射到 1000300 範圍（輸入越小，輸出越大）。

output

700 − input × 550 200 \text{output} = 700 - \frac{\text{input} \times 550}{200} output=700−200input×550​
二、min_int
返回兩個整數中的較小值

三、output_to_servo
將輸入值output_x轉換為伺服電機的角度偏移量（基於反正切計算）。
計算對邊長度：

numerator

output_x × 1.8 66.0 \text{numerator} = \frac{\text{output\_x} \times 1.8}{66.0} numerator=66.0output_x×1.8​
計算弧度（反正切）：

radian

atan2 ( numerator , 15.0 ) \text{radian} = \text{atan2}(\text{numerator}, 15.0) radian=atan2(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{取整}) servo_dx=⌊radian×3.1415926180.0​⌉(取整)
四、pid_init
初始化指定通道的 PID 控制器，設置控制模式、比例係數（kp、kpp）、積分系數（ki）、微分系數（kd、kdd）及輸出最大值限制。

五、pid_calculate
計算指定通道的 PID 控制器輸出，包含積分限幅和輸出限幅功能。
積分項累加：

integral

integral + error \text{integral} = \text{integral} + \text{error} integral=integral+error
積分限幅：

integral

{ 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} integral=⎩ ⎨ ⎧​max_limit−max_limitintegral​if integral>max_limitif integral<−max_limitotherwise​
微分項計算：

derivative

error − prev_error \text{derivative} = \text{error} - \text{prev\_error} derivative=error−prev_error
PID輸出：

output

k p × error + k i × integral + k d × derivative \text{output} = kp \times \text{error} + ki \times \text{integral} + kd \times \text{derivative} output=kp×error+ki×integral+kd×derivative
輸出限幅：

output

{ 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} output=⎩ ⎨ ⎧​max_limit−max_limitoutput​if output>max_limitif output<−max_limitotherwise​

這些函數里面的很多值都是需要根據實際設備調整的，比如*16是因為42電機對角度進行了16細分，需要乘於16才是正常的值，然後pid不用說肯定是要自己調的，output_to_servo中的值也需要根據實際情況進行調整

（3） 控制部分

控制部分的函數只有一個就是d36a_set_angle_both，具體的內容在前兩章都講過這邊就不贅述了，可以直接參考之前的博客
MSPM0開發學習筆記：D36A驅動的42步進電機二維雲臺（2025電賽 附源代碼及引腳配置）
值得注意的一點是這個控制是阻塞型的，就是電機需要運動完這個指令才會從openart那邊接收到新的誤差參數進行下一步調整，並且在接受信息的這一瞬間電機的速度直接就是0，而不是根據誤差實時調整頻率以及方向，所以效果並不是非常準並且電機會有較大的抖動，雖然大體效果是還可以的但是仍需要精進。後續的博客會發在電賽期間寫的非阻塞控制代碼，基礎部分定位第二題一秒不到第二題2.6秒左右，相對來說還是一個不錯的成績的。

如果無法很好的復現博客裡的代碼，可以私信作者博取源代碼