這段日子喜事連連，暫時把寫博客的事情放下了，有時候想想好久沒有寫博客了，要不要寫點啥呢。轉念一想，好像也沒有啥值得寫的心得體會，加上最近忙着結婚的事情，也就把寫博客的事擱置了。週五本來是要上班的，但是公司大廈供電系統維護，所以調休一天。藉着安靜的週五，加上最近一個多月研究GRBL源代碼的心得，寫下這篇博客，供後來者參考學習。網上關於GRBL源代碼分析的資料幾乎找不到，這篇博客裏的內容大多是自己對源代碼的理解，也有一部分是QQ羣裏的同行的心得。博客裏沒有寫到，或者寫的不對的地方，歡迎大家留言共同探討，共同進步。
GRBL的核心是帶有梯形加減速過程的DDA直線插補算法的實現，整個GRBL源代碼中包含了以下內容：
（1）串口中斷接收上位機的指令，包括自定義的系統命令和G代碼指令；
（2）串口指令解析，自定義的系統命令直接執行，G代碼指令調用相關操作，這裏只關注直線段、圓弧指令的解析；
（3）圓弧拆分成直線段進行插補的方法；
（4）多條直線段之間轉角速度優化的前瞻速度控制的方法；
（5）單條線段梯形加減速過程換算成定時器定時不同時間長短來輸出脈衝的方法；
（6）限位條件的判斷及軸自動歸位的方法；
（7）其它spindle、coolant接口等，這些我也不知道幹嘛用的，推測是給用戶二次開發預留的接口。
下面詳細介紹這些模塊的實現細節，串口接收、指令解析這些很容易理解，就簡單說說；spindle、coolant這些還沒有琢磨透，好像用到的情況也不多，不做描述；重點還是關注圓弧拆分線段、轉角速度優化、線段梯形加減速插補和自動歸位的實現方法。
一、串口接收
串口配置成中斷接收和中斷髮送模式，並創建了串口接收環形隊列和串口發送環形隊列，中斷接收的數據存放在串口接收環形隊列裏，串口需要發送的數據放到串口發送環形隊列裏。當串口產生中斷時，如果接收中斷標誌位置位，說明接收到數據，把數據讀出放到接收隊列，如果發送中斷標誌置位，說明發送寄存器空，把發送隊列裏的數據寫入發送寄存器裏。這就是串口處理的流程，具體的實現難度不大，就不詳細解釋源碼了 。
二、串口接收到的數據解析
串口接收到的數據放在串口接收環形隊列裏，主程序裏每次從接收隊列裏讀出一個字節，以'\r'或'\n'爲標誌截取一行完整的指令，如果指令以'$'開始，說明是自定義的系統指令，其它的是G代碼指令。如果是系統自定義指令，就調用uint8_t system_execute_line(char *line)函數，裏面包含了一些讀取軟件版本信息、讀取默認配置參數信息和把外部設置的參數信息寫入eeprom裏，軸歸位操作等功能。如果是G代碼指令，就調用uint8_t gc_execute_line(char *line)函數，裏面包含了很多G代碼的指令解析過程，由於重點放在研究直線插補算法上，沒有對G代碼解析源碼深入分析，這裏只關注裏面的執行圓弧和直線插補的代碼，即只需要關心void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate)和void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate,uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear)兩個函數即可。
三、圓弧拆分成多線段
GRBL中把圓弧拆分成多條逼近的直線段，然後對直線段進行插補，這種方法也就是俗稱的把複雜曲線拆分成多條逼近的直線的插補方法。圓弧拆分成直線段的方法，在void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate,uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear)函數裏實現，下面對該函數進行詳細介紹：
position，圓弧起始點位置座標，爲了後面解釋方便，這裏設爲(x0,y0,z0)
target，圓弧終點座標，這裏設爲(x1,y1,z1)
offset，圓心相對於起始點的偏移向量，這裏設爲(rx,ry,yz)，那麼圓心座標爲(x0+rx,y0+ry,z0+rz)
radius，圓弧半徑長度
feed_rate，軸的進給速率
invert_feed_rate，進給速率含義標誌位，這裏默認爲零，表示進給速率的單位是min/mm，即分鐘/毫米
axis_0，圓弧所在平面的第一個軸，可以是x/y/z中任意一個
axis_1，圓弧所在平面的第二個軸，可以是x/y/z中任意一個
axis_linear，除了圓弧平面之外的第三個軸，即與圓弧平面垂直的軸
void mc_arc(float *position, float *target, float *offset, float radius, float feed_rate,
uint8_t invert_feed_rate, uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear)
{
//圓弧所在平面的圓心座標
float center_axis0 = position[axis_0] + offset[axis_0];
float center_axis1 = position[axis_1] + offset[axis_1];
//圓心指向圓弧起始點的向量座標
float r_axis0 = -offset[axis_0];float r_axis1 = -offset[axis_1];
//圓心指向圓弧終點的向量座標
float rt_axis0 = target[axis_0] - center_axis0;float rt_axis1 = target[axis_1] - center_axis1;
//計算圓心到圓弧起始點向量b(r_axis0 ,r_axis1)和圓心到圓弧終點向量c(rt_axis0 ,rt_axis1)的夾角的正切值，注意夾角a的方向是起始點向量b逆時針轉向終點向量c的角，這個在後面判斷角度值符號時會用到。根據向量夾角餘弦公式，推出向量夾角正切公式，套入向量b和向量c的座標，即可得出tana。下面公式中的atan2是反正切函數，a=atan2(y,x)，即角度a=y/x的正切值
float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
if (gc_state.modal.motion == MOTION_MODE_CW_ARC) {
//如果圓弧順時針移動，角度應該是負值，如果計算出的角度爲正值，需要在計算出的角度基礎上減去2*pi（pi爲圓周率）
if (angular_travel >= 0) { angular_travel -= 2*M_PI; }} else {
//如果圓弧逆時針移動，角度應該是正值，如果計算出的角度爲負值，需要在計算出的角度基礎上加上2*pi（pi爲圓周率）
if (angular_travel <= 0) { angular_travel += 2*M_PI; }}
//計算起點到終點的圓弧可以劃分多少條小線段，計算方法：總共的弧長/每條小線段的長度，angular_travel是圓弧的弧度，radius是圓弧的半徑，那麼它們的乘積angular_travel*radius就是圓弧的弧長，再乘以0.5就是弧長的一半。settings.arc_tolerance是圓弧上兩點之間連接的小線段到這段圓弧的最大距離，即圓弧上的小線段到弧頂的最大距離，這裏設爲h，
uint16_t segments = floor(fabs(0.5*angular_travel*radius)/
sqrt(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) );
}
......
//這裏是計算圓心與每條小線段所夾的角T，即上圖中角AOB的餘弦值和正弦值，由於角度很小，這裏採用了三角函數的泰勒級數展開公式計算cosT和sinT。cosT的二階泰勒級數爲1-T*T/2，sinT的三階泰勒級數爲T-T*T*T/6，爲了計算方便，cos_T 被放大了兩倍，後面又乘上了0.5復原了。有cosT和sinT的泰勒級數公式，cosT被放大了兩倍，可以推出sinT=T*(4+cosT)/6，即下面計算sinT的公式的來源。
float cos_T = 2.0 - theta_per_segment*theta_per_segment;
float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0);
cos_T *= 0.5;
//循環累加每一條小線段，圓的極座標公式爲x=r*cosa,y=r*sina，假設當前線段的起始座標爲(rcosa,rsina),下一條線段比當前線段移動的角度已知爲T，那麼下一條線段的起始座標爲(rcos(a+T),rsin(a+T)),運算得到rcos(a+T)=r*cosa*cosT-r*sina*sinT，rsin(a+T)=r*sina*cosT+r*cosa*sinT。由於我們知道當前線段的座標爲(r_axis0,r_axis1)，又知道sinT和cosT的值，下一條線段的起始座標根據公式可立即求出。
for (i = 1; i<segments; i++) {
if (count < N_ARC_CORRECTION) {
//這就是用上述公式求下一條線段的起始座標的具體計算。當累計計算線段數超過N_ARC_CORRECTION個數時，要調用N_ARC_CORRECTION個線段總共轉動的角度來計算最終的座標移動值，這樣可以消除因每次使用sinT、cosT的近似泰勒值運算的累積誤差
r_axisi = r_axis0*sin_T + r_axis1*cos_T;
r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
r_axis1 = r_axisi;
count++;
} else {
//調用N_ARC_CORRECTION個線段總共轉動的角度來計算最終的座標移動值
cos_Ti = cos(i*theta_per_segment);
sin_Ti = sin(i*theta_per_segment);
r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
count = 0;
}
// 計算出下一條線段的起始座標，也就是當前線段的終點座標，前面已知當前線段的起始座標，這樣就可以把線段的座標傳遞給直線插補函數mc_line進行線段插補了
position[axis_0] = center_axis0 + r_axis0;
position[axis_1] = center_axis1 + r_axis1;
position[axis_linear] += linear_per_segment;
//線段插補
mc_line(position, feed_rate, invert_feed_rate);
......
}
}
//把圓弧終點作爲最後一條線段的終點座標進行直線插補，確保圓弧上所有的點包含進了直線裏
mc_line(target, feed_rate, invert_feed_rate);
}
四、多線段速度規劃前瞻算法
連續執行多條線段插補的時候，爲了加快軸的移動速度，執行完一條直線指令後不能停下來，然後重新啓動執行下一條直線指令。而是需要保持一定的速度去執行下一條直線插補，但是由於相鄰兩條直線之間有一定的夾角，導致轉彎的時候，軸的速度不能過快，還要考慮兩條直線執行的最大速度限制和直線頭尾速度銜接等問題，這些問題的處理方法就是前瞻算法。
GRBL中使用了環形隊列的方式存儲每一條直線段的信息，這個隊列的名稱是block_buffer[BLOCK_BUFFER_SIZE]，這個結構體數組裏存放的是線段的初速度、最大初速度限制、最大轉角速度限制、正常運行速度、加速度、線段長度信息。當G代碼解析出一條線段指令或者圓弧拆分出線段後，調用void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate)函數，把當前線段的信息存入block_buffer隊列中，然後把當前線段和隊列裏前一條線段結合在一起，用前瞻算法修正隊列裏前一條線段的最大運行速度，以便保證在前一條線段執行結束時的速度與當前線段的初速度一致。另外，根據兩條線段的夾角確定最大轉角速度，用於修正前一條線段的結束速度和當前線段的初速度不能超過最大轉角速度。下面對mc_line函數進行詳細分析：
target，線段移動到的最終位置，單位是mm，也就是說當前線段移動的長度是target值減去之前的所有線段的移動長度；
feed_rate，線段的最大運行速度，梯形加減速值是提前設定好保存在eeprom裏的；
invert_feed_rate，線段運行速度含義標誌位，feed_rate有多種含義，這裏我們只瞭解feed_rate的單位是min/mm即可；
void mc_line(float *target, float feed_rate, uint8_t invert_feed_rate)
{
//如果限位使能，就檢查target值是否超出了軸能到達的最遠位置，如果超出了就告警限位錯誤，並復位系統
if (bit_istrue(settings.flags,BITFLAG_SOFT_LIMIT_ENABLE)) { limits_soft_check(target); }
......
do {
//這個函數的功能很多，有很多地方都會調用它，這裏調用的目的是判斷有沒有系統異常發生，比如系統告警或復位，如果有異常，這個函數裏處理異常的代碼就會執行，沒有就退出函數，繼續運行
protocol_execute_runtime(); // Check for any run-time commandsif (sys.abort) { return; } // Bail, if system abort.
//檢查block_buffer是否滿，如果滿就執行嘗試打開線段插補執行使能開關。如果系統配置裏開啓了auto-cycle功能，就可以自動開始執行線段插補，block_buffer裏的線段會被系統執行插補操作而空出一些空間，這樣隊列就不滿了，也就退出下面的do-while循環繼續執行代碼
if ( plan_check_full_buffer() ) { protocol_auto_cycle_start(); } // Auto-cycle start when buffer is full.else { break; }
} while (1);
//把當前線段的的信息添加到block_buffer隊列裏，這個函數裏包含了前瞻算法的處理過程，在下面會詳細介紹
plan_buffer_line(target, feed_rate, invert_feed_rate);
......
}
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
{
......
for (idx=0; idx<N_AXIS; idx++) {
//target是軸移動的距離，單位是毫米，系統設定了steps_per_mm值，也就是每毫米代表的軸移動步數，直接換算得到軸移動步數target_steps
target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
//target表示軸從原點移動到終點的總距離，所以當前線段的移動步數需要用target減去之前所有線段移動的總步數
block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
//獲得三個軸裏移動距離最遠的軸移動的距離，後面DDA直線插補時會用到這個值。關於DDA插補算法的方法後面會介紹
block->step_event_count = max(block->step_event_count, block->steps[idx]);//根據步數換算出真實移動的距離，保存在unit_vec中，後面計算兩條線段夾角時會用到
delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];
unit_vec[idx] = delta_mm;
// 這個值小於零，說明這個軸需要向與原來方向相反的方向移動
if (delta_mm < 0 ) { block->direction_bits |= get_direction_pin_mask(idx); }
//三個軸是正交的，知道了每個軸移動的距離，那麼線段在空間裏移動的真實距離是s*s=x*x+y*y+z*z，這個值在後面也會用到
block->millimeters += delta_mm*delta_mm;
}
//開平方求出線段空間裏移動的距離
block->millimeters = sqrt(block->millimeters);
......
for (idx=0; idx<N_AXIS; idx++) {
if (unit_vec[idx] != 0) {
//這裏是爲計算兩條線段的夾角做準備工作，爲了便於理解，這裏假設當前線段的座標是向量c=(x2,y2,z2)，上一條線段的座標向量d=(x1,y1,z1) ,它們的空間向量長度分別是s2和s1，那麼有座標正交可得，s2*s2=x2*x2+y2*y2+z2*z2，s1*s1=x1*x1+y1*y1+z1*z1，inverse_millimeters表示1/s，uint_vec[0/1/2]分別表示x/y/z，經過運算後uint_vec[0/1/2]表示的是x/s,y/s或者z/s，那麼inverse_unit_vec_value 表示s/x，s/y或者s/z的絕對值
unit_vec[idx] *= inverse_millimeters;inverse_unit_vec_value = fabs(1.0/unit_vec[idx]);
//配置裏的max_rate是表示線段向量每個分座標的最大速度限制，feed_rate是線段向量合成後的速度，所以feed_rate要與每個軸分向量最大速度換算成合成後的最大速度比較，然後取最小值作爲最終的線段運行最大速度，最大加速度也是用同樣的方法進行比較
feed_rate = min(feed_rate,settings.max_rate[idx]*inverse_unit_vec_value);block->acceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value);
//計算兩條線段的夾角餘弦值，夾角餘弦公式cosa=(x1*x2+y1*y2+z1*z2)/(s1*s2)，因爲兩條線段是首尾相連，那麼用兩條線段的向量座標計算出來的夾角其實是它的補角，夾角和它補角的餘弦值剛好取負值即可，所以下面計算夾角餘弦的方法裏多了一個負號
junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx];
}
}
//這個用半角公式sin(a/2)=sqrt((1-cosa)/2)直接運算
float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta));
// 計算轉角最大速度v，有圓弧加速度公式可知，v*v=a*r,其中a是圓弧向心加速度，這裏近似值爲block->acceleration，r是圓弧的半徑。settings.junction_deviation是兩條線段內切圓弧到兩條線段交點的距離，這裏設爲h，如下圖所示，角EAD即爲上面的a/2，內切圓的半徑爲r，那麼AD的長度即爲r+h，那麼sin_theta_d2=r/(r+h)，已知h的值，那麼r=h*sin_theta_d2/(1-sin_theta_d2)，那麼套入v*v=a*r即可求得v*v，即block->max_junction_speed_sqr的值。
(block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) );
......
//對沒有優化過的線段進行優化
planner_recalculate();
}
static void planner_recalculate()
{
......
//如果所有的線段都已經優化過了，直接退出函數
if (block_index == block_buffer_planned) { return; }
......
//當前線段的起始速度取最大起始限制速度與末速度爲零反推的最大起始速度的最小值
current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters);
......
//這段代碼的含義是從當前線段往前推，直到所有的線段都優化過退出循環，即每條線段的初速度不能超過線段設置的最大初速度的限制
while (block_index != block_buffer_planned) { ......}
......
//這段代碼的含義是從第一個沒有優化過的線段往前，直到到達當前線段時退出循環，即每條線段的末速度不能超過下一條線段的初速度，這樣多條線段才能保持連續的速度運行
while (block_index != block_buffer_head) {}
}
到這裏多線段速度前瞻規劃已經完成了，下面分析GRBL中怎麼把帶有加減速的線段轉化成用定時器輸出脈衝的過程。
五、線段轉化成不同頻率的輸出脈衝
第四節裏block_buffer隊列裏存放的就是每條線段的詳細信息，根據線段的初速度、末速度、加速度和線段距離信息，計算出這條線段運行時軸需要移動的總步數。假定把這條線段總的運行時間截取成多個微小的時間段DT，即可求出每個DT時間段內的平均速度，同時可以求出DT時間段內軸移動的步數n，這樣就可以求出DT內每步需要的時間dt=DT/n，把dt設定爲定時器定時間隔，直到中斷計數次數到達n結束。由於線段總步數選取的是線段向量座標(x,y,z)裏的最大值，所以定時器中斷裏不能每次都輸出脈衝，而是需要用DDA插補算法運算出每次中斷哪個軸需要輸出脈衝。
這裏介紹一下GRBL中用到的DDA算法的實現過程，假設線段向量座標a(x,y,z)，選取x,y,z絕對值最大的作爲累加溢出值c=|max(x,y,z)|，假定累加初值b=c/2，那麼三個軸輸出脈衝的DDA算法如下：
m=l=k=b;
for(i=0;i<c;i++)
{
m+=x;
l+=y;
k+=z;
if(m>=c)
{
x軸輸出一個脈衝；
m-=c;
}
if(l>=c)
{
y軸輸出一個脈衝；
l-=c;
}
if(k>=c)
{
z軸輸出一個脈衝；
k-=c;
}
}
下面開始進行源碼分析，第四節裏對每個線段進行預處理之後，進入主循環裏protocol_auto_cycle_start()函數和protocol_execute_runtime()函數，我們從protocol_auto_cycle_start()函數開始分析。
//當GRBL默認配置裏使能了auto_start功能，就把系統執行標誌sys.execute設置爲EXEC_CYCLE_START，也就是系統可以自動執行線段輸出脈衝，如果沒有使能auto_start，在系統運行過程中可以用串口命令手動開啓線段輸出脈衝
void protocol_auto_cycle_start() { if (sys.auto_start) { bit_true_atomic(sys.execute, EXEC_CYCLE_START); } }
void protocol_execute_runtime()
{
//省略了一些系統告警標誌位的處理
......
//當系統沒有使能線段輸出脈衝功能時，只調用st_prep_buffer
if (rt_exec & EXEC_FEED_HOLD)
{
.....
st_prep_buffer();
......
}
if (rt_exec & EXEC_CYCLE_START)
{
......
//修改系統狀態爲STATE_CYCLE，這樣啓動定時器後下次循環就不會在進入這裏重複啓動定時器了
sys.state = STATE_CYCLE;
//把線段換算成定時器輸出脈衝頻率和脈衝個數
st_prep_buffer();
//啓動定時器開始輸出脈衝
......
}
......
//當定時器已經啓動後，以後的循環就是不斷的把線段換算成定時器輸出脈衝
if (sys.state & (STATE_CYCLE | STATE_HOLD | STATE_HOMING)) { st_prep_buffer(); }
}
void st_prep_buffer()
{
//線段拆分成多個DT時間片，每個時間片軸運行的總步數和每步需要的時間存放在segment_buffer隊裏中，while循環判斷這個隊列是否滿，如果滿了就退出循環，沒滿繼續把線段拆分的時間片存入隊列
while (segment_buffer_tail != segment_next_head) {
//判斷當前線段拆分時間片是否完成，如果沒有完成，pl_block不爲空，if裏的語句不會被執行。如果pl_block爲空，說明當前線段時間片拆分完成，執行if裏的語句，開始把下一條線段的信息讀出來進行時間片拆分
if (pl_block == NULL) {
//從block_buffer隊列中獲取一條新的線段
pl_block = plan_get_current_block();
......
//開闢新的隊列st_block_buffer存放新線段拆分時間片計算過程數據
st_prep_block = &st_block_buffer[prep.st_block_index];
//記錄線段每個軸的運行方向
st_prep_block->direction_bits = pl_block->direction_bits;
//AMASS功能用於平滑脈衝頻率太慢的線段，如果AMASS功能使能，線段步數放大MAX_AMASS_LEVEL倍，但是定時器定時間隔將會縮短，相當於定時器中斷加快了，更多次的中斷累積才輸出一個脈衝，這樣輸出脈衝變得更平滑了
st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS];
st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS];
st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS];
st_prep_block->step_event_count = pl_block->step_event_count;
#else
st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS] << MAX_AMASS_LEVEL;
st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS] << MAX_AMASS_LEVEL;
st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS] << MAX_AMASS_LEVEL;
st_prep_block->step_event_count = pl_block->step_event_count << MAX_AMASS_LEVEL;
#endif
//inv_2_accel 是加速度a的過程值，inv_2_accel=1/2a
float inv_2_accel = 0.5/pl_block->acceleration;
if (sys.state == STATE_HOLD) {
//STATE_HOLD就是線段沒有開始輸出脈衝，這樣線段的末速度爲零，整個線段都是減速過程，後面分析了線段輸出脈衝的過程，再回頭看這裏就很簡單了
......
}
else
{
......
//intersect_distance是線段加速和減速過程速度最大值交點離線段末尾的距離s2，如下圖所示：
如圖中所示，假設這條線段的長度s剛好只有加速和減速過程，最大速度爲vm，已知線段的初速度爲v0，線段的加速度爲a，線段的末速度爲vt，根據公式可求出當前假設條件下交點vm到線段末尾的距離s2，即intersect_distance
float intersect_distance =
0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr));
if (intersect_distance > 0.0) {
if (intersect_distance < pl_block->millimeters){
//計算線段最大限制速度減速到末速度需要的減速距離
prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr);
if (prep.decelerate_after < intersect_distance) {
......
//如果初速度等於最大限制速度，那麼線段只有勻速和減速過程
if (pl_block->entry_speed_sqr == pl_block->nominal_speed_sqr) {
}
else
{
//線段有加速、勻速和減速過程
......
}
else{
//減速距離大於交點距離，說明線段設置的最大限制速度大於交點處的最大速度，那麼線段只有三角形狀的加速和減速過程，沒有勻速過程
......
}
}
else{
//交點到末尾的距離大於線段總長度，說明只有線段只有減速過程
......
}
}
else{
//交點到末尾的距離小於零，說明加減速沒有交點，即線段只有加速過程
......
}
}
}
......
//設定了時間片爲DT_SEGMENT，上面我們已經知道了線段的加減速過程，通過下面的do-while循環，計算出每個時間片線段的平均速度和移動的步數，由於它們的值是根據線段的加減速計算得來的，所以裏面隱含了脈衝發送頻率的信息。注意：每個segment_buffer隊列裏的數據的時間粒度一般都是DT_SEGMENT，也就是說假設一個時間片內，加速移動的距離是s1，但是這時候加速完成了，而加速過程所用的時間dt小於DT_SEGMENT，那麼時間片剩餘的時間DT_SEGMENT-dt，必須用來計算線段在剩餘時間裏勻速或減速移動的距離s2，那麼在DT_SEGMENT時間片內移動的總距離是s1+s2。另外，要注意由於脈衝頻率太低，導致在時間片DT_SEGMENT內移動的距離可能爲零，爲了防止這種情況發生，倍數放大DT_SEGMENT的值，直到至少移動的距離大於一步爲止。
do {
......
}while (mm_remaining > prep.mm_complete);
}
......
//計算出時間片內移動的步數
prep_segment->n_step = last_n_steps_remaining-n_steps_remaining;
......
//計算出每步需要的時間
float inv_rate = dt/(last_n_steps_remaining - steps_remaining);
......
//每步需要的時間設置爲定時器的定時時間間隔
uint32_t cycles = ceil( (TICKS_PER_MICROSECOND*1000000*60)*inv_rate );
......
//如果使能AMASS，定時器週期縮短，移動步數放大
cycles >>= prep_segment->amass_level;
prep_segment->n_step <<= prep_segment->amass_level;
......
}
調用void st_wake_up() 使能定時器1後，就會進入定時器1中斷處理函數，在函數裏使用DDA插補算法，輸出脈衝
ISR(TIMER1_COMPA_vect)
{
......
//使能定時器0，用於恢復脈衝輸出管腳的電平。例如定時器1裏面拉高了管腳，表示輸出了一個脈衝，這時候需要定時器0把管腳復位爲低，便於定時器1輸出下一個脈衝
TCNT0 = st.step_pulse_time;
TCCR0B = (1<<CS01);
......
//st.exec_segment 爲空，表示從segment_buffer隊列裏取出的段已經輸出脈衝完成，調用if裏的語句獲取segment_buffer隊列裏的下一個執行時間片，主要是獲取該時間片裏每個軸移動的步數、插補步數，然後重新設置定時器1的定時週期
if (st.exec_segment == NULL) {......}
......
//對x步數進行DDA累加溢出操作
#ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
st.counter_x += st.steps[X_AXIS];
#else
st.counter_x += st.exec_block->steps[X_AXIS];
#endif
//如果溢出，管腳輸出一個高電平，表示輸出一個脈衝，沒有溢出，管腳不輸出電平
st.step_outbits |= (1<<X_STEP_BIT);
st.counter_x -= st.exec_block->step_event_count;
if (st.exec_block->direction_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
else { sys.position[X_AXIS]++; }
}
......
}
到這裏第五節終於講完了，最難的部分也已經過去，下面說說歸爲操作的過程。
六、軸歸位操作
軸歸位操作就是軸以一定的速度進行最大距離的線段插補操作，插補過程中遇到限位信號的時候強行終止插補操作，反覆多次進行這種插補後，軸就停在了限位點附近了。
void mc_homing_cycle()
{
......
//歸位操作
limits_go_home(HOMING_CYCLE_0);
......
}
void limits_go_home(uint8_t cycle_mask)
{
......
//循環n_cycle次進行直線插補，直線距離設置爲軸允許的最大移動距離，插補過程中遇到限位信號強行復位系統來終止插補操作，並記錄每次軸移動到限位信號的實際距離，通過改變每次插補的速度，計算出最合理的實際距離
do {
......
} while (n_cycle-- > 0);
......
//最後用最合理的插補距離和插補速度把軸移動到歸位的位置
plan_buffer_line(target, settings.homing_seek_rate, false);
......
}
經過幾天的奮戰，終於寫完了這篇博客，長舒一口氣。