为啥要使用这个算法:

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);
}

Logo

免费领 150 小时云算力,进群参与显卡、AI PC 幸运抽奖

更多推荐