代码收藏家 【STM32篇】S型曲线在步进电机控制中的应用
使用步进电机的S曲线算法的目的是为了使电机缓慢加速到目标转速或从高转速减速到0。防止电机在高转速时立即停止而对电机造成损伤，减少电机的使用寿命。
本文主要讲述S型算法的使用，对于具体的原理，可通过其他博主的文章学习。
图1.S算法加减速图
如图1所示，使用S算法的步进电机运转主要包含S型加速、匀速、S型减速3个阶段。
图中v表示电机转速，t为时间。
S型算法代码如下：
/*
* period: 指向保存计时器周期值的数组
* len: 数组长度
* fre_max: 最大速度，频率值。
* fre_min: 最小速度，频率值。
* flexible: 灵活值。调整S曲线
*/
void CalculateSModelLine(uint16_t * period, uint32_t len, uint16_t fre_max, uint16_t fre_min, float flexible)
{
int32_t i;
float deno ;
float melo ;
float fre ;
float Fdelt = fre_max-fre_min;
if(len>MAX_ACC_PULSE) len=MAX_ACC_PULSE;
for(i=0; i<len; i++){
melo = flexible * (2.0*i/len-1) ;
deno = 1.0 / (1 + expf(-melo));
fre = Fdelt * deno + fre_min;
period[i] = (uint16_t)(TIM_CLOCK/fre);//TIM_CLOCK为定时器频率
}
}该函数的速度（频率）为一个脉冲的频率。例如：定时器计数一次为1us，PWM波的脉宽为1000，即一个脉冲时间为1000us=0.001s，频率f=1/t=1000Hz;
根据该函数，我对S型算法的理解与图1有所不同。如下图
图2.步进电机S曲线脉冲图
这里我将X轴看作脉冲数，Y轴看作一个脉冲所需的时间（或频率）。
电机按指定频率运行
如图2所示，①为加速阶段，x越小即一个脉冲的时间越短，转动一个角度的速度越快。②为匀速阶段，速度达到最大值，③为减速阶段。①③对称。
直接使用void CalculateSModelLine(…)函数，可控制电机在一定角度上按S曲线运转，但不可直接控制电机在该角度上运行时间。
拿电机控制云台振动来说，1/16步进模式下，给电机1600个脉冲使云台向上10cm，再给电机1600个脉冲使它往下10cm，反复切换方向，减少脉冲时间，便可增大云台振动频率。
计算过程：
如图2.设电机运行范围为range，频率为frequency,即控制电机的脉冲数为range，总时间为1/frequency。求出最大速度。
设匀速运行阶段一个脉冲的定时器计数值为TIM_min,加速和减速阶段平均计数值为2TIM_min（当然觉得误差大也可改为(TIM_max+TIM_min)/2），不会影响后面的计算。设定S型曲线脉冲数为一定值NUM。
所以有：
(1/frequency)/(1/TIM_CLOCK) = (range -2* MUX)*TIM_min + 2*NUM * 2TIM_min;
TIM_min = TIM_CLOCK / (frequency * (range +2* NUM) );
TIM_min为计数值，1/frequency 为总时间，(1/frequency)/(1/TIM_CLOCK)为总计数值
TIM_min * 1/TIM_CLOCK便是一个脉冲周期，
倒数即为脉冲频率：TIM_CLOCK/TIM_min = frequency * (range +2* MUX)；
所以匀速运行时的脉冲频率为frequency * (range +2* NUM)
代码如下：（运行范围小于2*NUM属于没有匀速阶段，只有加速和减速）
/*
freq:频率HZ，即range个脉冲需要的时间
range:范围，即脉冲数
return：最大脉冲频率
*/
uint16_t calculate_S_MaxHZ(uint16_t freq, uint16_t range)
{
uint32_t maxHz;
if(freq == 0 || range == 0)
{
return 0;
}
if(range > MOTOR_S_NUM * 2)
{
maxHz = freq * ( range + MOTOR_S_NUM * 2);
}
else
{
maxHz = 2 * range * freq ;
}
return maxHz;
}这个函数算出的maxHz，存在这一定误差，freq越大，实际运行时间越长。
图3.配置motor工作 1HZ频率
如图3所示，电机S曲线运行500个脉冲期望时间为1s,而实际时间为0.974s，看似误差不大。
提高工作频率，freq = 4，理论时间为0.25s,实际时间为0.34s，即在规定时间内，电机无法运行到指定目的地。
图4.配置motor工作 4HZ频率
解决办法：
| timer1 – timer2 |为理论时间与实际时间的差值，将其转换为每个脉冲的时间差（均数）：
| timer1 – timer2 | / range ；
每个脉冲定时器计数差值（均数）为：| timer1 – timer2 | / range /（1 / TIM_CLOCK）
按理说，实际脉冲与理论脉冲在周期相差一定值，那在实际脉冲上增加或减去相差的时间，总时间就会相等。但这是S曲线脉冲，所求得的最大值脉冲频率并非均值，所以实际时间和理论时间只会慢慢接近。
代码实现：
/*
freq:频率HZ，即range个脉冲需要的时间
range:范围，即脉冲数
maxHz:最大脉冲频率
*/
void calculate_S_PulseFreq(uint16_t freq, uint16_t range)
{
uint32_t maxHz;
float timer1,timer2;
uint32_t Vibrate_Time=0;
if(freq == 0 || range == 0)
{
return ;
}
if(range > MOTOR_S_NUM * 2)
{
maxHz = freq * ( range + MOTOR_S_NUM * 2);
}
else
{
maxHz = 2 * range * freq ;
}
//计算准确时间
while(1)
{
Vibrate_Time = 0;
CalculateSModelLine(motor_S_period, MOTOR_S_NUM, maxHz, min_Hz, 6.0);
for(uint8_t i=0;i<MOTOR_S_NUM;i++)
{
Vibrate_Time += (motor_S_period[i]);
}
Vibrate_Time = 2*Vibrate_Time+((range - MOTOR_S_NUM * 2) * motor_S_period[MOTOR_S_NUM - 1]);//总计数
timer1 = Vibrate_Time*1.0 / (TIMER4_CLOCK); //实际时间
timer2 = 1.0/freq; //理论所需的时间
//再计算最大脉冲频率，确保误差在1ms以内
if(timer1 > timer2)
{
if((timer1-timer2)>0.001)
{
maxHz = TIMER4_CLOCK/( TIMER4_CLOCK/maxHz - (timer1-timer2)*TIMER4_CLOCK/range);
}
else break;
}
else
{
if((timer2-timer1)>0.001)
{
maxHz = TIMER4_CLOCK/( TIMER4_CLOCK/maxHz + (timer2-timer1)*TIMER4_CLOCK/range);
}
else break;
}
}
}在计算脉冲最大频率的函数里直接求出最大脉冲频率，进而慢慢逼近。那问题来了，会不会在循环算S曲线脉冲时，耗费大小时间？
1HZ工作频率频率下：循环3次便可求出最大脉冲频率，总时间误差值在1ms内。
4HZ工作频率下：原实际时间为0.347s,理论时间为0.25s,差值较大。在5次循环中可将误差值减少至1ms以内。
具体情况受电机运行范围和运行频率的影响，在具体还得看使用环境。
当然，也可将频率改为秒单位时间等。
/*
sTime:运行时间
range:范围，即脉冲数
maxHz:最大脉冲频率
*/
void calculate_S_PulseFreq(uint16_t sTime, uint16_t range)
{
uint32_t maxHz;
float timer1,timer2;
uint32_t Vibrate_Time=0;
if(freq == 0 || range == 0)
{
return ;
}
if(range > MOTOR_S_NUM * 2)
{
maxHz = 1/sTime * ( range + MOTOR_S_NUM * 2);
}
else
{
maxHz = 2 * range / sTime ;
}
//计算准确时间
uint8_t k=0;
while(1)
{
k++;
Vibrate_Time = 0;
CalculateSModelLine(motor_S_period, MOTOR_S_NUM, maxHz, min_Hz, 6.0);
for(uint8_t i=0;i<MOTOR_S_NUM;i++)
{
Vibrate_Time += (motor_S_period[i]);
}
Vibrate_Time = 2*Vibrate_Time+((range - MOTOR_S_NUM * 2) * motor_S_period[MOTOR_S_NUM - 1]);//总计数
timer1 = Vibrate_Time*1.0 / (TIMER4_CLOCK); //实际时间
timer2 = 1.0/freq; //理论所需的时间
//再计算最大脉冲频率，确保误差在1ms以内
if(timer1 > timer2)
{
if((timer1-timer2)>0.001)
{
maxHz = TIMER4_CLOCK/( TIMER4_CLOCK/maxHz - (timer1-timer2)*TIMER4_CLOCK/range);
}
else break;
}
else
{
if((timer2-timer1)>0.001)
{
maxHz = TIMER4_CLOCK/( TIMER4_CLOCK/maxHz + (timer2-timer1)*TIMER4_CLOCK/range);
}
else break;
}
}
}重新梳理：
2023/5/30
此次为了更清晰的理解计算过程，上述方式同样可行。
如图2所示，将S曲线理解为一个S加速段，匀速段，S减速段运动，将y坐标看做脉冲周期，x轴为脉冲数。
void CalculateSModelLine(uint16_t * period, uint32_t len, uint16_t fre_max, uint16_t fre_min, float flexible)；该函数中fre_max和fre_min分别为脉冲最大和最小频率，可设定其中一个参数为已知量，此处将fre_min设为已知量500，即脉冲周期（时间）为1/fre_min = 2ms。（该速度已经很慢了，可根据实际情况设定最小频率）。
计算过程：
-
特定频率下电机往返转动。
期望频率freq，期望时间：1/freq；
计算时间 = S运动时间 + 匀速运动时间；
匀速运动脉冲时间：1/fre_max； //可理解为均值
S运动脉冲时间：（1/fre_max + 1/fre_min）/ 2； //可理解为中值
（1/fre_max + 1/fre_min）/ 2 * 2 * len + 1/fre_max * (range – 2* len) = 1/freq ;
两段S曲线运动时间 + 匀速运动时间；
len：一段S曲线的脉冲数
range：总脉冲数
最终结果：fre_max = (freq * fre_min *(range – len))/(fre_min – len * freq);
CalculateSModelLine(period, len, fre_max, fre_min, 6.0)；调用该函数，便可计算出S曲线的TIM计数值，结果保存至period中。
代码如下：定义变量有所不同
其中：
#define NUM 100
#define min_Hz 500
#define TIMER2_CLOCK SystemCoreClock/(timer_prescaler_read(TIMER2)+1)
uint16_t motor_S_period[100];
/*
Calculate S model line
计算加减速的S型曲线
param <in> period uint32_t* 存放S曲线数据的数组
len uint32_t 数组长度
fre_max uint32_t 最大频率
fre_min uint32_t 最小频率
float flexible S型曲线调整系数，flexible越大，曲线越陡
return void
*/
void CalculateSModelLine(uint16_t * period, uint32_t len, uint16_t fre_max, uint16_t fre_min, float flexible)
{
int32_t i;
float deno ;
float melo ;
float fre ;
float Fdelt = fre_max-fre_min;
if(len>MAX_ACC_PULSE) len=MAX_ACC_PULSE;
for(i=0; i<len; i++){
melo = flexible * (2.0*i/len-1) ;//melo = flexible * (i-len/2) / (len/2); //flexible is (0~10),adjust the S curves
deno = 1.0 / (1 + expf(-melo)); //expf is a library function of exponential(e)
fre = Fdelt * deno + fre_min;
period[i] = (uint16_t)(TIMER2_CLOCK/fre);//(72000000.0 / fre); // 72000000 is the timer drive frequency,64 prescaler
if(period[i] % 2)
period[i]=period[i]-1;//化奇为偶
}
}
/*
freq:频率HZ，即range个脉冲需要的时间
range:范围，即脉冲数
maxHz:最大脉冲频率
*/
void calculate_S_PulseFreq(uint16_t freq, uint16_t range)
{
uint32_t maxHZ;
float timer1,timer2;
uint32_t Vibrate_Time=0;
if(range > NUM * 2)
{
/*
计算过程：计算时间 = 预期时间
NUM（1/Max_HZ + 1/Min_HZ） + (range - 2 * NUM) * 1/Max_HZ = 1/freq;
*/
maxHZ = ((freq) * min_Hz * (range - NUM)) / (min_Hz - NUM * freq);
//计算准确时间
while(1)
{
Vibrate_Time = 0;
CalculateSModelLine(motor_S_period, NUM, maxHZ, min_Hz, 6.0);
for(uint16_t i=0;i<NUM;i++)
{
Vibrate_Time += (motor_S_period[i]);
}
Vibrate_Time = 2*Vibrate_Time+((range - NUM * 2) * motor_S_period[NUM - 1]);//总计数
timer1 = Vibrate_Time*1.0 / (TIMER2_CLOCK); //实际振动时间
timer2 = 1.0/(freq); //理论所需的时间
if(timer1 > timer2)
{
if((timer1-timer2)>0.001)//实际时间大于理论时间
{
maxHZ = TIMER2_CLOCK/( TIMER2_CLOCK/maxHZ - (timer1-timer2)*TIMER2_CLOCK/range);//增大最大脉冲频率
}
else break;
}
else
{
if((timer2-timer1)>0.001)
{
maxHZ = TIMER2_CLOCK/( TIMER2_CLOCK/maxHZ + (timer2-timer1)*TIMER2_CLOCK/range);//减小最大脉冲频率
}
else break;
}
}
}
}在精确计算时间时，先计算S曲线中的100个脉冲，motor_S_period中保存的数据为定时器计数值（而非直接脉冲频率），随后计算所有脉冲的和,再乘上定时器频率的倒数即为这500个脉冲所用的时间。这个时间和预期的时间是有差别，只能进行进一步的计算才能接近预期值。
这里我的计算是考虑实际时间与预期时间的差值，实际时间大，则可说明在计算fre_max时fre_max值大了，就需要减小。减小多少？这里采取时间差值的平均数。如果实际时间小，那就加上这个差值。
maxHZ = TIMER2_CLOCK/( TIMER2_CLOCK/maxHZ – (timer1-timer2)*TIMER2_CLOCK/range);
对于这个式子，计算的还是频率，TIMER2_CLOCK需要保留。
如果去掉TIMER2_CLOCK，1/maxHZ为最大脉冲时间， (timer1-timer2)/range为差值的平均时间，看似没问题，但这个是用机器语言计算，在计算过程中可能会出现问题。（在实际测试过程中会产生问题）
1/maxHZ为时间，再除以1/TIMER2_CLOCK便可将时间转换为TIM计数值，最后再转换为频率即可。
实际测试：
这里需要电机运行500个脉冲，并在0.25s（4HZ）内完成。
第一次计算S(k=1)，实际时间0.166207999s；实际时间小，maxHZ就需要加大，就执行下面语句。
第二次计算S（k=2）,此时看到maxHZ由8000变为3417，频率加大了。实际时间也变为0.26465866，也接近0.25了。
第三次计算S（k=3）,实际时间0.248768661
第四次计算S（k=4）,实际时间0.250145346，这个时间已经在我的预期时间范围（±1ms）内，所以计算已经完成。在使用过程中，担心K值太大，可在上电后立即计算S，以后只需从数组中读取数值给定时器使用即可（写入重装载寄存器中）。
为了方便演示，直接使用串口打印。
如下图，为电机运行时的3个过程。
S曲线的使用
1.电机制定时间运行至目标位置。
main.c
#include "motor.h"
#include "systick.h"
void clock_config(void);
void nvic_config(void);
int main(void)
{
clock_config(); //配置RCC时钟
nvic_config(); //配置中断优先级
MOTOR_Init(); //motor初始化
MOTOR_ConfigWork(CW,500,1);//顺时针56°
while(1);
}
void clock_config(void)
{
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
}
void NVIC_IQR_Confing(uint8_t nvic_IRQChannel,uint8_t nvic_PreemptionPriority, uint8_t nvic_SubPriority)
{
NVIC_InitTypeDef nvic_Init;
nvic_Init.NVIC_IRQChannel = nvic_IRQChannel;//中断号
nvic_Init.NVIC_IRQChannelPreemptionPriority = nvic_PreemptionPriority;//抢占优先级
nvic_Init.NVIC_IRQChannelSubPriority = nvic_SubPriority;//子优先级
nvic_Init.NVIC_IRQChannelCmd = ENABLE;//启用中断优先级
NVIC_Init(&nvic_Init);
}
void nvic_config(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//配置中断优先级分组
NVIC_IQR_Confing(TIM4_IRQn,1,0);
}2.脉冲波形：
3.实验结果：
最后，由于是我在手动计算出来编写的函数，在一些步骤上可能合理度不够，也不简洁，所以各位在浏览文章的时候有更好的方法，也可在评论区中分享分享。希望能帮助到正在学习步进电机的朋友们。如果找到更好的方法，也会慢慢分享到博客上。
欢迎指正！
2023年3月14日
