步进电机S型曲线加减速算法

项目场景：

项目场景：项目中Z轴电机对速度和精度提出了要求，现在需要平滑的进行电机控制，并能在到达定位之前提前降低速度。遂采用S曲线加减速算法对步进电机进行控制。

原理

原理部分参考这篇优秀文章，看懂后结合项目情况进行了功能实现。
S曲线方程：

硬件资源：

TIM1作为主定时器进行脉冲输出
TIM4作为从定时器对TIM1动态修改频率。
TIM5对编码器（光栅）进行计数
TIM6进行PID闭环控制

具体实现：

曲线方程生成数组：

uint32_t CurveS_Para[100]={0};  //频率列表

/*
 正常版
*pbuff 存放S曲线的脉冲对应 ARR寄存器的值
 fre_max  最大频率
 fre_min  起始频率
 count 需要加速次数
*/
void CurveS_init_1(uint32_t *pbuff,uint32_t fre_max, uint32_t fre_min,int16_t count)
{
	int16_t i;
	uint16_t flexible =4;//曲线区间4-6
	float del_fre = fre_max-fre_min;
	float deno;
	float melo;
	float fre;

	for(i = 0;i<count;i++)
	{
		melo = flexible* (i-count/2.0f) / (count/2.0f);
		deno = 1.0f / (1 + expf(-melo));
		fre = del_fre * deno + fre_min;
	    pbuff[i] = (uint32_t)ceil((float)TIM1_CLOCK_FREQ/fre);
	}	
}

仿真效果

拉伸变换后：

void CurveS_init_2(uint32_t *pbuff,uint32_t fre_max, uint32_t fre_min,int16_t count)
{
	int16_t i;
	float del_fre = fre_max-fre_min;
	float deno;//分母部分
	float melo;//指数函数自变量
	float fre;

	for(i = 0;i<count;i++)
	{
		melo = ((-i*14.0f) /count)+7.0f;
		deno = 1.0f / (1.0f + expf(melo));
		fre = del_fre * deno + fre_min;
	    pbuff[i] = (uint32_t)ceil((float)TIM1_CLOCK_FREQ/fre);
	}	
}

仿真效果

状态机控制：

主从定时器参考我之前这篇文章：传送门
加减速控制

void SpeedAdjust(uint16_t count)
{
	
	TIM_SetAutoreload(TIM1,CurveS_Para[count]);//设定自动重装值	
	TIM_SetCompare2(TIM1,CurveS_Para[count]>>1); //匹配值2等于重装值一半，是以占空比为50%	
	TIM_CtrlPWMOutputs(TIM1,ENABLE);	//MOE 主输出使能		
	
	if(Motor.Status == SPEED_INCREASE)            //加速
	{
		Motor.CountTemp++;
	}
	else if(Motor.Status == SPEED_DECREASE)       //减速
	{
		Motor.CountTemp--;	
	}
	//Motor.Count = Motor.CountTemp*(50/Motor.CountPropor);    //加速次数
	Motor.Count = Motor.CountTemp; 
}

状态机放在TIM4中断中

void SpeedAdjustMachine(void)
{
	switch(Motor.Status)
	{
		/*加速*/						
		case SPEED_INCREASE:
			if(Motor.Count >= Motor.COUNT_MAX)
			{
				Motor.Status = SPEED_STABLE;
			}
			else
			{
				SpeedAdjust(Motor.Count); 
				
			}
			break;

		/*匀速*/	
		case SPEED_STABLE:
			if(Motor.PWMcount >= (Motor.PWMneed-Motor.SpeedDecPWM))
			{		
				Motor.Status = SPEED_DECREASE;	
			}
			break;
				
		/*减速*/	
		case SPEED_DECREASE:
			if(Motor.Count >=1)
			{
				SpeedAdjust(Motor.Count); 
			}
			if(Motor.PWMcount==Motor.PWMneed )
			{
				Motor.Status = Stop;	
			}
			break;
		/*停止*/
		case Stop:	
			TIM_CtrlPWMOutputs(TIM1,DISABLE);
			TIM_Cmd(TIM1,DISABLE);
			TIM_Cmd(TIM4,DISABLE);	
			Motor.CountTemp = 0;
			Motor.PWMcount  = 0;
			TIM_ITConfig( TIM4, TIM_IT_Update, DISABLE);
			break;
		/*无加减速*/
		case UNIFM:
			if(Motor.PWMcount==Motor.PWMneed )
			{
				Motor.Status = Stop;	
			}
			break;
		default :
			break;
	}
}

运行效果：

下班啦下班啦！！！！