S型曲線的的方程,在[-5,5]的圖形如下圖所示:
如要將此曲線應用在步進電機的加、減速過程中,需要將方程在XY座標系進行平移,同時對曲線進行拉昇變化:
其中的A分量在y方向進行平移,B分量在y方向進行拉伸,ax+b分量在x方向進行平移和拉伸。
項目中加速過程:從5600Hz加速到64000Hz,採用4細分。輸出比較模塊所用的定時器驅動頻率爲10M,採用1000個點進行加速處理。最終根據項目的需要,在加速過程中採用的曲線方程爲:。
其中的Fcurrent爲length(1000)個點中的單個頻率值。Fmin起始頻率爲5600; Fmax爲最大頻率64000; -flexible*(i - num)/num是對S型曲線進行拉伸變化,其中flexible代表S曲線區間(越大代表壓縮的最厲害,中間(x座標0點周圍)加速度越大;越小越接近勻加速。理想的S曲線的取值爲4-6),i是在循環計算過程中的索引,從0開始,num爲 length/2 大小(這樣可以使得S曲線對稱)。在項目中i的區間[0,1000), num=1000/2=500。
提供的計算接口如下。
對應的計算接口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 ;
}
以上基本原理摘自CSDN一篇文章
在脈衝模式下,步進電機啓動器的脈衝頻率與速度成正比,函數void CalculateSModelLine(float fre[], unsigned short period[], float len, float fre_max, float fre_min, float flexible)主要目的就是生成每一個細分步加速的速度。芯片用的是stm32f103c8t6,驅動器用的是2DM420,親自測試效果還是很不錯的。
程序流程
1.STM32硬件基本初始化,主要是針對定時器。
2.調用函數CalculateSModelLine()生 成每一個細分步定時器的自動重裝載值(實際就是改變脈衝的週期)。
3.打開定時器。
4.更新中斷髮生,將數組中的數據賦值給定時器的自動重裝載寄存器。
5.跳出中斷後脈衝頻頻率就變化了。
6.下一次更新中斷產生。
部分核心代碼
#define ACCELERATED_SPEED_LENGTH 3000 //定義加速度的點數(其實也是3000個細分步的意思),調這個參數改變加速點
#define FRE_MIN 3000 //最低的運行頻率,調這個參數調節最低運行速度
#define FRE_MAX 24000 //最高的運行頻率,調這個參數調節勻速時的最高速度
int step_to_run; //要運行的步數
float fre[ACCELERATED_SPEED_LENGTH]; //數組存儲加速過程中每一步的頻率
unsigned short period[ACCELERATED_SPEED_LENGTH]; //數組儲存加速過程中每一步定時器的自動裝載值
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)(1000000.0 / fre[i]); // 10000000 is the timer driver frequency
}
return ;
}
//主函數
int main(void)
{
u8 t;
delay_init();
NVIC_Config();
LED_Init(); //初始化IO口
TIM1_Int_Init(10,72); //設置定時器的頻率等
//PB8 9 12 13
//KEY_Init();
//SMG_io_init();
CalculateSModelLine(fre,period,ACCELERATED_SPEED_LENGTH,FRE_MAX,FRE_MIN,4);
TIM_Cmd(TIM1, ENABLE);
while(1)
{
}
}
定時器初始化
void TIM1_Int_Init(u16 arr,u16 psc)
{
NVIC_InitTypeDef NVIC_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_DeInit(TIM1); /*¸´Î»TIM1¶¨Ê±Æ÷*/
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); /*¿ªÊ±ÖÓ*/
TIM_TimeBaseStructure.TIM_Period = arr; /*ʱÖӵδðµÄ´ÎÊý£¬¹»ÊýÖжÏÕâÀïÊÇ1msÖжÏÒ»´Î*/
TIM_TimeBaseStructure.TIM_Prescaler = psc; /* ·ÖƵ720*/
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; /*¼ÆÊý·½ÏòÏòÉϼÆÊý*/
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
/* Channel 1, 2,3 and 4 Configuration in PWM mode */
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; //²»Ê¹ÓÃNµÄÊä³ö£¬ËùÒÔÒª¹Øµô
TIM_OCInitStructure.TIM_Pulse = 0; //µÚÒ»´ÎûÓÐÊä³öÂö³å
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;
TIM_OC4Init(TIM1, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable);
/* Clear TIM1 update pending flag Çå³ýTIM1Òç³öÖжϱêÖ¾] */
TIM_ClearFlag(TIM1, TIM_FLAG_Update);
NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_IRQn; /*Òç³öÖжÏ*/
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* TIM1 counter enable */
TIM_Cmd(TIM1, DISABLE);
/* TIM1 Main Output Enable */
TIM_CtrlPWMOutputs(TIM1, ENABLE);
TIM_ClearFlag(TIM1, TIM_FLAG_Update);
TIM_ITConfig(TIM1,TIM_IT_Update,ENABLE);
}
//初始化定時器IO
void LED_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB, ENABLE);
//設置PA11爲複用推輓輸出
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_8 | GPIO_Pin_11 | GPIO_Pin_12;
GPIO_Init(GPIOA, &GPIO_InitStructure);
//Âö³å¿Ú+·½Ïò
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
GPIO_Init(GPIOB, &GPIO_InitStructure);
/* TIM1 Full remapping pins */
GPIO_PinRemapConfig(GPIO_FullRemap_TIM1, ENABLE); //打開重映射
}
定時器更新中斷
int i = 0;
void TIM1_UP_IRQHandler(void)
{
TIM_ClearFlag(TIM1, TIM_FLAG_Update);
if(i < ACCELERATED_SPEED_LENGTH && g_motor_dir_s == 0) //加速,直到步數達到 ACCELERATED_SPEED_LENGTH
{
TIM1->ARR= period[i];
TIM1->CCR4= period[i]/2;
i++;
step_to_run = 20000; //這裏設置要跑的步數,當然可以寫在main函數中,這裏沒有把加減速的步數算上
}
else if(step_to_run > 0) //勻速
{
step_to_run--;
}
else if(i > 0 && g_motor_dir_s == 1) //電機減速
{
i--;
TIM1->ARR= period[i];
TIM1->CCR4= period[i]/2;
}
if(i == ACCELERATED_SPEED_LENGTH || i == 0)
{
g_motor_dir_s = (~g_motor_dir_s)&0x01;
}
}