无刷电机FOC控制（三）速度闭环

一、前言

前面先对 FOC 控制原理进行介绍，并对位置闭环代码展开分析。与位置控制不同，闭环速度控制需实时解算出当前的旋转速度（单位为 Rad/s）值，滤波后将其放入 PID 中进行控制，在此过程中，可能需要对 PID 的参数进行适当调整。

现在依据原位置控制的代码继续进行编写。

无刷电机FOC控制（二）位置闭环https://blog.csdn.net/m0_67278762/article/details/142217625?spm=1001.2014.3001.5501

二、软件代码

1.磁编码器驱动

as5600.c 

float last_s_angle=0;  ///上一次角度

unsigned long last_tim_cnt; ///上一次时间


///当前速度值  = 当前角度-上一次角度 / 时间
float GetSensor_Velocity(void)
{

	float vel;    ///速度值
	float Ts;
	
	unsigned long tim_cnt;///时间值
	float s_angle = getAngle(); 获取整体角度
	
	
	tim_cnt = micros();
	Ts = (tim_cnt - last_tim_cnt) * 1e-6f;
	
	
	if(Ts < 0.0f)
		Ts = 1e-3f;
	
	
	vel = (s_angle - last_s_angle) / Ts;


	last_s_angle  = s_angle;
	last_tim_cnt = tim_cnt;
	
	
	
	return vel;
}

 2.低通滤波代码

lowpass.c 

#define Tf 0.01f

unsigned long last_timestamp;

float last_y=0;

float LowPassFilter(float x) 滤波
{
	
	float alpha;
	float y;
  unsigned long timestamp;
	 float dt;
	
	timestamp = micros();
  dt = (timestamp - last_timestamp)*1e-6f;  ///1ms
	
	
    if (dt < 0.0f) 
				dt = 1e-3f;
    else if(dt > 0.3f) 
		{
        last_y = x;
        last_timestamp = timestamp;
        return x;
    }

     alpha = Tf/(Tf + dt);
     y = alpha*last_y + (1.0f - alpha)*x;
		
		
    last_y = y;
    last_timestamp = timestamp;
    return y;
}

3.主函数 

main.c

// 获取最终速度值 (有滤波) 
float DFOC_M0_Velocity(void)
{
  //获取速度数据并滤波
  float vel_M0_ori = GetSensor_Velocity();
  float vel_M0_flit = LowPassFilter(vel_M0_ori);  

	
	//as5600传感器读出原始数据 需要乘上实际方向
  return motorDataM0.dir*vel_M0_flit;   //考虑方向
}

void DFOC_M0_setVelocity(float Target) //速度闭环
{
  setTorque(PIDController(&pidVelM0,(Target-DFOC_M0_Velocity())*180/PI),_electricalAngle()); 
}

float Target_Angle=12.0f;


 int main(void)   
 {		
	delay_init();	    	 //延时函数初始化	  
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //设置NVIC中断分组2:2位抢占优先级，2位响应优先级
	 
	uart_init(921600);	 //串口初始化为115200
	IIC_Init();
	
 	LED_Init();			     //LED端口初始化
	KEY_Init();          //初始化与按键连接的硬件接口
	 
	 
	TIM3_PWM_Init(300-1,8-1); 30khz    arr      pas
	
	motorDataM0.voltage = 12.0f;  //电压
	motorDataM0.pp = 7;       ///电极
	motorDataM0.dir = 1;      ///方向
		
	foc_init(); 
	pid_init();
	
	 
 	while(1)
	{
		
/*闭环位置*/
#if 0
		
		DFOC_M0_SET_ANGLE_PID(0.133f,0.000f,0.00f,0);
		DFOC_M0_setAngle(Target_Angle);
#endif
		
		
/*闭环速度*/			
#if 1
   DFOC_M0_SET_VEL_PID(0.0001f,0.05f,0.000000f,0);
   DFOC_M0_setVelocity(Target_Angle);
#endif		

		if(USART_RX_STA == 1)
		{
			
			Target_Angle = atof((char *)USART_RX_BUF);
			
				if(Target_Angle > 100)
					Target_Angle = 100;
				else if(Target_Angle < (-100))
					Target_Angle = -100;
				
			
			printf("Target_Angle:%f\r\n",Target_Angle);
			USART_RX_STA = 0;
			memset(USART_RX_BUF,0,200);
		}
	}	 

 }

三、总结

FOC 速度闭环控制通过实时解算出电机当前的旋转速度（单位为 Rad/s）滤波，并将其与期望速度进行比较。利用 PID 控制器根据速度误差调整电机的输入，以实现对电机速度的精确控制。