步进电机滤波算法实现加减速
·
为啥要使用这个算法:
AccStep数组 = 楼梯
加速 = 上楼梯
减速 = 下楼梯
S曲线 = 楼梯坡度很缓慢,不会摔倒
缓停:慢慢走下楼梯,不是直接跳下来
S 型曲线加减速算法(Sigmoid 曲线 / S-Curve)
也就是工业控制里最常用、最平稳的电机加减速算法!
核心公式:
deno = 1.0f / (1 + expf(-melo));
他的特点:
起步慢
中间加速快
结束又变慢
全程平缓无冲击
最终得到的AccStep数组:
AccStep[0] → 最大数值 → 最慢
AccStep[1] → 稍小
AccStep[2] → 更小
...
AccStep[mid] → 最小数值 → 最快
...
AccStep[end] → 又变大 → 慢慢结束
这里给出具体代码实现:
//让电机启动时慢慢加速,停止时,慢慢减速,运行平稳不抖动
Motor_t Motor;
//extern volatile uint16_t AccStep[ACC_STEP_NUM];
uint16_t AccStep[ACC_STEP_NUM]={0};
//功能:S加速曲线初始化
//参数1 *pbuff 计算出的定时器的周期
//参数2 fre_max 最大频率 Hz
//参数3 fre_min 最小频率 Hz
//参数4 len 加速需要的脉冲数
void CurveS_init(uint16_t *pbuff,uint32_t fre_max,uint32_t fre_min,int16_t len)
{
int16_t i;
uint16_t flexible =4;
float delt = fre_max-fre_min;
float deno ;
float melo ;
float fre;
for(i=0; i<len; i++)
{
melo = flexible* (i-len/2) / (len/2);
deno = 1.0f / (1 + expf(-melo)); //
fre = delt * deno + fre_min;
pbuff[i] = (unsigned short)(TIM5_CLOCK_FREQ / fre);
printf("pbuff[%d]:%d\r\n,",i,pbuff[i]);
}
}
//速度调节函数
volatile uint8_t RUN_FLAG = 0;
//电机速度状态机,这个函数是电机的大脑,他根据电机的状态,自动改PWM周期->自动该速度
//四种状态:
//1, SPEED_INCREASE 加速
//2, SPEED_STABLE 匀速
//3, SPEED_DECREASE 减速
//4, SPEED_STOP 缓停
void SpeedAdjust(void)
{
switch(Motor.Status)
{
/*加速*/
case SPEED_INCREASE:
if(Motor.ACC_STEP_Count < Motor.ACC_Count_Max)
{
TIM5->ARR = AccStep[Motor.ACC_STEP_Count]; //计算下一个PWM的周期
TIM5->CCR1 = AccStep[Motor.ACC_STEP_Count]/2; //占空比50%
Motor.ACC_STEP_Count++; //加速次数
}
else
{
Motor.Status = SPEED_STABLE;
Motor.ACC_STEP_Count--;
}
break;
/*匀速*/
case SPEED_STABLE:
if(Motor.Pulse_Count >= (Motor.Pulse_Max - Motor.ACC_STEP_Count))
{
Motor.Status = SPEED_DECREASE;
}
break;
/*减速*/
case SPEED_DECREASE:
if(Motor.ACC_STEP_Count >= 0)
{
TIM5->ARR = AccStep[Motor.ACC_STEP_Count]; //计算下一个PWM的周期
TIM5->CCR1 = AccStep[Motor.ACC_STEP_Count]/2;//占空比50%
Motor.ACC_STEP_Count--;
}
break;
/*停止*/
case SPEED_STOP:
if(Motor.ACC_STEP_Count >= 1)
{
TIM5->ARR = AccStep[Motor.ACC_STEP_Count]; //计算下一个PWM的周期
TIM5->CCR1 = AccStep[Motor.ACC_STEP_Count]/2;//占空比50%
Motor.ACC_STEP_Count--;
}
else{TIM_Cmd(TIM5, DISABLE);}
default :
break;
}
}
//PWM--需要输出的脉冲个数
//启动电机
//重置所有计数
//生成S型加速表
//设置初始速度
//打开PWM->电机转动
void Motor_Start(uint32_t pulse_num)
{
Motor.Pulse_Count = 0;
Motor.Pulse_Max = pulse_num;
Motor.ACC_STEP_Count = 0;
Motor.Status = SPEED_INCREASE;
Motor.ACC_Count_Max = ACC_STEP_NUM;
//初始化加速曲线
CurveS_init(AccStep,MAX_SPEED,MIN_SPEED,Motor.ACC_Count_Max);
TIM5->ARR = AccStep[0];
TIM5->CCR1 = AccStep[0]/2;
TIM_Cmd(TIM5, ENABLE);
}
更多推荐

所有评论(0)