理論實現(xiàn)
由于算法在計算過程中涉及到一些浮點型運算，大量的浮點型運算會使得效率大大降低為了使在計算浮點型的速度得到更好的優(yōu)化。
控制步進(jìn)電機(jī)需要四個描述速度曲線的參數(shù)；速度曲線從零速度開始，加速到給定速度并持續(xù)到減速開始，并且最后減速至零給定步數(shù)的速度。
1.不同速度段的處理方法
通過第14篇文章思路大概已經(jīng)清晰，接下來就是軟件代碼的設(shè)計，其實使用定時器產(chǎn)生脈沖，并按梯形規(guī)律加速即可，使用定時器中斷產(chǎn)生步脈沖并且只有在步進(jìn)電機(jī)移動時進(jìn)入。根據(jù)我們前面的長篇大論概述，需要在這個中斷中處理四個不同速度的狀態(tài)，分別是 stop——accel——run——decel——stop
每一個速度變化的階段的劃分是前面計算得出的對應(yīng)脈沖個數(shù)，當(dāng)脈沖個數(shù)到達(dá)限制則啟動切換即可，這種操作可以通過我們熟悉的狀態(tài)機(jī)在定時器中斷中實現(xiàn)。STOP 為停止?fàn)顟B(tài)，ACCEL 為加速狀態(tài)，RUN 為勻速狀態(tài)(對應(yīng)最大速度 speed)，DECEL為減速狀態(tài)。
當(dāng)應(yīng)用程序啟動或者步進(jìn)電機(jī)停止?fàn)顟B(tài)機(jī)處于 STOP 狀態(tài)。當(dāng)輸入移動步數(shù)建立計算完成，一個新狀態(tài)被置位同時定時器的中斷被使能。當(dāng)運行的步數(shù)超過 1 步狀態(tài)機(jī)進(jìn)入 ACCEL 狀態(tài)，如果只移動 1 步，那么狀態(tài)直接變?yōu)?DECEL，因為只移動一步是不需要加速的。當(dāng)狀態(tài)變?yōu)?ACCEL，應(yīng)用程序一直加速步進(jìn)電機(jī)達(dá)到期望的最大速度，這時狀態(tài)變?yōu)?RUN, 或者減速必須開始，狀態(tài)變?yōu)?DECEL。當(dāng)狀態(tài)為 RUN 時候，步進(jìn)電機(jī)會持續(xù)保持最大速度 speed 旋轉(zhuǎn)，直到必須開始減速然后把狀態(tài)改變?yōu)?DECEL。它會一直保持DECEL 狀態(tài)并一直減速到期望的步數(shù)并且速度為 0。然后狀態(tài)變?yōu)?STOP。
加減速情況分析
對于加減速的情況，由于已經(jīng)設(shè)定好了步進(jìn)電機(jī)加速度、減速度、最大速度和步數(shù)，一共分為兩種情況：
第一種情況：持續(xù)加速到最大速度然后再減速到 0
第二種情況：在沒達(dá)到最大速度之前就需要開始減速到 0
第一種情況
根據(jù)上圖 7 個參數(shù)，其中：
speed: 算法設(shè)置的最大速度
accel：加速度
decel：加速度
step ：總步數(shù)
以上的參數(shù)都是程序里直接給出的。
max_s_lim：速度從 0 加速到 speed 所需的步數(shù)
accel_lim：在忽略雖大速度的情況下，開始減速之前的步數(shù)，也可以理解為加速度曲線與減速度曲線的交點；
decel_val：實際減速的步數(shù)；
以上的參數(shù)都是需要根據(jù)前面的計算
max_s_lim：根據(jù)速度與路程的物理公式，所以有以下公式
speed 是擴(kuò)大 100 倍后的數(shù)值，那么平方就是 10000 倍，所以分子需要乘以100，才能保證結(jié)果不變
accel_lim：
如果 max_s_lim<accel_lim ，則通過達(dá)到所需速度來限制加速度；所以減速度取決于此，在這種情況下，通過以下方法找到="" decal_val：<="" p="">
decel_val：
推出decel_val 的表達(dá)式；但是由于是減速度的步數(shù)，所以需要帶上負(fù)號，
具體公式如下：
第二種情況
這種情況是在還未達(dá)到最大速度時就已經(jīng)開始減速了；其中 accel_lim、max_s_lim 不需要重復(fù)計算了；
當(dāng) max_s_lim>accel_lim 時，如上圖加速受減速開始的限制，所以 decel_val 表達(dá)式為：
中斷狀態(tài)
第一種情況：當(dāng)步數(shù)為 1 時，毫無疑問直接進(jìn)入到減速階段然后到停止?fàn)顟B(tài)
第二種情況：當(dāng)步數(shù)大于 1，并且會加到最大速度，會經(jīng)過：加速狀態(tài)-> 勻速狀態(tài)-> 減速狀態(tài)-> 停止?fàn)顟B(tài)
第三種情況：當(dāng)步數(shù)大于 1，并且不會加到最大速度，會經(jīng)過：加速狀態(tài)-> 減速狀態(tài)-> 停止?fàn)顟B(tài)
對于加減速的每一步來說，都需要重新計算下一步的時間，計算的過程中可能會出現(xiàn)除不盡的項式，為了更有利的加減速，可以采用加速向上取整，減速向下取整的原則來做運算，也可以采用余數(shù)累計的方法，在這里使用的是將余數(shù)累計的方法來提高間隔時間的精度和準(zhǔn)確性。
2.梯形加減速的控制實現(xiàn)程序設(shè)計
編程要點
(1) 通用 GPIO 配置
(2) 步進(jìn)電機(jī)、定時器中斷初始化
(3) 在定時器中對速度和狀態(tài)進(jìn)行決策
(4) 通過對步進(jìn)電機(jī)的步數(shù)、加減速度和最大速度的設(shè)置來決定步進(jìn)電機(jī)的運動
對于加減速來說有兩個部分的框架，分別是中斷函數(shù)里面的速度決策調(diào)用和 stepper_move_T() 函數(shù)相關(guān)數(shù)值計算。
2.1 速度決策
/** * @brief 速度決策 *@note 在中斷中使用，每進(jìn)一次中斷，決策一次 * @retval 無 */void speed_decision(void){ uint32_t tim_count=0; uint32_t tmp = 0; // 保存新（下）一個延時周期 uint16_t new_step_delay=0; // 加速過程中最后一次延時（脈沖周期）. static uint16_t last_accel_delay=0; // 總移動步數(shù)計數(shù)器 static uint32_t step_count = 0; static int32_t rest = 0; //定時器使用翻轉(zhuǎn)模式，需要進(jìn)入兩次中斷才輸出一個完整脈沖 static uint8_t i=0; if(__HAL_TIM_GET_IT_SOURCE(&TIM_TimeBaseStructure, MOTOR_TIM_IT_CCx) !=RESET) { // 清楚定時器中斷 __HAL_TIM_CLEAR_IT(&TIM_TimeBaseStructure, MOTOR_TIM_IT_CCx); // 設(shè)置比較值 tim_count=__HAL_TIM_GET_COUNTER(&TIM_TimeBaseStructure); tmp = tim_count+srd.step_delay; __HAL_TIM_SET_COMPARE(&TIM_TimeBaseStructure,MOTOR_PUL_CHANNEL_x,tmp); i++; // 定時器中斷次數(shù)計數(shù)值 if(i==2) // 2次，說明已經(jīng)輸出一個完整脈沖 { i=0; // 清零定時器中斷次數(shù)計數(shù)值 switch(srd.run_state) { /*步進(jìn)電機(jī)停止?fàn)顟B(tài)*/ case STOP: step_count = 0; // 清零步數(shù)計數(shù)器 rest = 0; // 清零余值 // 關(guān)閉通道 TIM_CCxChannelCmd(MOTOR_PUL_TIM, MOTOR_PUL_CHANNEL_x, TIM_CCx_DISABLE); __HAL_TIM_CLEAR_FLAG(&TIM_TimeBaseStructure, MOTOR_TIM_FLAG_CCx); status.running = FALSE; break; /*步進(jìn)電機(jī)加速狀態(tài)*/ case ACCEL: step_count++; srd.accel_count++; new_step_delay = srd.step_delay - (((2 *srd.step_delay) + rest)/(4 * srd.accel_count + 1));//計算新(下)一步脈沖周期(時間間隔) rest = ((2 * srd.step_delay)+rest)%(4 * srd.accel_count + 1);// 計算余數(shù)，下次計算補(bǔ)上余數(shù)，減少誤差 //檢查是夠應(yīng)該開始減速 if(step_count >= srd.decel_start) { srd.accel_count = srd.decel_val; srd.run_state = DECEL; } //檢查是否到達(dá)期望的最大速度 else if(new_step_delay <= srd.min_delay) { last_accel_delay = new_step_delay; new_step_delay = srd.min_delay; rest = 0; srd.run_state = RUN; } break; /*步進(jìn)電機(jī)最大速度運行狀態(tài)*/ case RUN: step_count++; new_step_delay = srd.min_delay; //檢查是否需要開始減速 if(step_count >= srd.decel_start) { srd.accel_count = srd.decel_val; //以最后一次加速的延時作為開始減速的延時 new_step_delay = last_accel_delay; srd.run_state = DECEL; } break; /*步進(jìn)電機(jī)減速狀態(tài)*/ case DECEL: step_count++; srd.accel_count++; new_step_delay = srd.step_delay - (((2 * srd.step_delay) + rest)/(4 * srd.accel_count + 1)); //計算新(下)一步脈沖周期(時間間隔) rest = ((2 * srd.step_delay)+rest)%(4 * srd.accel_count + 1);// 計算余數(shù)，下次計算補(bǔ)上余數(shù)，減少誤差 //檢查是否為最后一步 if(srd.accel_count >= 0) { srd.run_state = STOP; } break; } /*求得下一次間隔時間*/ srd.step_delay = new_step_delay; } }}47~65 行這部分是加速狀態(tài)，在加速狀態(tài)中需要時刻計算下一次的脈沖間隔時間，由于加減速分為兩種情況，這兩種情況可以參考 加減速情況分析所以需要判斷當(dāng)前的步數(shù)是否到達(dá)了需要減速步數(shù)或者已經(jīng)達(dá)到了設(shè)置的最大速度需要開始減速了，根據(jù)不同條件判斷下一狀態(tài)；
67~81 行這部分是以最大速度運行的狀態(tài)；如果說在加速階段判斷下一階段可以達(dá)到最大速度，那么就會跳轉(zhuǎn)到這個狀態(tài)中，那么這個狀態(tài)的下一狀態(tài)一定是減速，所以說需要在這部分使用步數(shù) step_count 的條件來判斷是否到達(dá)了減速階段；
stepper_move_T
tepper_move_T() 這個函數(shù)主要是對給定步數(shù)和加減速度等參數(shù)的計算，將加減速整個過程的最大速度位置最小速度位置以及到達(dá)加減速區(qū)域的步數(shù)等。
/*! \brief 以給定的步數(shù)移動步進(jìn)電機(jī) * 通過計算加速到最大速度，以給定的步數(shù)開始減速 * 如果加速度和減速度很小，步進(jìn)電機(jī)會移動很慢，還沒達(dá)到最大速度就要開始減速 * \param step 移動的步數(shù) (正數(shù)為順時針，負(fù)數(shù)為逆時針). * \param accel 加速度,如果取值為100，實際值為100*0.01*rad/sec^2=1rad/sec^2 * \param decel 減速度,如果取值為100，實際值為100*0.01*rad/sec^2=1rad/sec^2 * \param speed 最大速度,如果取值為100，實際值為100*0.01*rad/sec=1rad/sec */void stepper_move_T( int32_t step, uint32_t accel, uint32_t decel, uint32_t speed){ //達(dá)到最大速度時的步數(shù). unsigned int max_s_lim; //必須開始減速的步數(shù)(如果還沒加速到達(dá)最大速度時)。 unsigned int accel_lim; /*根據(jù)步數(shù)和正負(fù)判斷*/ if(step == 0) { return ; } else if(step < 0)//逆時針 { srd.dir = CCW; step = -step; } else//順時針 { srd.dir = CW; } // 輸出電機(jī)方向 MOTOR_DIR(srd.dir); // 如果只移動一步 if(step == 1) { // 只移動一步 srd.accel_count = -1; // 減速狀態(tài) srd.run_state = DECEL; // 短延時 srd.step_delay = 1000; // 配置電機(jī)為運行狀態(tài) status.running = TRUE; } // 步數(shù)不為零才移動 else if(step != 0) { // 設(shè)置最大速度極限, 計算得到min_delay用于定時器的計數(shù)器的值。 // min_delay = (alpha / tt)/ w srd.min_delay = (int32_t)(A_T_x10/speed); // 通過計算第一個(c0) 的步進(jìn)延時來設(shè)定加速度，其中accel單位為0.1rad/sec^2 // step_delay = 1/tt * sqrt(2*alpha/accel) // step_delay = ( tfreq*0.676/10 )*10 * sqrt( (2*alpha*100000) / (accel*10) )/100 srd.step_delay = (int32_t)((T1_FREQ_148 * sqrt(A_SQ / accel))/10); // 計算多少步之后達(dá)到最大速度的限制 // max_s_lim = speed^2 / (2*alpha*accel) max_s_lim = (uint32_t)(speed*speed/(A_x200*accel/10)); // 如果達(dá)到最大速度小于0.5步，我們將四舍五入為0 // 但實際我們必須移動至少一步才能達(dá)到想要的速度 if(max_s_lim == 0) { max_s_lim = 1; } // 計算多少步之后我們必須開始減速 // n1 = (n1+n2)decel / (accel + decel) accel_lim = (uint32_t)(step*decel/(accel+decel)); // 我們必須加速至少1步才能才能開始減速. if(accel_lim == 0) { accel_lim = 1; } // 使用限制條件我們可以計算出第一次開始減速的位置 //srd.decel_val為負(fù)數(shù) if(accel_lim <= max_s_lim) { srd.decel_val = accel_lim - step; } else{ srd.decel_val = -(max_s_lim*accel/decel); } // 當(dāng)只剩下一步我們必須減速 if(srd.decel_val == 0) { srd.decel_val = -1; } // 計算開始減速時的步數(shù) srd.decel_start = step + srd.decel_val; // 如果最大速度很慢，我們就不需要進(jìn)行加速運動 if(srd.step_delay <= srd.min_delay) { srd.step_delay = srd.min_delay; srd.run_state = RUN; } else { srd.run_state = ACCEL; } // 復(fù)位加速度計數(shù)值 srd.accel_count = 0; status.running = TRUE; } /*獲取當(dāng)前計數(shù)值*/ int tim_count=__HAL_TIM_GET_COUNTER(&TIM_TimeBaseStructure); /*在當(dāng)前計數(shù)值基礎(chǔ)上設(shè)置定時器比較值*/ __HAL_TIM_SET_COMPARE(&TIM_TimeBaseStructure,MOTOR_PUL_CHANNEL_x,tim_count+srd.step_delay); /*使能定時器通道*/ TIM_CCxChannelCmd(MOTOR_PUL_TIM, MOTOR_PUL_CHANNEL_x, TIM_CCx_ENABLE); MOTOR_EN(ON);}點擊“閱讀原文”可查看詳情
Copyright ? 2021-2022 精北傳動機(jī)械（上海）有限公司 All Rights Reserved. 滬公網(wǎng)安備31012002005970 滬ICP備20024360號-1
