stm32f103步进电机S曲线加减速计算

news2024/11/25 3:05:09

S曲线主要实现低速扭力大，更快更稳
https://zhuanlan.zhihu.com/p/396648926?utm_campaign=&utm_medium=social&utm_oi=1361101006265331712&utm_psn=1686906450235133952&utm_source=zhihu
可点击上面链接查看啤酒杯的运动动画

摘自一段知乎上一段关于S曲线方程的解释

首先说一下什么是S型曲线加速，为什么要S型曲线加速。S型曲线加速是指步进电机的启动速度按照S型曲线逐渐增加，以达到设定的最大速度。具体的S型曲线方程如下：
在这里插入图片描述

x取值-5~5的曲线图如下：
在这里插入图片描述

可以看到，刚开始加速和达到最大速度时加速比较缓慢，中间加速比较快。电机的转矩和转速的乘积的k倍等于功率，也就是说，功率一定的时候，转速与转矩成反比关系。所以，转速越低，转矩越大。当电机直接高速启动时，电机可能存在震动、丢步甚至启动不起来的情况。因此需要S型曲线加速，使电机能够缓慢启动。

程序实现控制电机的速度，其实就是控制PWM的输出频率。首先需要对S曲线方程进行一些变化，如下：

Fcurrent = Fmin + (Fmax-Fmin)/(1+exp( -Flexible(i - num )/num) )

Fcurrent为计算出的当前频率。
Fmin为加速的起始频率。
Fmax为加速的最大频率。
-Flexible*(i - num)/num是对S型曲线进行拉伸变化，其中Flexible代表S曲线区间（越大代表压缩的最厉害，中间加速度越大；越小越接近匀加速。理想的S曲线的取值为4-6）。
i是在循环计算过程中的索引，从0开始。
num为加速脉冲数/2 大小。

S曲线加减算法计算

/*加速曲线计算*/
	for (i = 0; i < Motor_S_Type_ARRAY.step_accel; i++)
	{
		// F_current = F_min + (F_max - F_min) / (1 + exp(-flexible*(i-num)/num));
		F_current = (float)(Motor_S_Type_ARRAY.speed_min_frq + 
		(Motor_S_Type_ARRAY.speed_max_frq - Motor_S_Type_ARRAY.speed_min_frq) 
		/ (1 + exp(-FLEXIBLE*(i - (float)(Motor_S_Type_ARRAY.step_accel/2)) 
		/ (Motor_S_Type_ARRAY.step_accel/2))));
		
		speed_per = 1000000 / (F_current*2);//定时器分频后1M的频率，除以当前电平翻转的频率得重装载值。电平翻转的频率为电机频率的2倍
		MotorDataAlgorithm_Struct_Array.accel_array[i] = (uint16_t)speed_per;
	}
	
	/*减速曲线计算*/
    for (i = 0; i < Motor_S_Type_ARRAY.step_decel; i++)
	{
		// F_current = F_max - (F_max - F_min) / (1 + exp(-flexible*(i-num)/num));
		F_current = (float)(Motor_S_Type_ARRAY.speed_max_frq - 
		(Motor_S_Type_ARRAY.speed_max_frq - Motor_S_Type_ARRAY.speed_min_frq) 
		/ (1 + exp(-FLEXIBLE*(i - (float)(Motor_S_Type_ARRAY.step_decel/2))
		/ (Motor_S_Type_ARRAY.step_decel/2))));
		
		speed_per = 1000000 / (F_current*2);//定时器分频后1M的频率，除以当前电平翻转的频率得重装载值。电平翻转的频率为电机频率的2倍
		MotorDataAlgorithm_Struct_Array.decel_array[i] = (uint16_t)speed_per;
	}

程序设定的最小频率是2000，最大脉冲翻转频率根据需要设定，由于S曲线的计算需要占用比较多的时间，需要在电机运转前计算好存放于一个数组，提供给定时器中断服务程序使用。

void motor_prog(void)
{
	static uint8_t i=0,j=0;
	MA_STEP_LEVEL = !MA_STEP_LEVEL;
	Motor_S_Type_ARRAY.step_counter ++;

	if(Motor_S_Type_ARRAY.step_counter < Motor_S_Type_ARRAY.step_num)//还没达到最大步数，即还没运行完
	{
		if(Motor_S_Type_ARRAY.step_counter < Motor_S_Type_ARRAY.step_accel*ACCEL_DECEL_MULTIPLE)//加速过程中
		{
			i++;
			if(i==ACCEL_DECEL_MULTIPLE)
			{
				i=0;
				MotorTimArrUpdate(*MA_accel_data_p++);
			}
		}
		else if(Motor_S_Type_ARRAY.step_counter == Motor_S_Type_ARRAY.step_accel)//刚好加速完成
		{
			MotorTimArrUpdate(Motor_S_Type_ARRAY.step_per);
		}
		else if((Motor_S_Type_ARRAY.step_num - Motor_S_Type_ARRAY.step_counter) //减速过程中
				< Motor_S_Type_ARRAY.step_decel*ACCEL_DECEL_MULTIPLE)
		{
			j++;
			if(j==ACCEL_DECEL_MULTIPLE)
			{
				j=0;
				MotorTimArrUpdate(*MA_decel_data_p++);
			}
		}
        
        MotorStartStopClk(ENABLE);//启动定时器
	}
    
        if(Motor_S_Type_ARRAY.step_counter >= Motor_S_Type_ARRAY.step_num){//已达到最大步数
            
            MotorHalfCurrent(HalfEn);//半流锁开启
            MotorStartStopClk(DISABLE);//关闭定时器
            Motor_S_Type_ARRAY.run_state = STOP;//运行状态为停止
		
            /*电机位置信息更新*/
            if(Motor_S_Type_ARRAY.motor_dir != config_get_ptr()->rst_dir)
            {
                if( Location_Last + Motor_S_Type_ARRAY.step_counter/2 < MOTOR_LIMIT )
                    Location_Last = Location_Last + Motor_S_Type_ARRAY.step_counter/2;
                else
                    Location_Last = MOTOR_LIMIT;
            }
            else
            {
                if( Location_Last > Motor_S_Type_ARRAY.step_counter/2 ) 
                    Location_Last = Location_Last - Motor_S_Type_ARRAY.step_counter/2;
                else
                    Location_Last = 0;
            }
	}
	
}