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

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

---

文章目錄

• 前言
• 一、硬件選擇
• 二、硬件連線
• 三、軟件代碼
• 四、總結

---

前言

今年的電賽（2025），很多題都與雲臺相關，因此為備戰電賽，博主這邊也是準備了一個由兩個42步進電機驅動的雲臺並提前進行調試，避免賽題出來之後手忙腳亂的，這邊的兩個42步進電機採用同一個驅動模塊進行驅動（D36A）,主控肯定採用MSPM0G3507。然後3D打印了一個二維雲臺的結構並進行組裝。本章博客主要是講這個雲臺進行繪圖的思路以及代碼。激光還沒有安裝上去，目前只是雲臺的循跡代碼

---

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

一、硬件選擇

主控：MSPM0G3507
驅動：D36A雙路步進電機驅動
電機：42步進電機*2

二、硬件連線

硬件連線部分在上一篇博客裡面已經說過了，可以直接去上一篇裡面看，這邊附上鍊接
MSPM0開發學習筆記：D36A驅動的42步進電機二維雲臺（2025電賽 附源代碼及引腳配置）

三、軟件代碼

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

參數	含義以及作用
current_x	目前的X座標
current_y	目前的Y座標
target_x	需要移動到的X座標
target_y	需要移動到的Y座標
move_x	需要移動的X距離
move_y	需要移動的Y距離
point_count	繪製圓形時候的點數

代碼實現如下：

函數一：限幅函數

#define MAX_ANGLE_X  1000.0f   
#define MIN_ANGLE_X -1000.0f   
#define MAX_ANGLE_Y  1000.0f  
#define MIN_ANGLE_Y -1000.0f  


float limit_angle(float angle, float min, float max) {
    if (angle < min) return min;
    if (angle > max) return max;
    return angle;
}

函數二：激光雲臺繪製正方形

// Function to draw a square trajectory
// Parameters:
//   x_len - length of the square's X-axis dimension
//   y_len - length of the square's Y-axis dimension
//   MOVE_SPEED - speed of movement between points
void draw_square(int x_len, int y_len, int MOVE_SPEED) {

    // Structure to store X and Y angle coordinates
    typedef struct {
        float x_angle;  // X-axis angle position
        float y_angle;  // Y-axis angle position
    } Point;
    
    // Invert Y-axis length (likely for coordinate system adjustment)
    y_len = y_len * -1;
    
    // Calculate half-lengths for easier coordinate calculation
    float x_len_2 = x_len / 2;
    float y_len_2 = y_len / 2;
    
    // Define square vertices relative to origin (0,0)
    // Coordinates form a square shape when connected sequentially
    Point square_points[] = {
        {-1 * x_len_2, y_len_2},    // Top-left corner
        {x_len_2, y_len_2},         // Top-right corner
        {x_len_2, -1 * y_len_2},    // Bottom-right corner
        {-1 * x_len_2, -1 * y_len_2},// Bottom-left corner
        {-1 * x_len_2, y_len_2},    // Back to top-left to close the square
        {0.0f, 0.0f}                // Final point: return to origin
    };
    
    // Calculate total number of points in the square trajectory
    uint8 point_count = sizeof(square_points) / sizeof(Point);  
    
    // Initialize current position at origin (0,0)
    float current_x = 0;
    float current_y = 0;
    
    // Move to each defined point in sequence
    for (uint8 i = 0; i < point_count; i++) {
        // Ensure target angles stay within allowed range
        float target_x = limit_angle(square_points[i].x_angle, MIN_ANGLE_X, MAX_ANGLE_X);
        float target_y = limit_angle(square_points[i].y_angle, MIN_ANGLE_Y, MAX_ANGLE_Y);

        // Calculate relative movement from current position to target
        float move_x = target_x - current_x;
        float move_y = target_y - current_y;
        
        // Update current position to target coordinates
        current_x = target_x;
        current_y = target_y;
        
        // Send movement command to both axes
        d36a_set_angle_both(move_x, move_y, MOVE_SPEED);
        
        // Pause 300ms after reaching each point
        system_delay_ms(300);
    }
    
    // Pause 2 seconds after completing the square
    system_delay_ms(2000);
}

一、函數定義：draw_square函數接收三個參數，分別是正方形的 X 軸長度（x_len）、Y 軸長度（y_len）和移動速度（MOVE_SPEED）。
二、數據結構：定義了Point結構體用於存儲座標點的 X 和 Y 角度值。
三、座標處理：
1、將 Y 軸長度取負值（為了調整座標系方向）
2、計算半長（x_len_2, y_len_2），用於確定正方形頂點座標
3、座標定義：定義了正方形的 4 個頂點座標和原點（0,0）座標以中心點為原點，通過半長計算得出四個頂點位置
4、最後回到一個點 (0,0) 用於回到起點
四、繪製邏輯：
1、遍歷所有定義的座標點
2、對每個目標點進行角度限制（通過limit_angle函數確保在有效範圍內）
3、計算當前位置到目標點的移動量
4、調用d36a_set_angle_both函數移動到目標點（同時設置 X 和 Y 方向角度）
5、每個點移動後延遲 300 毫秒，繪製完成後延遲 2000 毫秒

函數三：激光雲臺繪製圓形

// Function to draw a circle trajectory
// Parameters:
//   r - radius of the circle
//   MOVE_SPEED - speed of movement between points
//   point_count - number of points to use for drawing the circle (more = smoother)
void draw_circle(int r, int MOVE_SPEED, const uint8 point_count) {

    // Structure to store X and Y angle coordinates
    typedef struct {
        float x_angle;  // X-axis angle position
        float y_angle;  // Y-axis angle position
    } Point;

    // Array to store circle points (extra element to close the loop)
    Point circle_points[point_count + 1];  

    // Calculate coordinates for each point on the circle
    for (uint8 i = 0; i < point_count; i++) {
        // Convert angle from degrees to radians (full circle = 2π radians)
        float rad = 2 * 3.1415926f * i / point_count;

        // Calculate X and Y positions using trigonometric functions
        // cosine for X-axis, sine for Y-axis to form circular path
        circle_points[i].x_angle = r * cosf(rad); 
        circle_points[i].y_angle = r * sinf(rad);  
    }
    // Close the circle by duplicating the first point as the last point
    circle_points[point_count] = circle_points[0];

    // Initialize current position at origin (0,0)
    float current_x = 0.0f;  
    float current_y = 0.0f; 

    // Move to each calculated point in sequence
    for (uint8 i = 0; i <= point_count; i++) {
        // Ensure target angles stay within allowed range
        float target_x = limit_angle(circle_points[i].x_angle, MIN_ANGLE_X, MAX_ANGLE_X);
        float target_y = limit_angle(circle_points[i].y_angle, MIN_ANGLE_Y, MAX_ANGLE_Y);
        
        // Calculate relative movement from current position to target
        float move_x = target_x - current_x;
        float move_y = target_y - current_y;

        // Update current position to target
        current_x = target_x;
        current_y = target_y;
        
        // Send movement command to both axes
        d36a_set_angle_both(move_x, move_y, MOVE_SPEED);

        // Optional delay between point movements
        // system_delay_ms(10);
    }
    
    // Return to origin (0,0) after completing the circle
    float target_x = 0;
    float target_y = 0;

    // Calculate movement from last circle point to origin
    float move_x = target_x - current_x;
    float move_y = target_y - current_y;

    // Update current position to origin
    current_x = target_x;
    current_y = target_y;

    // Send final movement command to return to origin
    d36a_set_angle_both(move_x, move_y, MOVE_SPEED);

    // Pause for 2 seconds after completing the circle
    system_delay_ms(2000);  
}

通過三角函數（X 軸用餘弦、Y 軸用正弦）計算圓周上指定數量的點的座標，這些點基於圓周的等角度增量分佈。函數會按順序在這些點之間移動（帶速度控制），同時確保角度在有效範圍內；通過回到第一個點來閉合圓形軌跡，最後返回原點，結束時短暫暫停。點的數量越多，繪製的圓越平滑。
以下是函數中涉及的公式：

1. 弧度計算（將圓周等分為指定數量的點）

   rad

   2 × π × i point_count \text{rad} = 2 \times \pi \times \frac{i}{\text{point\_count}} rad=2×π×point_counti​
   其中， i i i 為當前點的索引（0 到 point_count-1）， point_count \text{point\_count} point_count 為圓周上的總點數， rad \text{rad} rad 為對應角度的弧度值。

2. X軸座標計算

   x _ a n g l e

   r × cos ⁡ ( rad ) x\_angle = r \times \cos(\text{rad}) x_angle=r×cos(rad)
   其中， r r r 為圓的半徑， cos ⁡ ( rad ) \cos(\text{rad}) cos(rad) 為弧度對應的餘弦值。

3. Y軸座標計算

   y _ a n g l e

   r × sin ⁡ ( rad ) y\_angle = r \times \sin(\text{rad}) y_angle=r×sin(rad)
   其中， sin ⁡ ( rad ) \sin(\text{rad}) sin(rad) 為弧度對應的正弦值。

四、總結

這邊給的都是一些簡單圖形的繪製代碼，但是思路都是通用的，複雜圖形也可以複用這一套邏輯。大家參考參考就好

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