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

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

---

文章目錄

• 前言
• 一、硬件選擇
• 二、硬件連線
• 三、軟件代碼
• • 1、思路
  • 2、代碼實現

---

前言

在前幾篇的博客中，有提到一個追蹤紅色小球的代碼，但是會發現追蹤的過程中存在很大的抖動並且響應速度相對較慢，推測其原因是因為步進電機的控制函數是阻塞型的，就是說步進電機一定要以某一個給定的速度走完當前規定的角度步數，才會根據新接受到的視覺誤差數據再次進行調整運動，而在接受的這幾毫秒，步進電機的頻率是0，也就是說會直接停下來，之後又根據新的誤差數據馬上運動起來，所以會有很大的抖動，非常影響識別，也導致pid很難調，沒有辦法達到一個很好的效果。

本章博客的目的就是介紹一個新的運動思路以及代碼，通過實時根據接受到的視覺數據進行二維雲臺的步進電機的頻率更新，從而避免大幅的抖動以及保證電機數據更新的時效性，更好的瞄準目標。

---

如果無法很好的復現博客裡的代碼，可以私信作者博取源代碼，電賽期間都在線

一、硬件選擇

主控：MSPM0G3507
驅動：D36A雙路步進電機驅動
電機：42步進電機*2
視覺：樹莓派連接USB攝像頭

二、硬件連線

硬件連線部分在前幾篇博客裡面已經說過了，可以直接去裡面看，這邊附上鍊接
機器視覺：樹莓派結合攝像頭解決2025電賽E題鎖定靶心 思路與代碼分享

三、軟件代碼

1、思路

對於非阻塞的控制，一開始的思路是通過GPIO引腳輸出PWM波，通過接受UART信息的中斷實時更新PWM的佔空比從而控制給D36A的ST引腳上升沿脈衝的數量和速度（D36A是42步進電機的閉環集成驅動模塊，給ST引腳一個脈衝步進電機就會轉動一步，具體可以去看看前幾章的博客那邊有具體講到）。但是後面發現可能不能用這個高的頻率（樹莓派給的視覺誤差信息幀率可以達到一百幀）去更新PWM引腳的佔空比，一運行這個程序TI芯片就會卡死。

因此後面我們換了一種思路，不用GPIO引腳來輸出PWM波來進行控制，而是通過我們自己設置引腳的高低電平進而給ST引腳脈衝，我們只需要設置每一次循環的延時就可以控制給上升沿脈衝的數量與速度。這樣就不需要頻繁的改變PWM引腳的佔空比導致芯片卡死。

2、代碼實現

軟件部分採用C語言實現，IDE採用keil，基於逐飛庫進行編寫
這邊先進
行一下簡單的參數說明，便於理解後面的思路

參數	含義以及作用
UART_INDEX	串口索引，定義使用的串口（此處為UART_2），用於指定數據接收和發送的硬件串口
UART_BAUDRATE	串口波特率，定義串口通信的波特率（與調試口一致），確保與外部設備通信速率匹配
UART_TX_PIN	串口發送引腳，定義UART_2的發送引腳（B15），用於串口數據的發送
UART_RX_PIN	串口接收引腳，定義UART_2的接收引腳（B16），用於串口數據的接收
UART_PRIORITY	串口中斷優先級，指定串口接收中斷的優先級（使用UART0的中斷編號映射到UART2），控制中斷響應順序
PIT_CON_PRIO	中頻PIT中斷優先級，定義用於PID計算的定時器（TIMG12）中斷優先級，確保PID計算的實時性
PIT_IMPLUSE_PRIO	高頻PIT中斷優先級，定義用於生成步進電機脈衝的定時器（TIMA0）中斷優先級，確保脈衝生成的高精度
LASER_PIN	激光控制引腳，定義控制激光的GPIO引腳（A30），用於在接近目標時開啟/關閉激光
uart_get_data	串口接收數據緩衝區，存儲串口接收的原始數據（最大5049字節），避免數據丟失
fifo_get_data	FIFO輸出緩衝區，從FIFO中讀取數據並用於解析，臨時存儲待解析的串口數據
get_data	單次接收數據字節，存儲串口中斷中單次接收的一個字節數據
fifo_data_count	FIFO數據個數，記錄FIFO緩衝區中當前存儲的數據字節數，用於判斷是否有足夠數據可解析
uart_data_fifo	FIFO結構體，用於串口數據的緩衝管理，解決串口數據突發接收時的處理壓力
PID_CH_X	X軸PID通道，定義X方向對應的PID控制器通道（0），用於區分X軸的PID計算
PID_CH_Y	Y軸PID通道，定義Y方向對應的PID控制器通道（1），用於區分Y軸的PID計算
FOC_CH_X_PIN	X軸步進電機脈衝引腳，定義X軸步進電機的脈衝控制引腳（B11），用於生成X軸電機的驅動脈衝
FOC_CH_Y_PIN	Y軸步進電機脈衝引腳，定義Y軸步進電機的脈衝控制引腳（B12），用於生成Y軸電機的驅動脈衝
ParseResult.valid	解析結果有效性標誌，標記串口數據解析是否成功（true為有效），用於判斷是否使用解析後的數據
ParseResult.first_num	解析結果前三位數字，存儲串口數據中X方向相關的前三位數字，用於計算X軸誤差
ParseResult.second_num	解析結果後三位數字，存儲串口數據中Y方向相關的後三位數字，用於計算Y軸誤差
gpio_status	GPIO狀態變量，預留用於存儲GPIO引腳的電平狀態，暫未在代碼中實際使用
pit_state	PIT中斷觸發標誌，標記中頻PIT定時器（10ms）是否觸發中斷（1為觸發），用於主循環同步中斷狀態
target_x	X軸目標位置，由串口數據解析並轉換得到的X軸目標位置，作為X軸PID控制器的目標值
target_y	Y軸目標位置，由串口數據解析並轉換得到的Y軸目標位置，作為Y軸PID控制器的目標值
freq_X	X軸PID輸出頻率，X軸PID控制器計算得到的輸出頻率（正負表示方向），用於控制X軸電機轉速
freq_Y	Y軸PID輸出頻率，Y軸PID控制器計算得到的輸出頻率（正負表示方向），用於控制Y軸電機轉速
ticks	全局計數器，每10ms遞增一次，用於計時和控制週期性任務（如幀率統計、搜索間隔）
fps	當前幀率，記錄每秒接收的有效數據幀數，反映系統數據接收的實時性
fps_ticks	幀計數器，累計1秒內接收的有效幀數，用於計算fps
data_proceed	數據處理標誌，標記是否有新的解析數據需要更新PID目標（true為需要），用於同步主循環與中斷的數據流
update_fps	幀率更新標誌，標記是否需要更新並打印幀率（true為需要），控制幀率的週期性打印
implus_X_count	X軸脈衝計數器，每100us遞增一次，用於累計X軸脈衝間隔，觸發脈衝生成
implus_Y_count	Y軸脈衝計數器，每100us遞增一次，用於累計Y軸脈衝間隔，觸發脈衝生成
skip_pits_X	X軸脈衝間隔，X軸兩次脈衝之間的100us計數間隔（由頻率計算得到），決定X軸電機轉速
skip_pits_Y	Y軸脈衝間隔，Y軸兩次脈衝之間的100us計數間隔（由頻率計算得到），決定Y軸電機轉速
Impluse_mode	手動脈衝模式標誌，標記是否處於手動脈衝模式（true為手動），手動模式下暫停自動脈衝生成
no_found	目標未找到標誌，標記是否丟失目標（true為丟失），用於觸發目標搜索邏輯
lost_frames	丟失幀數計數器，累計連續丟失的無效數據幀數，用於判斷是否進入目標搜索模式
no_lost_frames	連續找到幀數計數器，累計連續接收的有效數據幀數，用於判斷是否從搜索模式恢復
impluse_count	搜索脈衝計數，累計目標丟失時發送的搜索脈衝總數，用於限制搜索範圍（避免無限搜索）
step	搜索步長，目標丟失時每次發送的脈衝數（18），控制搜索時的移動距離
skip_ticks	搜索間隔計數器，記錄上次發送搜索脈衝的ticks值，用於控制搜索動作的間隔（每100ms一次）

核心函數

/**
 * 高頻PIT中斷處理函數（100us觸發一次）
 * 功能：生成步進電機脈衝（根據set_freq設置的間隔）
 * @param state：中斷狀態（未使用）
 * @param ptr：用戶參數（未使用）
 */
void pit_impluse_handler(uint32 state, void *ptr){
    if(Impluse_mode == true) return;  // 手動模式下不生成自動脈衝
    
    // 脈衝計數器遞增（每100us+1）
    implus_X_count += 1;
    implus_Y_count += 1;
    
    // X軸：計數器達到間隔時，生成一個脈衝
    if(implus_X_count >= skip_pits_X){
        implus_X_count = 0;  // 重置計數器
        // 生成10us高電平脈衝
        gpio_set_level(FOC_CH_X_PIN,GPIO_HIGH);
        system_delay_us(10);
        gpio_set_level(FOC_CH_X_PIN,GPIO_LOW);
    }
    
    // Y軸：同上
    if(implus_Y_count >= skip_pits_Y){
        implus_Y_count = 0;
        gpio_set_level(FOC_CH_Y_PIN,GPIO_HIGH);
        system_delay_us(10);
        gpio_set_level(FOC_CH_Y_PIN,GPIO_LOW);
    }
}

pit_impluse_handler是一個高頻中斷處理函數，每 100 微秒觸發一次，核心功能是為步進電機生成精確的控制脈衝。
1、函數首先判斷是否處於手動脈衝模式（Impluse_mode）若是則暫停自動脈衝生成；否則分別遞增 X 軸和 Y 軸的脈衝計數器（每 100 微秒 + 1）。
2、當計數器值達到預設的脈衝間隔（skip_pits_X或skip_pits_Y，由set_freq_X/Y根據目標頻率計算得出）時，會生成一個 10 微秒的高電平脈衝（通過控制對應 GPIO 引腳電平）隨後重置計數器。這一機制直接決定了電機的轉速 —— 間隔越小，單位時間內生成的脈衝越多，電機轉動越快，從而實現根據目標頻率實時調節電機速度的效果。

/**
 * 中頻PIT中斷處理函數（10ms觸發一次）
 * 功能：執行PID計算、更新電機頻率、幀率統計
 * @param state：中斷狀態（未使用）
 * @param ptr：用戶參數（未使用）
 */
void pit_handler (uint32 state, void *ptr)
{
    ticks += 1;  // 全局計數器遞增（每10ms+1）
    
    if(Impluse_mode == true) return;  // 手動模式下不執行自動控制
    
    // 若有新數據，執行PID計算並更新頻率
    if(data_proceed){
        // 計算X/Y軸PID輸出（頻率）
        freq_X = pid_calculate_cam(0, target_x);
        freq_Y = pid_calculate_cam(1, target_y);
        
        // 設置X/Y軸頻率（包含方向）
        set_freq_X(freq_X);
        set_freq_Y(freq_Y);
        
        data_proceed = false;  // 重置數據處理標誌
    }
    
    pit_state = 1;  // 標記PIT中斷已觸發
    
    // 每100箇中斷（10ms×100=1s）更新一次幀率
    if(ticks % 100 == 0){
        tick2 += 1;  // 輔助計數遞增
        update_fps = true;  // 標記需更新幀率
        fps = fps_ticks;    // 存儲當前幀率
        fps_ticks = 0;      // 重置幀計數器
    }
}

pit_handler是一箇中頻中斷處理函數，每 10 毫秒觸發一次，主要負責閉環控制邏輯與狀態更新。
1、函數首先遞增全局計數器（ticks），若處於手動模式則暫停自動控制；
2、當檢測到新數據標誌（data_proceed）時，會調用 PID 控制器計算 X 軸和 Y 軸的目標頻率（freq_X/Y），並通過set_freq_X/Y更新脈衝間隔，完成後重置數據標誌。
3、此外，函數會標記中斷觸發狀態（pit_state），並每 100 次中斷（即 1 秒）更新一次幀率統計（fps），通過記錄有效幀數量反映系統數據接收的實時性。這一函數是連接數據解析與電機控制的核心，確保根據最新誤差動態調整電機控制參數。

計時器中斷初始化

pit_ms_init(PIT_TIM_G12, 10, pit_handler, NULL);                            
pit_us_init(PIT_TIM_A0, 100, pit_impluse_handler, NULL);    

總體代碼這邊因為是電賽控制題基礎部分第三題的代碼，所以額外還多了一些找不到目標時候的手動收縮函數，但是都是一個邏輯上的區別而已，在找到目標之後還是根據以上講到的這兩個函數進行pid的控制逼近中心收斂。

大概思路如下：

完整代碼如下：

//---------------------------------------------------------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------Include區域（頭文件包含）------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------------------------------------

// 通用頭文件（包含基礎類型、宏定義等）
#include "zf_common_headfile.h"
// PID控制器頭文件（提供PID計算相關函數）
#include "mc_pid_controller.h"
// 延時驅動頭文件（提供微秒/毫秒級延時函數）
#include "zf_driver_delay.h"
// GPIO驅動頭文件（提供GPIO初始化、電平控制等函數）
#include "zf_device_d36a.h"  // 步進電機驅動相關頭文件
#include <math.h>            // 數學函數庫（用於atan2等計算）
#include <stdint.h>          // 標準整數類型定義
#include <stdio.h>           // 標準輸入輸出（用於printf）


//---------------------------------------------------------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------Define區域（宏定義）------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------------------------------------

// 串口配置：使用UART2
#define UART_INDEX              (UART_2   )                   
// 串口波特率（與調試口一致）
#define UART_BAUDRATE           (DEBUG_UART_BAUDRATE)        
// UART2發送引腳（B15）
#define UART_TX_PIN             (UART2_TX_B15  )              
// UART2接收引腳（B16）
#define UART_RX_PIN             (UART2_RX_B16  )              
// 串口中斷優先級（使用UART0的中斷編號，實際映射到UART2）
#define UART_PRIORITY           (UART0_INT_IRQn)              

// PIT控制器中斷優先級（用於PID計算的定時器）
#define PIT_CON_PRIO           (TIMG12_INT_IRQn)   
// 脈衝生成PIT中斷優先級（用於生成步進電機脈衝的定時器）
#define PIT_IMPLUSE_PRIO       (TIMA0_INT_IRQn)  

// 激光控制引腳（A30）
#define LASER_PIN                A30                                

// 串口接收數據緩衝區（最大5049字節，用於暫存接收的原始數據）
uint8 uart_get_data[5049];                                        
// FIFO輸出緩衝區（用於從FIFO中讀取數據並解析）
uint8 fifo_get_data[5049];                                        

// 單次接收的數據字節
uint8 get_data = 0;                                             
// FIFO中存儲的數據個數
uint32 fifo_data_count = 0;                                     

// FIFO結構體（用於串口數據的緩衝，解決數據突發接收問題）
fifo_struct uart_data_fifo;

// PID通道定義：X軸和Y軸分別使用0和1通道
#define PID_CH_X                0  
#define PID_CH_Y                1  

// 步進電機脈衝引腳：X軸（B11）、Y軸（B12）
#define FOC_CH_X_PIN                B11  
#define FOC_CH_Y_PIN                B12  


// 解析結果結構體（存儲串口數據解析後的有效信息）
typedef struct {          
    bool valid;           // 數據是否有效（解析成功為true）
    int first_num;        // 前三位數字（X方向相關）
    int second_num;       // 後三位數字（Y方向相關）
} ParseResult;

// GPIO狀態變量（預留，用於存儲GPIO電平狀態）
uint8 gpio_status;


//---------------------------------------------------------------------------------------------------------------------------------------------------
// -----------------------------------------------------------------函數定義區域（工具函數與回調函數）------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------------------------------------

/**
 * 串口接收中斷處理函數
 * 功能：在串口收到數據時觸發，將數據寫入FIFO緩衝區，避免中斷中處理耗時邏輯
 * @param state：中斷狀態（未使用）
 * @param ptr：用戶參數（未使用）
 */
void uart_rx_interrupt_handler (uint32 state, void *ptr)
{ 
    // 讀取一個字節（查詢式，無數據時不阻塞）
    uart_query_byte(UART_INDEX, &get_data);                                     
    // 將接收的字節寫入FIFO，實現數據緩衝
    fifo_write_buffer(&uart_data_fifo, &get_data, 1);                           
}


/**
 * 線性映射函數：將0-200的輸入映射到300-1000的輸出（反向映射）
 * @param input：輸入值（0-200）
 * @return 映射後的輸出值（300-1000）
 */
int32_t map_0_200_to_1000_300(int32_t input) {
    // 限制輸入範圍在0-200
    if (input < 0) {
        input = 0;
    } else if (input > 200) {
        input = 200;
    }
    
    // 反向映射公式：input越小，output越大（1000→300）
    int32_t output = 700 - (input * 550) / 200;
    
    return output;
}


/**
 * 取兩個整數的較小值
 * @param a：第一個整數
 * @param b：第二個整數
 * @return 較小的整數
 */
int min_int(int a, int b) {                                               
    if (a < b) {
        return a;
    } else {
        return b;
    }
}


/**
 * 解析串口數據中的X-Y座標信息
 * 格式要求：數據需包含"XxxxxxxE"，其中xxxxxx為6位數字（前3位為X相關，後3位為Y相關）
 * @param data：待解析的字符串
 * @return 解析結果（包含有效性、前三位數字、後三位數字）
 */
ParseResult parse_xy_data(const char *data) {
    ParseResult result = {false, 0, 0};  // 初始化結果為無效
    const char *start, *end;
    char num_str[7];  // 存儲6位數字（+1位結束符）
    
    // 查找"X"的位置（數據起始標誌）
    start = strchr(data, 'X');
    if (!start) {
        return result;  // 未找到"X"，數據無效
    }
    start++;  // 跳過"X"，指向數字部分
    
    // 查找"E"的位置（數據結束標誌）
    end = strchr(start, 'E');
    if (!end) {
        return result;  // 未找到"E"，數據無效
    }
    
    // 檢查"X"和"E"之間是否正好6位數字
    if (end - start != 6) {
        return result;  // 位數不正確，數據無效
    }
    
    // 提取6位數字並添加結束符
    strncpy(num_str, start, 6);
    num_str[6] = '\0';  
    
    // 檢查是否全為數字
    for (int i = 0; i < 6; i++) {
        if (num_str[i] < '0' || num_str[i] > '9') {
            return result;  // 含非數字字符，數據無效
        }
    }
    
    // 拆分前3位和後3位數字
    char first_str[4] = {0};  // 前3位（+結束符）
    char second_str[4] = {0}; // 後3位（+結束符）
    
    strncpy(first_str, num_str, 3);  // 提取前3位
    strncpy(second_str, num_str + 3, 3);  // 提取後3位
    
    // 轉換為整數並標記數據有效
    result.first_num = atoi(first_str);
    result.second_num = atoi(second_str);
    result.valid = true;
    
    return result;
}


/**
 * 將誤差值轉換為舵機角度（通過幾何計算）
 * @param output_x：X方向誤差值
 * @return 對應的舵機角度（度）
 */
float output_to_servo(float output_x)
{
    // 計算對邊長度：誤差值×1.8/66（幾何比例參數）
    float numerator = output_x * 1.8f / 66.0f;
    // 計算反正切（鄰邊固定為15，單位：弧度）
    float radian = atan2f(numerator, 15.0f);  
    // 弧度轉角度（×180/π）
    int servo_dx = (int)(radian * 180.0f / 3.1415926f);  
    
    return servo_dx;
}


/**
 * 將誤差距離映射到脈衝延遲時間（用於調整步進電機速度）
 * @param distance：誤差距離（0-1000）
 * @return 延遲時間（300-3000μs，距離越大延遲越小）
 */
uint16 distance_to_delay(int32 distance) {
    // 取距離絕對值（誤差非負）
    uint32 abs_dist = (distance < 0) ? -distance : distance;
    // 限制最大距離為1000（避免延遲過小）
    uint32 limited_dist = (abs_dist > 1000) ? 1000 : abs_dist;
    // 反向映射：距離越大，延遲越小（3000→300）
    return 300 - (limited_dist * 200) / 300;  
}


//---------------------------------------------------------------------------------------------------------------------------------------------------
// -----------------------------------------------------------------全局變量定義（狀態與參數存儲）------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------------------------------------

uint8_t pit_state = 0;               // PIT中斷觸發標誌（1表示觸發）
float target_x = 0, target_y = 0;    // X/Y軸目標位置（由串口數據解析得到）
int16_t freq_X, freq_Y;              // X/Y軸PID計算輸出的頻率
int ticks = 0;                       // 全局計數器（10ms遞增一次）
int fps = 0, fps_ticks = 0;          // 幀率相關：fps為當前幀率，fps_ticks為幀計數
bool data_proceed = false;           // 數據處理標誌（1表示需更新PID目標）
bool update_fps = false;             // 幀率更新標誌（1表示需打印幀率）


// 脈衝計數與間隔變量（用於高頻脈衝生成）
int implus_X_count = 0, implus_Y_count = 0;  // X/Y軸脈衝計數器（100us遞增）
float skip_pits_X = 999999.0f, skip_pits_Y = 999999.0f;  // X/Y軸脈衝間隔（100us為單位）

/**
 * 設置X軸步進電機頻率
 * @param freq：頻率值（正負表示方向，絕對值為頻率）
 */
void set_freq_X(float freq){
    // 設置方向引腳：頻率>0時正轉，否則反轉
    gpio_set_level(D36A_IN4_PIN, freq > 0 ? 1 : 0);  
    // 取頻率絕對值（僅關心大小）
    freq = fabsf(freq);
    // 頻率為0時，間隔設為極大值（不生成脈衝）
    if(freq == 0) skip_pits_X = 999999;
    // 計算脈衝間隔：10000（100us×100）/頻率 = 間隔（單位：100us）
    else skip_pits_X = 10000.0f/freq;
}

/**
 * 設置Y軸步進電機頻率
 * @param freq：頻率值（正負表示方向，絕對值為頻率）
 */
void set_freq_Y(float freq){
    // 設置方向引腳：頻率<0時正轉，否則反轉（與X軸邏輯相反）
    gpio_set_level(D36A_IN2_PIN, freq < 0 ? 1 : 0);  
    // 取頻率絕對值
    freq = fabsf(freq);
    // 頻率為0時不生成脈衝
    if(freq == 0) skip_pits_Y = 999999;
    // 計算脈衝間隔（同X軸）
    else skip_pits_Y = 10000.0f/freq;
}

bool Impluse_mode = false;  // 手動脈衝模式標誌（1表示手動模式，暫停自動控制）

/**
 * 手動發送脈衝序列（用於復位、搜索等特定動作）
 * @param ch：通道（0=X軸，1=Y軸）
 * @param impluse：脈衝數（正負表示方向）
 * @param delay_us：基礎延遲時間（μs）
 */
void SendImpluse(uint8_t ch,int16 impluse,int16_t delay_us){
    Impluse_mode = true;  // 進入手動模式，暫停自動脈衝生成
    impluse *= 16.5;      // 脈衝數縮放（根據機械結構調整）
    
    if (ch == 0){  // X軸處理
        // 設置方向：脈衝數>0時正轉
        gpio_set_level(D36A_IN4_PIN, impluse > 0 ? 1 : 0);  
        impluse = abs(impluse);  // 取絕對值（僅關心脈衝數量）
        // 計算減速點：前70%脈衝正常速度，後30%減速
        uint16_t slow_now = impluse * 0.7;
        uint16_t delay = delay_us;  // 基礎延遲
        
        // 循環生成脈衝
        for(uint16_t i = 0;i < impluse;i ++){
            // 超過減速點後，延遲增至3倍（降低速度，避免過沖）
            if(i > slow_now) delay = delay_us * 3;
            // 生成10us高電平脈衝
            gpio_set_level(FOC_CH_X_PIN,GPIO_HIGH);
            system_delay_us(10);
            gpio_set_level(FOC_CH_X_PIN,GPIO_LOW);
            // 脈衝間延遲
            system_delay_us(delay);
        }
    }
    else{  // Y軸處理（邏輯同X軸，引腳不同）
        // 設置方向：脈衝數<0時正轉
        gpio_set_level(D36A_IN2_PIN, impluse < 0 ? 1 : 0);
        impluse = abs(impluse);
        uint16_t slow_now = impluse * 0.7;
        uint16_t delay = delay_us;
        
        for(uint16_t i = 0;i < impluse;i ++){
            if(i > slow_now) delay = delay_us * 3;
            gpio_set_level(FOC_CH_Y_PIN,GPIO_HIGH);
            system_delay_us(10);
            gpio_set_level(FOC_CH_Y_PIN,GPIO_LOW);
            system_delay_us(delay);
        }
    }
    Impluse_mode = false;  // 退出手動模式，恢復自動控制
}

/**
 * 復位Y軸偏移（特定機械結構的復位動作）
 */
void ResetOffset(){
    SendImpluse(1,-100,100);  // Y軸反向發送100脈衝（基礎延遲100us）
    system_delay_ms(100);     // 等待動作完成
    SendImpluse(1,50,100);   // Y軸正向發送50脈衝
    system_delay_ms(50);      // 等待動作完成
}

/**
 * X軸搜索動作（用於丟失目標時的範圍搜索）
 */
void SearchAtX(){
    ResetOffset();               // 先復位Y軸
    SendImpluse(0,100,1000);    // X軸正向100脈衝（延遲1000us，慢速）
    SendImpluse(0,-100,100);    // X軸反向100脈衝（延遲100us，快速）
    SendImpluse(0,-100,1000);   // X軸反向100脈衝（延遲1000us，慢速）
}


/**
 * 高頻PIT中斷處理函數（100us觸發一次）
 * 功能：生成步進電機脈衝（根據set_freq設置的間隔）
 * @param state：中斷狀態（未使用）
 * @param ptr：用戶參數（未使用）
 */
void pit_impluse_handler(uint32 state, void *ptr){
    if(Impluse_mode == true) return;  // 手動模式下不生成自動脈衝
    
    // 脈衝計數器遞增（每100us+1）
    implus_X_count += 1;
    implus_Y_count += 1;
    
    // X軸：計數器達到間隔時，生成一個脈衝
    if(implus_X_count >= skip_pits_X){
        implus_X_count = 0;  // 重置計數器
        // 生成10us高電平脈衝
        gpio_set_level(FOC_CH_X_PIN,GPIO_HIGH);
        system_delay_us(10);
        gpio_set_level(FOC_CH_X_PIN,GPIO_LOW);
    }
    
    // Y軸：同上
    if(implus_Y_count >= skip_pits_Y){
        implus_Y_count = 0;
        gpio_set_level(FOC_CH_Y_PIN,GPIO_HIGH);
        system_delay_us(10);
        gpio_set_level(FOC_CH_Y_PIN,GPIO_LOW);
    }
}

int tick2 = 0;  // 輔助計數器（用於調試）

/**
 * 中頻PIT中斷處理函數（10ms觸發一次）
 * 功能：執行PID計算、更新電機頻率、幀率統計
 * @param state：中斷狀態（未使用）
 * @param ptr：用戶參數（未使用）
 */
void pit_handler (uint32 state, void *ptr)
{
    ticks += 1;  // 全局計數器遞增（每10ms+1）
    
    if(Impluse_mode == true) return;  // 手動模式下不執行自動控制
    
    // 若有新數據，執行PID計算並更新頻率
    if(data_proceed){
        // 計算X/Y軸PID輸出（頻率）
        freq_X = pid_calculate_cam(0, target_x);
        freq_Y = pid_calculate_cam(1, target_y);
        
        // 設置X/Y軸頻率（包含方向）
        set_freq_X(freq_X);
        set_freq_Y(freq_Y);
        
        data_proceed = false;  // 重置數據處理標誌
    }
    
    pit_state = 1;  // 標記PIT中斷已觸發
    
    // 每100箇中斷（10ms×100=1s）更新一次幀率
    if(ticks % 100 == 0){
        tick2 += 1;  // 輔助計數遞增
        update_fps = true;  // 標記需更新幀率
        fps = fps_ticks;    // 存儲當前幀率
        fps_ticks = 0;      // 重置幀計數器
    }
}


//---------------------------------------------------------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------主函數（程序入口）------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------------------------------------

bool no_found = 0;          // 目標未找到標誌
int lost_frames = 0;        // 丟失幀數計數器
int no_lost_frames = 0;     // 連續找到幀數計數器
int impluse_count = 0;      // 搜索脈衝計數
int16_t step = 0;           // 搜索步長
int skip_ticks = 0;         // 搜索間隔計數器

/**
 * 主函數：初始化系統並進入主循環
 */
int main(void) {
    // 初始化系統時鐘（80MHz）
    clock_init(SYSTEM_CLOCK_80M);  
    // 初始化調試口（用於printf輸出）
    debug_init();
    // 初始化步進電機驅動（配置相關引腳）
    d36a_init();                   
    printf("begin\n");  // 打印啟動信息

    // 初始化FIFO（8位數據，緩衝區為uart_get_data，大小64字節）
    fifo_init(&uart_data_fifo, FIFO_DATA_8BIT, uart_get_data, 64);
    // 初始化串口（波特率、引腳）
    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_set_callback(UART_INDEX, uart_rx_interrupt_handler, NULL);

    printf("pid\n");  // 打印PID初始化信息
    // 初始化X軸PID（位置式，Kp=5, Ki=0, Kd=0，輸出限幅±1500）
    pid_init(0, PID_POSITIONAL, 5, 0, 0, 1, 0, 1500);
    // 初始化Y軸PID（參數同X軸）
    pid_init(1, PID_POSITIONAL, 5, 0, 0, 1, 0, 1500);
    
    // 初始化激光引腳（推輓輸出，初始高電平）
    gpio_init(LASER_PIN, GPO, 1, GPO_PUSH_PULL);
    // 初始化中頻PIT（10ms中斷，綁定pit_handler）
    pit_ms_init(PIT_TIM_G12, 10, pit_handler, NULL);                            
    // 初始化高頻PIT（100us中斷，綁定pit_impluse_handler）
    pit_us_init(PIT_TIM_A0, 100, pit_impluse_handler, NULL);                            
    // 設置PIT中斷優先級
    interrupt_set_priority(PIT_CON_PRIO, 0);
    interrupt_set_priority(PIT_IMPLUSE_PRIO, 1);

    printf("int\n");  // 打印中斷初始化信息
    printf("init\n"); // 打印初始化完成信息
    ResetOffset();    // 執行Y軸復位

    // 主循環（程序核心邏輯）
    while (1) {
        // 1. 處理串口數據（從FIFO中讀取並解析）
        fifo_data_count = fifo_used(&uart_data_fifo);  // 獲取FIFO中數據量

        // 若FIFO中數據超過6字節（滿足最小解析長度）
        if (fifo_data_count > 6) {
            memset(fifo_get_data,0,sizeof(fifo_get_data));  // 清空解析緩衝區
            // 從FIFO讀取數據並清空已讀部分
            fifo_read_buffer(&uart_data_fifo, fifo_get_data, &fifo_data_count, FIFO_READ_AND_CLEAN);
            fifo_get_data[fifo_data_count] = '\0';  // 添加字符串結束符

            // 解析數據（提取X-Y相關數字）
            ParseResult parsed = parse_xy_data((const char*)fifo_get_data);
            
            // 若解析有效
            if (parsed.valid) {
                // 計算X/Y方向誤差（目標值為500，偏離量=實際值-500）
                int32 dx = parsed.first_num - 500 ;   
                int32 dy = parsed.second_num - 500;  
                
                // 過濾無效誤差（499為特殊無效值）
                if  (dx!=499 && dy!=499){
                    fps_ticks += 1;  // 有效幀計數+1

                    // 計算目標位置（通過幾何轉換）
                    float dd_x = output_to_servo(dx);
                    float dd_y = output_to_servo(dy);
                    target_x = dd_x * 16.5 - 67;  // X軸目標位置（機械補償）
                    target_y = dd_y * 16.5 - 5;   // Y軸目標位置（機械補償）
                    
                    data_proceed = true;  // 標記需更新PID目標

                    // 動態調整X軸PID參數（小誤差時降低增益，避免震盪）
                    if(fabs(target_x)<15) pid_set_params(0, 0.8, 0, 1, 0);
                    else pid_set_params(0, 4, 0, 3, 0);
                    
                    // 動態調整Y軸PID參數（同X軸邏輯）
                    if(fabs(target_y)<15) pid_set_params(1, 0.8, 0, 1, 0);
                    else pid_set_params(1, 4, 0, 3, 0);
                    
                    // 當誤差較小時（接近目標），開啟激光
                    if(fabs(target_y)<15 && fabs(target_x)<15){
                        gpio_set_level(LASER_PIN,GPIO_HIGH);
                    }else{
                        gpio_set_level(LASER_PIN,GPIO_LOW);
                    }
                    
                    no_lost_frames ++;  // 連續找到幀數+1
                    // 打印目標位置（調試用）
                    printf("x:%f y:%f \n",target_x,target_y);
                }
                else{
                    // 無效誤差時，停止電機
                    target_x = 0;
                    target_y = 0;
                    set_freq_X(0);
                    set_freq_Y(0);
                    lost_frames ++;  // 丟失幀數+1
                }
            }
        }
        
        // 2. 打印幀率（每1s一次）
        if(update_fps){
            printf("fps:%d\n",fps);  // 打印當前幀率
            pit_state = 0;
            update_fps = 0;  // 重置幀率更新標誌
        }
        
        // 3. 處理PIT中斷標誌（僅標記用，無實際操作）
        if(pit_state){
            pit_state = 0;
        }
        
        // 4. 目標丟失處理（連續丟失20幀以上）
        printf("n:%d\n",lost_frames);  // 打印丟失幀數（調試用）
        if(lost_frames > 20){
            // 首次進入丟失狀態時初始化搜索參數
            if(no_found == false){ 
                no_lost_frames = 0;
                skip_ticks = ticks;  // 記錄當前計數
                step = 18;  // 初始搜索步長
            }
            no_found = true;  // 標記目標丟失
        }
        
        // 5. 重新找到目標時，重置丟失狀態
        if(no_lost_frames > 0){
            if(no_found == true){
                lost_frames = 0;  // 重置丟失幀數
            }
            no_found = false;  // 標記目標已找到
        }
        
        // 6. 目標丟失時執行搜索動作（每100ms一次）
        if(ticks - skip_ticks > 10 && no_found){
            skip_ticks = ticks;  // 更新搜索間隔計數
            
            // 限制搜索範圍（超過±100時反向）
            step = (impluse_count > 100) ? -18 : (impluse_count < -100) ? 18 : step;
            // 發送搜索脈衝（X軸，步長18，延遲180us）
            SendImpluse(0,step,180);
            impluse_count += step;  // 更新搜索脈衝計數
        }
        else
            system_delay_ms(5);  // 主循環小延時（降低CPU佔用）
    }
}