STM32步进电机S型加减速算法_电机缓起缓停 算法

简单说明一下硬件资源，需要用到STM32两个定时器，TIM1产生PWM脉冲并对脉冲个数计数，TIM2开启定时中断用于算法的实现。采用CubeMX+Hal库配置，这里不做详细介绍，重点介绍S型加减速算法的实现。

首先了解一下S曲线函数，f(x)=1/(1+e^(-x))，这是S曲线最原始的函数，为什么叫“最原始的函数”，因为需要对其进行变换才能为我们所用，后面会详细介绍。取x∈[-10,+10]之间的所有整数在excel（是个好东西）里绘制S曲线图像，可以看到在x=-7和x=+7时f(x)分别接近最小值0和最大值1，所以我们就假定x=-7时f(x)min=0，x=+7时f(x)max=1 （使用过程中x=±7时已经满足我的精度要求，如果你需要更高的精度x可以取±8、±9...）。

在实际的过程中我们更希望x=0时f(x)=0，这样我们就能更好的把控程序，该怎么办呢？函数变换。我们高中时都学过把函数图像沿x轴平移、沿y轴平移、函数拉伸等等，上面讲到原始函数图像在x=-7时f(x)=0，如果把曲线图像向沿x轴向右平移7个单位，f(x)在x=0时不就等于0了？曲线图像向右平移几个单位那么x减几，向左平移几个单位那么x就加几，我们需要向右平移7个单位那么-(x-7)即可，得到一个新的函数f1(x)：

可以发现经过右移7个单位后得到的函数，当x=0时f1(x)的值等于原始函数在x=-7时的值，x=14时f(x)的值等于原始函数在x=7时的值，这样我们就把原始函数f(x)中x∈[-7,+7]的f(x)值域转移到经过第一次变换后得到的1f(x)  x∈[0,+14]的值域了，是不是离目标更近一步了？别急。

经过第一次变换后得到的f1(x)在x∈[0,+14]时，x取该区间里的15个整数f1(x)即可由最小值0按照S型轨迹增加到最大值1，当然这期间x也可以取值为0.1、0.2、0.3......13.7、13.8、13.9等等这样的小数。但是我们程序里的设计思路是按次加速，加速n次后频率要达到最大。“次”是整数，我们也不可能说加速1.1次、加速1.2次，当x∈[0,+14]时，经过14次加速即可加速至至最大，显然加速过程不够平滑。我们希望x在某个区间里可以取值100个、200个、任意个整数，f(x)都能够按照S型轨迹变化，这样加速次数就可以灵活控制，这时我们需要对f(x)做横向拉伸变换。

我们先看一下效果：

图3是x的系数除以2，得到的函数记为f2(x)。

图4是x的系数乘以2，得到的函数记为f3(x)。

可以看到，f2(x)和f3(x)达到相同的最小值最大值时能够取得的x最大整数值分别为f1(x)的2倍、1/2倍。f1(x)在x=14时取得最大值、f2(x)在x=28时取得最大值、f3(x)在x=7时取得最大值，且最大值均相等。记f(x)=1/(1+e^(-ax+b))，那么我们是否可以认为当f(x)的值由0按照S型轨迹增至1时，能够取得x的整数个数为14*(1/a)个？OK，我们再来验证一下，取a=0.1，b=7，得到的函数记为f4(x)：

果然，x能够取得最大的整数值为140，为f1(x)的x=14的10倍，也就是说我们可以加速140次至最大值，假设成立！

看到这儿是不是觉得胜利就在眼前了？最后我们再来优化一下，通过上诉变换我们可以得到14的整数倍数的最大加速次数，如果我们能得到1或者10的整数倍数的最大加速次数不是更好？很简单只需把前面讲到的-ax+b改为-(14/n)x+b,其中14/n = a，n为加速次数，例如我们想通过100次加速，那么函数即为f(x)=1/(1+e^((14x/100)+7))，我们前面首次变换时将函数图像沿x轴右移了7个单位（ x∈[-7,+7] ），而14=7*2，数学是不是很美妙？有空可以验证一下如果向右平移8个单位、9个单位...，是不是可以将x的系数该为16/n、18/n？

在驱动步进电机过程中我们希望PWM脉冲频率按照S型轨迹变化，也就说f(x)的值对应PWM脉冲频率Freq。上诉函数中f(x)大小都在1以下，我们需要PWM脉冲频率达到几十k或者几百k。这这个简单呀，乘以倍数Freq=n*f(x)，最大20kHz，就乘以20000、最大100kHz就乘以100000。

我们来看一个很漂亮的S曲线：

按照上面讲的变换规则，最大PWM脉冲频率50kHz、经过500次加速至最大频率，生成的S曲线。

我们讲完了所有的S曲线变换规则，那么到底如何通过程序实现PWM脉冲频率按照S型曲线变换呢？

最初想通过公式计算得到本次的PWM脉冲频率值，算一次数据更新一下自动重载值，但是涉及到浮点数运算，对于STM32F103这样不带FPU的单片机来说计算一次数据需要100us+的时间，比较耗时。那么我们能不能提前算好数据，在调节频率时让单片机自己选择一个数据来更新重载值呢？好主意！

void CurveS_init(uint32_t *pbuff,uint32_t freq,int16_t count)
{
	int16_t i;
 
	for(i = 0;i<count;i++)
	{
	    *pbuff++ = (uint32_t)ceil(((float)(freq-PWM_START_FREQ))/(1.0+exp((-i*14.0/count)+7.0)));
	}	
}

通过上面这个函数即可生成S曲线参数，函数的原型即为图6中的f(x)稍加变换。

pbuff：存储S曲线参数        freq：你需要的最大频率        count：你需要的加速次数

count越大，S曲线越平缓，加速过程越平滑，加速时间越长；count越小，S曲线越陡峭，加速过程越急剧，加速时间越短。

有了S曲线生成函数后，我们需要做的就是在程序初始化时生成一组S参数：

uint32_t CurveS_Para[500];

CurveS_init(CurveS_Para,50000,500);

这500个S曲线参数就是x∈[0,+500]之间取整时f(x)的值，把这500个点连接起来就成S型曲线。

有了S参数，我们就可以调速了，速度调节函数如下：

void SpeedAdjust(uint16_t count)
{
	Motor.Speed = CurveS_Para[count]*Motor.FrePropor/50+PWM_START_FREQ;    //计算本次速度
	
	htim1.Instance->ARR = TIM1_CLOCK_FREQ/Motor.Speed; 
	htim1.Instance->CCR1 = (TIM1_CLOCK_FREQ/Motor.Speed)/2;
	
	if(Motor.Status == SPEED_INCREASE)            //加速
	{
		Motor.CountTemp++;
	}
	else if(Motor.Status == SPEED_DECREASE)       //减速
	{
		Motor.CountTemp--;	
	}
	Motor.Count = Motor.CountTemp*5/Motor.CountPropor;    //加速次数
}

count为加速次数，我们通过CurveS_Para[count]调用生成的S参数，Moto.FreqPropor为频率比例，MotorCountPropor为加速次数比例。由于我们是按照最大频率50kHz、最大加速次数500次（就是说加速500次频率可以达到50kHz）生成的S曲线，在实际使用过程中最大频率可能是任意值20kHz、30kHz等等，加速次数也可能是任意值200次、400次等等，但是我们只有一组S曲线参数，所以我们需要通过比例来对最大目标频率以及相应的加速次数做一下缩放。简单来说就是插值法，500个按照某个规律排列的数据中，每隔相同的间距取其中的一个数据组成一组新的数据，那么这组新数据应和原数据有相同的规律。

我们现在有一个数组CurveS_Para[500]，里面的的元素按照S型函数规律排列，如果每隔5/3的间距在这个数组中据中取出一个元素然后组成一个新的数组，那么我们可以在原数组中取300个元素，且组成的新数组里的元素同样是按照S型函数排列。而速度调节函数中的5/Motor.CountPropor就是我们的取数间隔，如果300次加速至最大值Motor.CountPropor=3即可。本次调速需要生效的频率值同样需要通过比例计算得到 CurveS_Para[count]*Motor.FrePropor/50，如果我们最大的目标频率为30kHz，Motor.FrePropor=30即可，当加速次数count达到最大499时，速度同样达到最大。

速度调节函数中真正生效的是Motor.Count这个参数，在后面的调速中Motor.Count作为参数传递给SpeedAdjust()函数中。Motor.CountTemp是用来计算Motor.Count的一个中间变量，也是实际的已经加速的次。在加速过程中，每进行一次加速调用一次SpeedAdjus()函数，同时Motor.CountTemp++，记录加速次数。加速过程中频率按照S型规律由最小增到最大，对应的S参数CurveS_Para[0]增加到CurveS_Para[499]。而减速过程中频率同样按照S型规律由最大减至最小，参数CurveS_Para[499]减小到CurveS_Para[0]，同时同时Motor.CountTemp--记录剩余的减速次数。简单的说，加速过中将CurveS_Para数组中的元素从前往后代入频率计算，减速过程中将CurveS_Para数组中的元素从后往前代入频率计算。OK，是不是很简单？

最后看一下速度调节状态机，电机状态由 加速->匀速->减速 变化，在加速和减速阶段调用速度调节函数。加速过程中加速次数达到最大后停止加速转为匀速运动，至于什么时候开始减速由你实际的程序决定。

void SpeedAdjustMachine(void)
{
	switch(Motor.Status)
	{
		/*加速*/						
		case SPEED_INCREASE:
			if(Motor.Count <= COUNT_MAX)
			{
				SpeedAdjust(Motor.Count); 
			}
			else
			{
				Motor.Status = SPEED_STABLE;
			}
			break;
 
		/*匀速*/	
		case SPEED_STABLE:
			if(Motor.PWMcount >= (Motor.PWMneed-Motor.SpeedDecrPWM))
			{		
				Motor.Status = SPEED_DECREASE;	
			}
			break;
				
		/*减速*/	
		case SPEED_DECREASE:
			if(Motor.Count >= 0)
			{
				SpeedAdjust(Motor.Count); 
			}
							
			if(Motor.PWMover == 1)
			{		
				HAL_TIM_PWM_Stop_IT(&htim1, TIM_CHANNEL_1);
			}
			break;
				
		default :
			break;
	}
}

速度调节状态接在另一个定时器的定时中断中调用，多久调节一次速度由该定时器的中断频率决定，500us、1ms、2ms都行。中断频率和加速次数共同决定电机的加减速时间，需要根绝电机能够承受的最大加速度调节加速次数以及调速时间间隔。

最后再看一下实际的效果，每100us读取一次TIM1的自动重载值ARR，计算当前PWM脉冲，生成的S曲线图像：

通过示波器可以观察到频率由大减小，由小增大的一个过程。