步進(jìn)電機(jī)S型曲線(xiàn)加減速算法與實(shí)現(xiàn)
步進(jìn)電機(jī)是將電脈沖信號(hào)轉(zhuǎn)變?yōu)榻俏灰苹蚓€(xiàn)位移的開(kāi)環(huán)控制電機(jī)，是現(xiàn)代數(shù)字程序控制系統(tǒng)中的主要執(zhí)行元件，應(yīng)用極為廣泛。。在非超載的情況下，電機(jī)的轉(zhuǎn)速、停止的位置只取決于脈沖信號(hào)的頻率和脈沖數(shù)，而不受負(fù)載變化的影響，當(dāng)步進(jìn)驅(qū)動(dòng)器接收到一個(gè)脈沖信號(hào)，它就驅(qū)動(dòng)步進(jìn)電機(jī)按設(shè)定的方向轉(zhuǎn)動(dòng)一個(gè)固定的角度，稱(chēng)為“步距角”，它的旋轉(zhuǎn)是以固定的角度一步一步運(yùn)行的?？梢酝ㄟ^(guò)控制脈沖個(gè)數(shù)來(lái)控制角位移量，從而達(dá)到準(zhǔn)確定位的目的；同時(shí)可以通過(guò)控制脈沖頻率來(lái)控制電機(jī)轉(zhuǎn)動(dòng)的速度和加速度，從而達(dá)到調(diào)速的目的。
步進(jìn)電機(jī)是一種感應(yīng)電機(jī)，它的工作原理是利用電子電路，將直流電變成分時(shí)供電的，多相時(shí)序控制電流，用這種電流為步進(jìn)電機(jī)供電，步進(jìn)電機(jī)才能正常工作，驅(qū)動(dòng)器就是為步進(jìn)電機(jī)分時(shí)供電的，多相時(shí)序控制器。
步進(jìn)電機(jī)S型曲線(xiàn)加減速算法與實(shí)現(xiàn)S型曲線(xiàn)的的方程，在［-5，5］的圖形如下圖所示：
如要將此曲線(xiàn)應(yīng)用在步進(jìn)電機(jī)的加、減速過(guò)程中，需要將方程在XY坐標(biāo)系進(jìn)行平移，同時(shí)對(duì)曲線(xiàn)進(jìn)行拉升變化：
其中的A分量在y方向進(jìn)行平移，B分量在y方向進(jìn)行拉伸，ax+b分量在x方向進(jìn)行平移和拉伸。
項(xiàng)目中加速過(guò)程：從5600Hz加速到64000Hz，采用4細(xì)分。輸出比較模塊所用的定時(shí)器驅(qū)動(dòng)頻率為10M，采用1000個(gè)點(diǎn)進(jìn)行加速處理。最終根據(jù)項(xiàng)目的需要，在加速過(guò)程中采用的曲線(xiàn)方程為：
。
其中的Fcurrent為length（1000）個(gè)點(diǎn)中的單個(gè)頻率值。Fmin起始頻率為5600; Fmax為最大頻率64000; -flexible*（i - num）/num是對(duì)S型曲線(xiàn)進(jìn)行拉伸變化，其中flexible代表S曲線(xiàn)區(qū)間（越大代表壓縮的最厲害，中間（x坐標(biāo)0點(diǎn)周?chē)┘铀俣仍酱?；越小越接近勻加速。理想的S曲線(xiàn)的取值為4-6），i是在循環(huán)計(jì)算過(guò)程中的索引，從0開(kāi)始，num為 length/2 大小（這樣可以使得S曲線(xiàn)對(duì)稱(chēng)）。在項(xiàng)目中i的區(qū)間［0，1000）， num=1000/2=500。這些參數(shù)均可以修改。提供的計(jì)算接口如下。
對(duì)應(yīng)的計(jì)算接口code：
/* calculate the Period and Freq array value， fill the Period value into the Period register during the TImer interrupt.
*calculate the acceleraTIon procedure ， a totally 1000 elements array.
* parameter fre［］： point to the array that keeps the freq value.
* period［］： point to the array that keeps the TImer period value.
* len： the procedure of acceleraTIon length.it is best thing to set the float number， some compile software maybe transfer error if set it as a int
* fre_max： maximum speed， frequency vale.
* fre_min： start minimum speed， frequency vale. mind ： 10000000/65535 = 152， so fre_min can‘t less than 152.
* flexible： flexible value. adjust the S curves
*/
void CalculateSModelLine（float fre［］， unsigned short period［］， float len， float fre_max， float fre_min， float flexible）
{
int i=0;
float deno ;
float melo ;
float delt = fre_max-fre_min;
for（; i《len; i++）
{
melo = flexible * （i-len/2） / （len/2）;
deno = 1.0 / （1 + expf（-melo））; //expf is a library function of exponential（e）
fre［i］ = delt * deno + fre_min;
period［i］ = （unsigned short）（10000000.0 / fre［i］）; // 10000000 is the timer driver frequency
}
return ;
}
// start move motor
void StartPWM（）
{
DriverMotorFlag = TRUE;
Index = 0;
MOTOR_EN_DISABLE = ENABLE;
OpenOC4（OC_ON | OC_TIMER_MODE16 | OC_TIMER3_SRC | OC_PWM_FAULT_PIN_DISABLE， 0， 0）;
// map rc13 to oc4 output
RPC13R = 11;
// 50 percent duty
OC4RS = OC_PERIOD_MIN / 2;
OpenTimer3（T3_ON | T3_PS_1_8， OC_PERIOD_MIN）;
INTSetVectorPriority（INT_TIMER_3_VECTOR， INT_PRIORITY_LEVEL_6）;
INTSetVectorSubPriority（INT_TIMER_3_VECTOR， INT_SUB_PRIORITY_LEVEL_1）;
EnableIntT3;
}
//stop motor， hereis no deceleration
void StopPWM（）
{
DriverMotorFlag = FALSE;
Index = 0;
MOTOR_EN_DISABLE = DISENABLE;
OpenOC4（OC_OFF | OC_TIMER_MODE16 | OC_TIMER3_SRC | OC_PWM_FAULT_PIN_DISABLE， 0， 0）;
// map rc13 to oc4 output
RPC13R = 0;
PORTCbits.RC13 = 0;
CloseTimer3（）;
DisableIntT3;
}
//change the timer Period value in the correspond timer rather than the other place， Or the motor will be stalled occasionally.
// 剛開(kāi)始我在另外的一個(gè)定時(shí)器中斷中每隔1ms改變 應(yīng)用在OC模塊的timer3 的Period值，結(jié)構(gòu)偶發(fā)的造成電機(jī)在加速過(guò)程中堵轉(zhuǎn)。其實(shí)應(yīng)該是在timer3的中斷中修改。
static unsigned short CountForAcc = 0;
void __ISR（_TIMER_3_VECTOR， ipl6） Timer3OutHandler（void）
{
// clear the interrupt flag， or the interrupt will not occur again.
mT3ClearIntFlag（）;
if（CountForAcc++ 》 2） // here can adjust the totally time of acceleration
{
CountForAcc = 0;
//if（DriverMotorFlag == TRUE && PR3 》 OC_PERIOD_MAX + SPEED_STEP）
if（DriverMotorFlag == TRUE && Index 《 ACC_TIMES）
{
PR3 = Period［Index++］;
OC4RS = PR3 / 2;
}
}
}
通過(guò)CalculateSModelLine接口得到如下不同的幾條加速曲線(xiàn)：
黃色：CalculateSModelLine（Freq， Period， 1000， 56000， 16000， 4）;
橙色：CalculateSModelLine（Freq， Period， 1000， 64000， 500， 8）;
藍(lán)色：CalculateSModelLine（Freq， Period， 1000， 64000， 500， 15）;
灰色：CalculateSModelLine（Freq， Period， 1000， 40000， 500， 5）;
最后可以估算加速過(guò)程的時(shí)間和角位移，以橙色曲線(xiàn)為例：CalculateSModelLine（Freq， Period， 1000， 64000， 500， 8）為例（假設(shè)在中斷中沒(méi)有 if（CountForAcc++ 》 2） 條件限制）：
時(shí)間：Period第一個(gè)點(diǎn)的值為10000000/500 = 20000，最后也點(diǎn)的值 10000000/64000=156，平均值為10000左右，timer中斷的平均時(shí)間Tn=10000/10000000=1ms， 1000個(gè)點(diǎn)，總時(shí)間為1s，當(dāng)然，起始頻率大加速時(shí)間就越短，比如Fmin=16000Hz，F(xiàn)max=64000，則40ms左右即可完成加速過(guò)程。
角位移：1.8（單步） * 1000（步數(shù)） / 4（細(xì)分）= 450°
上述為加速過(guò)程，減速同樣的道理，只要將方程改為：
可以得到減速曲線(xiàn)如下所示：
下載文檔
