无刷电机FOC控制（二）

@Leecx

已于 2024-09-13 22:44:25 修改

阅读量894

点赞数 29

文章标签：单片机嵌入式硬件 c语言 stm32 硬件工程 pcb工艺 mcu

于 2024-09-13 22:00:00 首次发布

本文链接：https://blog.csdn.net/m0_67278762/article/details/142217625

版权

无刷电机FOC控制（一）https://blog.csdn.net/m0_67278762/article/details/142179939

含代码在主页自取

一、前言

上期了解无刷电机BLDC驱动方式以及克拉克逆变换、帕克逆变换在FOC的数学模型等。

其中，克拉克逆变换已通过软件编程完成。帕克逆变换则需要电角度的配合（可以使用软件模拟电角度生成器来实现）。通过这两个模型算法，可以基本实现无刷电机的速度开环控制（开环状态下不支持位置控制）。我们可以通过修改Uq的值来控制速度，但并非实际的弧度每秒（Rad/s）。

若要实现 FOC 闭环控制，则需要一个高精度编码器。该编码器用于输出电机的机械角度，以便进行 PID 控制，同时还可将机械角度转换为电角度，供帕克逆变换使用。

二、硬件介绍

我用的编码器是AS5600磁编码器，AS5600输出精度：12位 DAC 输出分辨率。将径向磁铁，一定看清楚是径向磁铁，将磁铁固定在无刷电机，磁铁与AS5600距离0.3cm左右。

建议去购买灯哥foc套件（无刷电机+编码器+控制器+径向磁铁其他配件），可以少走很多弯路。

第一次接触先把软件跑通，基熟悉FOC控制本的原理，然后再考虑根据自己电机的需求定制控制器和编码器的硬件。

AS5600模块原理图（采用IIC通信）：

控制器模块原理图：

输入范围为 12 - 24V 的电压，通过 BUCK 电路进行降压处理。整个驱动电路由 EG3013 驱动芯片驱动 6 个 NMOS 管，虽然也可以使用其他集成驱动电路，但由于其通过电流较小，无法满足大电机的需求。过零点电路和虚拟中性点在此处无需考虑（它们主要用于无感电机驱动）。还是那句话，先把套件整明白，再来尝试硬件，否则一旦出现 BUG，很难判断是硬件问题还是程序问题，这会让人十分头痛。当然，硬件通常也不是一版就能搞定的，我已经对其进行了 6 个版本的迭代。这里的有感 FOC 控制电路十分稳定，可以放心使用。

实物： FOC控制时一定得将跳线帽按我这样接好

如果是用的成品的控制模块可以跳过硬件部分。

三、软件部分

1、AS5600磁编码器驱动

myiic.c 模拟I2C的驱动


//初始化IIC
void IIC_Init(void)
{					     
	GPIO_InitTypeDef GPIO_InitStructure;
	RCC_APB2PeriphClockCmd(	RCC_APB2Periph_GPIOB, ENABLE );	//使能GPIOB时钟
	   
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP ;   //推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB, &GPIO_InitStructure);
	GPIO_SetBits(GPIOB,GPIO_Pin_6|GPIO_Pin_7); 	//PB6,PB7 输出高
}
//产生IIC起始信号
void IIC_Start(void)
{
	SDA_OUT();     //sda线输出
	IIC_SDA=1;	  	  
	IIC_SCL=1;
	delay_us(4);
 	IIC_SDA=0;//START:when CLK is high,DATA change form high to low 
	delay_us(4);
	IIC_SCL=0;//钳住I2C总线，准备发送或接收数据 
}	  
//产生IIC停止信号
void IIC_Stop(void)
{
	SDA_OUT();//sda线输出
	IIC_SCL=0;
	IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
 	delay_us(4);
	IIC_SCL=1; 
	IIC_SDA=1;//发送I2C总线结束信号
	delay_us(4);							   	
}
//等待应答信号到来
//返回值：1，接收应答失败
//        0，接收应答成功
u8 IIC_Wait_Ack(void)
{
	u8 ucErrTime=0;
	SDA_IN();      //SDA设置为输入  
	IIC_SDA=1;delay_us(1);	   
	IIC_SCL=1;delay_us(1);	 
	while(READ_SDA)
	{
		ucErrTime++;
		if(ucErrTime>250)
		{
			IIC_Stop();
			return 1;
		}
	}
	IIC_SCL=0;//时钟输出0 	   
	return 0;  
} 
//产生ACK应答
void IIC_Ack(void)
{
	IIC_SCL=0;
	SDA_OUT();
	IIC_SDA=0;
	delay_us(2);
	IIC_SCL=1;
	delay_us(2);
	IIC_SCL=0;
}
//不产生ACK应答		    
void IIC_NAck(void)
{
	IIC_SCL=0;
	SDA_OUT();
	IIC_SDA=1;
	delay_us(2);
	IIC_SCL=1;
	delay_us(2);
	IIC_SCL=0;
}					 				     
//IIC发送一个字节
//返回从机有无应答
//1，有应答
//0，无应答			  
void IIC_Send_Byte(u8 txd)
{                        
    u8 t;   
	SDA_OUT(); 	    
    IIC_SCL=0;//拉低时钟开始数据传输
    for(t=0;t<8;t++)
    {              
        IIC_SDA=(txd&0x80)>>7;
        txd<<=1; 	  
		delay_us(2);   //对TEA5767这三个延时都是必须的
		IIC_SCL=1;
		delay_us(2); 
		IIC_SCL=0;	
		delay_us(2);
    }	 
} 	    
//读1个字节，ack=1时，发送ACK，ack=0，发送nACK   
u8 IIC_Read_Byte(unsigned char ack)
{
	unsigned char i,receive=0;
	SDA_IN();//SDA设置为输入
    for(i=0;i<8;i++ )
	{
        IIC_SCL=0; 
        delay_us(2);
		IIC_SCL=1;
        receive<<=1;
        if(READ_SDA)receive++;   
		delay_us(1); 
    }					 
    if (!ack)
        IIC_NAck();//发送nACK
    else
        IIC_Ack(); //发送ACK   
    return receive;
}

as5600.c AS5600磁编码器读当前角度

#define  RAW_Angle_Hi    0x0C
#define  RAW_Angle_Lo    0x0D
#define  AS5600_Address  0x36

float angle_prev=0;
int full_rotations=0;
 

u8 AS5600_ReadOneByte(u8 addr)
{
	u8 temp;		  	    																 
	
	IIC_Start();
	IIC_Send_Byte(AS5600_Address<<1);
	IIC_Wait_Ack();
	IIC_Send_Byte(addr);
	IIC_Wait_Ack();	    
	IIC_Start();  	 	   
	IIC_Send_Byte((AS5600_Address<<1)+1);
	IIC_Wait_Ack();	 
	temp=IIC_Read_Byte(0);		   
	IIC_Stop();
	
	return temp;
}


u16 AS5600_ReadRawAngleTwo(void)
{
	u8 dh,dl;		  	    																 
	
	IIC_Start();
	IIC_Send_Byte(AS5600_Address<<1);
	IIC_Wait_Ack();
	IIC_Send_Byte(RAW_Angle_Hi);
	IIC_Wait_Ack();
	IIC_Start();
	IIC_Send_Byte((AS5600_Address<<1)+1);
	IIC_Wait_Ack();
	dh=IIC_Read_Byte(1);   //1-ack for next byte
	dl=IIC_Read_Byte(0);   //0-end trans
	IIC_Stop();
	
	return ((dh<<8)+dl);
}

绝对角弧度 [0-6.28] -> 0 - 360
float getAngle_without_track(void)
{
	return AS5600_ReadRawAngleTwo() * 0.08789f * PI / 180;
}

///相对角弧度  [-9999 - + 9999]
float getAngle(void)
{
	float val = getAngle_without_track();
	float d_angle = val - angle_prev;
	
	if(fabs(d_angle) > (0.8f * _2PI))
	{
		full_rotations += (d_angle>0)? -1 :1;
	}
	
	angle_prev = val;
	
	return (float)full_rotations * _2PI + angle_prev;
}

2、PWM配置

void TIM3_PWM_Init(u16 arr,u16 psc)
{  
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	TIM_OCInitTypeDef  TIM_OCInitStructure;
	

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);	//使能定时器3时钟
 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB  |RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO, ENABLE);  //使能GPIO外设和AFIO复用功能模块时钟
	   
 
   //设置该引脚为复用输出功能,输出TIM3 CH2的PWM脉冲波形	GPIOB.5
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //TIM_CH2
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;  //复用推挽输出
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIO
	
	
		GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0; //TIM_CH3
	  GPIO_Init(GPIOB, &GPIO_InitStructure);//初始化GPIO
	
 
   //初始化TIM3
	TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值
	TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值 
	TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
	
	//初始化TIM3 Channel2 PWM模式	 
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; //选择定时器模式:TIM脉冲宽度调制模式2
 	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性:TIM输出比较极性高\
	
	TIM_OC1Init(TIM3, &TIM_OCInitStructure);  //根据T指定的参数初始化外设TIM3 OC2
	TIM_OC2Init(TIM3, &TIM_OCInitStructure);  //根据T指定的参数初始化外设TIM3 OC2
	TIM_OC3Init(TIM3, &TIM_OCInitStructure);  //根据T指定的参数初始化外设TIM3 OC2

	TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);  //使能TIM3在CCR2上的预装载寄存器
	TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);  //使能TIM3在CCR2上的预装载寄存器
	TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);  //使能TIM3在CCR2上的预装载寄存器
 
	TIM_Cmd(TIM3, ENABLE);  //使能TIM3
}

3、FOC核心代码

foc.c 包含克拉克逆变换、帕克逆变换、电角度解算

typedef struct
{
	float voltage; ///支持电压
	int dir;       ///编码器方向
	int pp;        ///电机极数
	
}motorInit_t;

float zero_electric_angle=0; 初始化角度
float Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0,dc_a=0,dc_b=0,dc_c=0; ///算法变量
motorInit_t motorDataM0;  ///电机M0参数
extern pidControl_t pidAngleM0;  ///M0电机角度pid结构体

#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))  ///幅值限制

///电机旋转2s 初始角度读取
void foc_init(void)
{
	delay_ms(1000);
	
	zero_electric_angle = _electricalAngle();  ///初始角度读取
}

// 电角度求解  *方向*电机极数
float _electricalAngle(void){
  return  _normalizeAngle((float)(motorDataM0.dir *  motorDataM0.pp) * getAngle_without_track()-zero_electric_angle);
}

// 设置PWM到控制器输出
void setPwm(float Ua, float Ub, float Uc) {

  // 计算占空比
  // 限制占空比从0到1
  dc_a = _constrain(Ua / motorDataM0.voltage, 0.0f , 1.0f );
  dc_b = _constrain(Ub / motorDataM0.voltage, 0.0f , 1.0f );
  dc_c = _constrain(Uc / motorDataM0.voltage, 0.0f , 1.0f );

  //写入PWM到PWM 0 1 2 通道
	TIM_SetCompare1(TIM3,dc_a*300); 
	TIM_SetCompare2(TIM3,dc_b*300); 
	TIM_SetCompare3(TIM3,dc_c*300); 

}

/变换  核心代码
///Uq : 输入速度值
void setTorque(float Uq,float angle_el) {
	
	float Ud=0;
  Uq=_constrain(Uq,-motorDataM0.voltage/2,motorDataM0.voltage/2);
  
  angle_el = _normalizeAngle(angle_el);
  // 帕克逆变换
  Ualpha =  -Uq*sin(angle_el); 
  Ubeta =   Uq*cos(angle_el); 

  // 克拉克逆变换
  Ua = Ualpha + motorDataM0.voltage/2;
  Ub = (sqrt(3)*Ubeta-Ualpha)/2 + motorDataM0.voltage/2;
  Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + motorDataM0.voltage/2;
  setPwm(Ua,Ub,Uc);
}

//获取最终角度值   
float FOC_M0_Angle(void)  
{
//	printf("%f\r\n",motorDataM0.dir*getAngle());
	//as5600传感器读出原始数据 需要乘上实际方向
  return motorDataM0.dir*getAngle();
	
}

pid.c

#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))  ///幅值限制
typedef struct
{
	float P;
	float I;
	float D;
	float output_ramp;
	float limit;
	float error_prev;
	float output_prev;
	float integral_prev;
	volatile unsigned  long timestamp_prev;
	
}pidControl_t;
pidControl_t pidAngleM0;

float PIDController(pidControl_t *pid ,float error)
{
    // 计算两次循环中间的间隔时间
    unsigned long timestamp_now; 
		float proportional;
		float integral;
	  float derivative;
	  float output;
		float output_rate;
		float Ts;
	
		timestamp_now = micros();
     Ts = (timestamp_now - pid->timestamp_prev) * 1e-6f;

	
    if(Ts <= 0.0f || Ts > 0.5f) 
			Ts = 1e-3f;
    

	
    // P环
    proportional = pid->P * error;
    // Tustin 散点积分（I环）
    integral = pid->integral_prev + pid->I*Ts*0.5f*(error + pid->error_prev);
    integral = _constrain(integral, - pid->limit, pid->limit);
    // D环（微分环节）
    derivative = pid->D*(error - pid->error_prev)/Ts;

    // 将P,I,D三环的计算值加起来
    output = proportional + integral + derivative;
    output = _constrain(output, - pid->limit, pid->limit);
		//printf("%f,%f\r\n",error,output);
    if(pid->output_ramp > 0){
        // 对PID的变化速率进行限制
        output_rate = (output - pid->output_prev)/Ts;
		
        if (output_rate > pid->output_ramp)
            output = pid->output_prev + pid->output_ramp * Ts;
        else if (output_rate < -pid->output_ramp)
            output = pid->output_prev - pid->output_ramp * Ts;
    }
    // 保存值（为了下一次循环）
    pid->integral_prev = integral;
    pid->output_prev = output;
    pid->error_prev = error;
    pid->timestamp_prev = timestamp_now;
		
			
		
    return output;
		
		
		
}

void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp)   //M0角度环PID设置
{
  pidAngleM0.P=P;
  pidAngleM0.I=I;
  pidAngleM0.D=D;
  pidAngleM0.output_ramp=ramp;
}
///pid初始设定
void pid_init(void)
{
	pidAngleM0.limit = 100;
}

main.c 仅需通过 setTorque 中的 Uq 便能改变电机的扭力，接着借助以 Target 为期望角度的 PID 位置环来实现位置闭环控制，仅需输入 Target 值，即可使电机旋转至固定位置。其中，_electricalAngle () 表示电角度。

#define  PI    3.14159265358f
#define _2PI   6.28318530718f
#define _3PI_2 4.71238898038f

void DFOC_M0_setAngle(float Target) //角度闭环
{
 setTorque(PIDController(&pidVelM0,(Target-FOC_M0_Angle())*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;      ///方向  注意编码器方向 如果电机异常 可以尝试改电机方向 -1
		
	foc_init(); 
	pid_init();
	
	 
 	while(1)
	{	
		DFOC_M0_SET_ANGLE_PID(0.133f,0.000f,0.00f,0); ///设置PID参数 出现抖动可根据情况修改
		DFOC_M0_setAngle(Target_Angle); //设置目标位置

		if(USART_RX_STA == 1)  //串口接收  例：20  目标为20角度
		{
			
			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);
		}

	}	 

 }