野火STM32F407（骄阳）+无刷直流驱动板+永磁同步电机控制学习记录（三）（基于HAL库）

基于ABZ编码器通过FOC+SVPWM控制电机

主要参考知乎大佬文章https://zhuanlan.zhihu.com/p/414721065?utm_medium=social&utm_oi=1192467678965895168

定时器1配置修改成中心对齐模式

定时器5配置成编码器模式，设置上下边沿都计数（一圈发出脉冲1000，上下边沿计数模式一圈计数4000）

编写FOC和SVPWM控制代码

//反PARK变换，由旋转坐标系d、q转到静止坐标系alpha、beta
void Park_Inverse_Transform(void)
{
	
	Motor1.Ualpha = Motor1.Ud* qfcosd(Motor1.Theta)- Motor1.Uq* qfsind(Motor1.Theta);
	Motor1.Ubeta  = Motor1.Ud* qfsind(Motor1.Theta)+ Motor1.Uq* qfcosd(Motor1.Theta);  
}

//SVPWM
void FOC_SVPWM(void)
{
	uint8_t N,A,B,C;
	float	Vref1,Vref2,Vref3,X,Y,Z,temp1,Tfirst,Tsecond,T0,Ta,Tb,Tc,Tcm1,Tcm2,Tcm3;
	
	//计算转子所在的山区
	Vref1=Motor1.Ubeta;
	Vref2=(SQRT_3* Motor1.Ualpha- Motor1.Ubeta)/2;
	Vref3=(-SQRT_3* Motor1.Ualpha- Motor1.Ubeta)/2;
	
	A=Vref1>0 ? 1 :0 ;
	B=Vref2>0 ? 1 :0 ;
	C=Vref3>0 ? 1 :0 ;
	
	N=4*C+2*B+A;

	temp1=SQRT_3* SVPWM_TS/ UDC;
	X=temp1*Vref1;
	Y=-temp1*Vref3;
	Z=-temp1*Vref2;

	//矢量作用时间计算
	switch(N)
	{
		case 1:
			Tfirst=  Z;
			Tsecond= Y;
			Motor1.Sector= 2;
			break;
		case 2:
			Tfirst= Y;
			Tsecond= -X;
			Motor1.Sector= 6;
			break;
		case 3:
			Tfirst= -Z;
			Tsecond= X;
			Motor1.Sector= 1;
			break;
		case 4:
			Tfirst= -X;
			Tsecond= Z;
			Motor1.Sector= 4;
			break;
		case 5:
			Tfirst= X;
			Tsecond= -Y;
			Motor1.Sector= 3;
			break;
		case 6:
			Tfirst= -Y;
			Tsecond= -Z;
			Motor1.Sector= 5;
			break;
		default:
			Tfirst= 0;
			Tsecond= 0;
			Motor1.Sector= 0;
			break;
	}
	
	//超限判断
	if(( Tfirst + Tsecond )> SVPWM_TS)
	{
		Tfirst=(Tfirst/(Tfirst+Tsecond))/ SVPWM_TS;
		Tsecond=(Tsecond/(Tfirst+Tsecond))/ SVPWM_TS;
	}
	
	T0= (SVPWM_TS- Tfirst- Tsecond)/2;
	
	Ta=T0/2;
	Tb=Ta+Tfirst/2;
	Tc=Tb+Tsecond/2;

	//每相桥臂切换时间计算
	switch(N)
	{
		case 1:
			Tcm1=Tb;
			Tcm2=Ta;
			Tcm3=Tc;
			break;
		case 2:
			Tcm1=Ta;
			Tcm2=Tc;
			Tcm3=Tb;
			break;
		case 3:
			Tcm1=Ta;
			Tcm2=Tb;
			Tcm3=Tc;
			break;
		case 4:
			Tcm1=Tc;
			Tcm2=Tb;
			Tcm3=Ta;
			break;
		case 5:
			Tcm1=Tc;
			Tcm2=Ta;
			Tcm3=Tb;
			break;
		case 6:
			Tcm1=Tb;
			Tcm2=Tc;
			Tcm3=Ta;
			break;
		default:
			break;
	}
		
	//设置定时器1的PWM占空比
	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,TIM1_ARR*(1-2*Tcm1/ SVPWM_TS));
	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,TIM1_ARR*(1-2*Tcm2/ SVPWM_TS));
	__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,TIM1_ARR*(1-2*Tcm3/ SVPWM_TS));
}

void Encoder_Control(void)
{	
	//根据编码器计算角度
	Motor1.Theta=Value_Limit(( Get_Encoder_Count() + ENCODER_OFFSET ) * Encoder_coefficient,0.0f,360.0f);
	
	Park_Inverse_Transform();
	
	
	FOC_SVPWM();
}

int main(void)
{
  HAL_Init();
  SystemClock_Config();

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_ADC1_Init();
  MX_TIM1_Init();
  MX_USART1_UART_Init();
  MX_TIM5_Init();
  MX_TIM2_Init();
  /* USER CODE BEGIN 2 */
  
//设置电机初始参数
	Motor1.Ud=0;
	Motor1.Uq=2;
	Encoder_coefficient = 360 * MOTOR1_Pn /(ENCODER_NUMBER * 4);

	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_1);//开启互补pwm
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
	
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_2);//开启互补pwm
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
	
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_3);//开启互补pwm
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);


	HAL_TIM_Encoder_Start(&htim5, TIM_CHANNEL_ALL);	//打开编码器计数
	HAL_TIM_Base_Start_IT(&htim2);

	

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {

if(Key_Scan(KEY1_GPIO_Port,KEY1_Pin)==KEY_ON)
	  {
		  //点亮LED_D11,串口输出电机启动，打开控制端口，设置初始速度（占空比）和初始方向，判断扇区并启动一次
	  	  LED_D11_ON;
		  printf("Motor Running\r\n");
		  HAL_GPIO_WritePin(MOTOR_SD_GPIO_Port,MOTOR_SD_Pin,GPIO_PIN_SET);
		  Motor1.Direction=Motor_Clockwise;
		  Motor1.Speed=Motor_Speed_20;		//初始执行一次霍尔换相
		  
	  }else if(Key_Scan(KEY2_GPIO_Port,KEY2_Pin)==KEY_ON)		//按键2按下，电机停止
	  {
		  //关闭LED_D11,串口输出电机停止，关闭控制端口，设置初始速度（占空比）和初始方向
	  	  LED_D11_OFF;
		  printf("Motor Stop\r\n");
		  HAL_GPIO_WritePin(MOTOR_SD_GPIO_Port,MOTOR_SD_Pin,GPIO_PIN_RESET);
		  Motor1.Direction=Motor_Stop;
		  Motor1.Speed=Motor_Speed_0;
		 
	  }else if(Key_Scan(KEY3_GPIO_Port,KEY3_Pin)==KEY_ON)		//按键3按下，电机加速
	  {
		  if(Motor1.Speed<Motor_Speed_100)
			 Motor1.Speed++;

	  }else if(Key_Scan(KEY4_GPIO_Port,KEY4_Pin)==KEY_ON)		//按键4按下，电机减速
	  {
		  if(Motor1.Speed>Motor_Speed_20)
			  Motor1.Speed--;
	  }else if(Key_Scan(KEY5_GPIO_Port,KEY5_Pin)==KEY_ON)		//按键5按下，电机反向
	  {
		  if(Motor1.Direction==Motor_Clockwise)
			  Motor1.Direction=Motor_AntiClockwise;
		  else if(Motor1.Direction==Motor_AntiClockwise)
			  Motor1.Direction=Motor_Clockwise;
		  printf("Zero=%f,Now=%d\n",Motor1.temp ,Get_Encoder_Count());
	  }
     printf("Zero=%f,Now=%d\n",Motor1.temp ,Get_Encoder_Count());
}
//定时器2更新中断（20k）
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	
	if(htim == &htim2)
	{
		//测试中断所需时间
		HAL_GPIO_WritePin(GPIO_TEST1_GPIO_Port,GPIO_TEST1_Pin,GPIO_PIN_SET);

        Encoder_Control();

		HAL_GPIO_WritePin(GPIO_TEST1_GPIO_Port,GPIO_TEST1_Pin,GPIO_PIN_RESET);
	}

}

//编码器Z信号外部中断
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if(GPIO_Pin==Encoder_Z_Pin)
	{
//		Motor1.temp = __HAL_TIM_GET_COUNTER(&htim5); 
	
		__HAL_TIM_SET_COUNTER(&htim5,0);

	}
}

通过以上代码，将Ud=0,Uq=2,Theta+=1;可以实现FOC开环控制，只是角度是强制给的，而不是通过编码器获取。要获取初始电角度，将Ud=2,Uq=0,Theta=0，开启电机，此时电机处于电角度为0的位置（如果Ud为负数，则处于电角度为180的位置），记录此刻的读数；然后停止控制电机，手动旋转电机，触发编码器的Z信号触发外部中断，记录此刻的读数，两者的差值即为编码器计算角度的补偿值。

Zero表示编码器零位时的计数值，Now表示转子电角度为0时的计数值，Offset=Zero-Now= -584，这就是编码器计算角度时的补偿值。角度计算为(角度制，非弧度制)：

Encoder_coefficient = 360 * 4 /(1000 * 4);
Motor1.Theta= ( Get_Encoder_Count() + ENCODER_OFFSET ) * Encoder_coefficient;

STM32F4用CUBEMX建立工程是默认开启了FPU的，所以浮点数的计算速度还是很快的，但是开根号和三角函数的计算比较慢，所以根号直接计算成小数，三角函数的计算参考https://zhuanlan.zhihu.com/p/585849724

（提高运算速率是为了后面无感算法的实现）

#include "mymath.h"


const float hollyst = 0.017453292519943295769236907684886f;

const float sin_table[] = {
    0.0f,                                    //sin(0)
    0.17364817766693034885171662676931f ,    //sin(10)
    0.34202014332566873304409961468226f ,    //sin(20)
    0.5f ,                                   //sin(30)
    0.64278760968653932632264340990726f ,    //sin(40)
    0.76604444311897803520239265055542f ,    //sin(50)
    0.86602540378443864676372317075294f ,    //sin(60)
    0.93969262078590838405410927732473f ,    //sin(70)
    0.98480775301220805936674302458952f ,    //sin(80)
    1.0f                                     //sin(90)
};

const float cos_table[] = {
    1.0f ,                                   //cos(0)
    0.99984769515639123915701155881391f ,    //cos(1)
    0.99939082701909573000624344004393f,    //cos(2)
    0.99862953475457387378449205843944f ,    //cos(3)
    0.99756405025982424761316268064426f ,    //cos(4)
    0.99619469809174553229501040247389f ,    //cos(5)
    0.99452189536827333692269194498057f ,    //cos(6)
    0.99254615164132203498006158933058f ,    //cos(7)
    0.99026806874157031508377486734485f ,    //cos(8)
    0.98768834059513772619004024769344f      //cos(9)
};

float qfsind(float x)
{
    int sig = 0;

    if(x > 0.0f){
        while(x >= 360.0f) {
            x = x - 360.0f;
        }
    }else{
        while(x < 0.0f) {
            x = x + 360.0f;
        }
    }

    if(x >= 180.0f){
        sig = 1;
        x = x - 180.0f;
    }

    x = (x > 90.0f) ? (180.0f - x) : x;

    int a = x * 0.1f;
    float b = x - 10 * a;
    
    float y = sin_table[a] * cos_table[(int)b] + b * hollyst * sin_table[9 - a];

    return (sig > 0) ? -y : y;
}

float qfcosd(float x)
{
	return qfsind(x+90.0f);
}


//将值限制在Min~Max之间
float Value_Limit(float Value,float Min,float Max)
{
	if(Value > Min)
	{
		while(Value >= Max) 
		{
			Value = Value - Max;
		}
    }else
	{
        while(Value < Min) 
		{
            Value = Value + Max;
        }
    }
	return Value;
}

此时已经实现了有感FOC的开环控制，且整个控制的计算时间不超过5us，这样的话后面无感算法高频方波注入的频率可以提高很多。

个人学习记录，写的比较乱，但是功能实现没有问题，后面考虑实现电流环、速度环、位置环、中高速滑模观测器，低速高频方波注入的无感算法。