前言

今年的電賽（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 coordinatestypedef struct {float x_angle;  // X-axis angle positionfloat 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 calculationfloat 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 sequentiallyPoint 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 trajectoryuint8 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 sequencefor (uint8 i = 0; i < point_count; i++) {// Ensure target angles stay within allowed rangefloat 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 targetfloat move_x = target_x - current_x;float move_y = target_y - current_y;// Update current position to target coordinatescurrent_x = target_x;current_y = target_y;// Send movement command to both axesd36a_set_angle_both(move_x, move_y, MOVE_SPEED);// Pause 300ms after reaching each pointsystem_delay_ms(300);}// Pause 2 seconds after completing the squaresystem_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 coordinatestypedef struct {float x_angle;  // X-axis angle positionfloat 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 circlefor (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 pathcircle_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 pointcircle_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 sequencefor (uint8 i = 0; i <= point_count; i++) {// Ensure target angles stay within allowed rangefloat 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 targetfloat move_x = target_x - current_x;float move_y = target_y - current_y;// Update current position to targetcurrent_x = target_x;current_y = target_y;// Send movement command to both axesd36a_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 circlefloat target_x = 0;float target_y = 0;// Calculate movement from last circle point to originfloat move_x = target_x - current_x;float move_y = target_y - current_y;// Update current position to origincurrent_x = target_x;current_y = target_y;// Send final movement command to return to origind36a_set_angle_both(move_x, move_y, MOVE_SPEED);// Pause for 2 seconds after completing the circlesystem_delay_ms(2000);  
}

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

1. 弧度計算（將圓周等分為指定數量的點）
   $$\text{rad}=2\times \pi \times \frac{i}{\text{point\_count}}$$
   其中，$i$ 為當前點的索引（0 到 point_count-1），$\text{point\_count}$ 為圓周上的總點數，$\text{rad}$ 為對應角度的弧度值。
2. X軸坐標計算
   $$x\_angle=r\times \cos (\text{rad})$$
   其中，$r$ 為圓的半徑，$\cos (\text{rad})$ 為弧度對應的余弦值。
3. Y軸坐標計算
   $$y\_angle=r\times \sin (\text{rad})$$
   其中，$\sin (\text{rad})$ 為弧度對應的正弦值。

四、總結

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

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