【STM32-HAL库】一步步搭建出FOC矢量控制（附C代码）说明0、系统配置1、电机有力了！（PWM模块）2、让电机转起来吧！（SVPWM）3、测量电角度！（编码器）4、测量电流吧！（三电阻采样）5、让电机飞！（电流闭环）

101 阅读 0 评论 67 点赞

我是靠谱客的博主勤劳汽车，这篇文章主要介绍【STM32-HAL库】一步步搭建出FOC矢量控制（附C代码）说明0、系统配置1、电机有力了！（PWM模块）2、让电机转起来吧！（SVPWM）3、测量电角度！（编码器）4、测量电流吧！（三电阻采样）5、让电机飞！（电流闭环），现在分享给大家，希望可以做个参考。

说明

本文为无刷电机或PMSM电机驱动的简易代码，旨在分享一些个人调试过程的小心得，提供一个demo文件，程序仍有许多不完善的地方，建立起个人的FOC底层驱动，可以帮助快速熟悉FOC算法原理与使用方法，可以帮助验证新的电机控制算法。原理部分不再阐述。
整个部分共有PWM模块、ADC电流采集、定时器编码器配置、SVPWM模块、FOC核心、PID模块、电压限幅模块，其实有了PWM与SVPWM以及一些必要的数学变换，我们就可以开环使电机转起来了，加入电角度与电流采集作为反馈后，我们就能做到电流闭环，再加入速度PID就可以做到速度闭环，其他的模块只是这些目的的辅助手段罢了。

注意：
调试一定要注意安全！！！
使用带有保护的电源，调试时一定要限制电流在安全等级，开关放手边，随时断电！

硬件相关：
（1）MCU为STM32F405RGT6
（2）引脚分配
PWM：TIM1–PA8、PA9、PA10、PB13、PB14、PB15
电流采样：IA–PA6、IB–PA7、IC–PC4
编码器： EA–PA0、EB–PA1
串口： PB6、PB7
（3）编码器为1250线，电机为PMSM、4対极
软件相关：
STM32CubeMX、Keil

如果自制硬件可参考：迷你FOC驱动器

参考资料：
（1）ST电机库
（2）PMSM的FOC 矢量控制算法调试流程，新手上手流程
（3）PMSM矢量控制算法调试流程
（4）FOC和SVPWM的C语言代码实现
（5）上官致远–深入理解无刷直流电机矢量控制技术–科学出版社

0、系统配置

将下列值加入到Cube的User Constants下，然后按照下面的图配置好基本外设。

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
#define CKTIM 168000000//定时器时钟频率
#define PWM_PRSC 0
#define PWM_FREQ 15000//PWM频率
#define PWM_PERIOD CKTIM/(2*PWM_FREQ*(PWM_PRSC+1))
#define REP_RATE 1 //电流环刷新频率为(REP_RATE+1)/(2*PWM_FREQ)
#define DEADTIME_NS 1000//死区时间ns
#define DEADTIME CKTIM/1000000/2*DEADTIME_NS/1000
#define POLE_PAIR_NUM 4//极对数
#define ENCODER_PPR 1250//编码器线数
#define ALIGNMENT_ANGLE 300
#define COUNTER_RESET (ALIGNMENT_ANGLE*4*ENCODER_PPR/360-1)/POLE_PAIR_NUM
#define ICx_FILTER 8

在这里插入图片描述

1、电机有力了！（PWM模块）

高级定时器主要用于产生6路互补的PWM来驱动MOS管，加入死区防止电源导通，本文未使用刹车引脚。高级定时器1通道1、2、3用于产生PWM，通道4用于触发ADC电流采样，根据扇区的位置，灵活设置PWM占空比，进而选择合理的触发点，避免在噪声点采样。引脚配置与PWM极性请根据自己的硬件合理配置，如IR2101是高电平有效，而IR2103则是低端低有效，高端高有效。
在这里插入图片描述

PWM测试

生成工程后，应首先对PWM模块进行测试，如果有示波器，先测试PWM是否正常（安全起见一路路测试），死区时间是否正确，然后主函数中加入下列代码，导通U相，注意：占空比一定不能设置的过大，防止电流过大，烧毁电机与驱动板。同理可测试其它相。测试完成后进入下一项。
当然，也可以通过这种方法知道你电机的极对数，导通一相后，用手转动电机一圈，感到有几次阻力，就是几对极。或者，不使用驱控板，先用万用表测试电机任意两相间的电阻，然后通合适的电压，如电阻为2欧，则可以通1V电压，然后用手转动电机一圈，感到有几次阻力，就是几对极。

复制代码

1
2
3
4
5
6
7
8
  /* USER CODE BEGIN 2 */
  //此时电机应该是有阻力的
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_2);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,400);//不能设置的过大
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2,5600);//5600为最大占空比
  /* USER CODE END 2 */

不加驱动板时50%占空比波形与1000ns死区
在这里插入图片描述

2、让电机转起来吧！（SVPWM）

在主函数头文件main.h中加入下面定义，这在后面都会用到。

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
typedef uint8_t u8;
typedef uint16_t u16;
typedef uint32_t u32;
typedef int8_t s8;
typedef int16_t s16;
typedef int32_t s32;
typedef __IO uint32_t  vu32;
typedef __IO uint16_t vu16;
typedef __IO uint8_t  vu8;

#define U8_MAX     ((u8)255)
#define S8_MAX     ((s8)127)
#define S8_MIN     ((s8)-128)
#define U16_MAX    ((u16)65535u)
#define S16_MAX    ((s16)32767)
#define S16_MIN    ((s16)-32768)
#define U32_MAX    ((u32)4294967295uL)
#define S32_MAX    ((s32)2147483647)
#define S32_MIN    ((s32)-2147483648)

加入下面代码，主要是数学变换中的Clark变换、Park变换、反Park变换，以及SVPWM模块。

复制代码

//结构体定义
typedef struct 
{
  s16 qI_Component1;
  s16 qI_Component2;
} Curr_Components;

typedef struct 
{
  s16 qV_Component1;
  s16 qV_Component2;
} Volt_Components;

typedef struct     //电压值结构体
{
  s16 hCos;
  s16 hSin;
} Trig_Components;  //存放角度sin和cos函数值的结构体

typedef struct
{
    s16 hKp_Gain;			   //比例系数
    u16 hKp_Divisor;		   //比例系数因子
    s16 hKi_Gain;		       //积分系数
    u16 hKi_Divisor;  	       //积分系数因子
    s16 hLower_Limit_Output;   //总输出下限
    s16 hUpper_Limit_Output;   //总输出上限
    s32 wLower_Limit_Integral; //积分项下限
    s32 wUpper_Limit_Integral; //积分项上限
    s32 wIntegral;			   //积分累积和
    s16 hKd_Gain;			   //微分系数
    u16 hKd_Divisor;		   //微分系数因子
    s32 wPreviousError;	       //上次误差
} PID_Struct_t;

//数学变换部分
#define S16_MAX    ((s16)32767)
#define S16_MIN    ((s16)-32768)
#define divSQRT_3	(s16)0x49E6      //1/sqrt(3)的Q15格式，1/sqrt(3)*2^15=18918=0x49E6 
#define SIN_MASK  0x0300
#define U0_90     0x0200
#define U90_180   0x0300
#define U180_270  0x0000
#define U270_360  0x0100
#define SQRT_3		1.732051
#define T		    (PWM_PERIOD * 4)
#define T_SQRT3     (u16)(T * SQRT_3)
//SVPWM部分
#define SECTOR_1	(u32)1
#define SECTOR_2	(u32)2
#define SECTOR_3	(u32)3
#define SECTOR_4	(u32)4
#define SECTOR_5	(u32)5
#define SECTOR_6	(u32)6
#define PWM2_MODE 0
#define PWM1_MODE 1
#define TW_AFTER ((u16)(((DEADTIME_NS+MAX_TNTR_NS)*168uL)/1000ul))
#define TW_BEFORE (((u16)(((((u16)(SAMPLING_TIME_NS)))*168uL)/1000ul))+1)
#define TNOISE_NS 1550     //2.55usec
#define TRISE_NS 1550     //2.55usec
#define SAMPLING_TIME_NS   700  //700ns
#define SAMPLING_TIME (u16)(((u16)(SAMPLING_TIME_NS) * 168uL)/1000uL) 
#define TNOISE (u16)((((u16)(TNOISE_NS)) * 168uL)/1000uL)
#define TRISE (u16)((((u16)(TRISE_NS)) * 168uL)/1000uL)
#define TDEAD (u16)((DEADTIME_NS * 168uL)/1000uL)

#if (TNOISE_NS > TRISE_NS)
  #define MAX_TNTR_NS TNOISE_NS
#else
  #define MAX_TNTR_NS TRISE_NS
#endif

//函数声明

//数学变换
Curr_Components Clarke(Curr_Components Curr_Input);
Trig_Components Trig_Functions(s16 hAngle);
Curr_Components Park(Curr_Components Curr_Input, s16 Theta);
Volt_Components Rev_Park(Volt_Components Volt_Input);
//SVPWM
void SVPWM_3ShuntCalcDutyCycles (Volt_Components Stat_Volt_Input);
//FOC核心
void FOC_Model(void);
//系统初始化
void motor_init(void);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92

//结构体定义
typedef struct 
{
  s16 qI_Component1;
  s16 qI_Component2;
} Curr_Components;


typedef struct 
{
  s16 qV_Component1;
  s16 qV_Component2;
} Volt_Components;

typedef struct     //电压值结构体
{
  s16 hCos;
  s16 hSin;
} Trig_Components;  //存放角度sin和cos函数值的结构体

typedef struct
{
    s16 hKp_Gain;			   //比例系数
    u16 hKp_Divisor;		   //比例系数因子
    s16 hKi_Gain;		       //积分系数
    u16 hKi_Divisor;  	       //积分系数因子
    s16 hLower_Limit_Output;   //总输出下限
    s16 hUpper_Limit_Output;   //总输出上限
    s32 wLower_Limit_Integral; //积分项下限
    s32 wUpper_Limit_Integral; //积分项上限
    s32 wIntegral;			   //积分累积和
    s16 hKd_Gain;			   //微分系数
    u16 hKd_Divisor;		   //微分系数因子
    s32 wPreviousError;	       //上次误差
} PID_Struct_t;
 




//数学变换部分
#define S16_MAX    ((s16)32767)
#define S16_MIN    ((s16)-32768)
#define divSQRT_3	(s16)0x49E6      //1/sqrt(3)的Q15格式，1/sqrt(3)*2^15=18918=0x49E6 
#define SIN_MASK  0x0300
#define U0_90     0x0200
#define U90_180   0x0300
#define U180_270  0x0000
#define U270_360  0x0100
#define SQRT_3		1.732051
#define T		    (PWM_PERIOD * 4)
#define T_SQRT3     (u16)(T * SQRT_3)
//SVPWM部分
#define SECTOR_1	(u32)1
#define SECTOR_2	(u32)2
#define SECTOR_3	(u32)3
#define SECTOR_4	(u32)4
#define SECTOR_5	(u32)5
#define SECTOR_6	(u32)6
#define PWM2_MODE 0
#define PWM1_MODE 1
#define TW_AFTER ((u16)(((DEADTIME_NS+MAX_TNTR_NS)*168uL)/1000ul))
#define TW_BEFORE (((u16)(((((u16)(SAMPLING_TIME_NS)))*168uL)/1000ul))+1)
#define TNOISE_NS 1550     //2.55usec
#define TRISE_NS 1550     //2.55usec
#define SAMPLING_TIME_NS   700  //700ns
#define SAMPLING_TIME (u16)(((u16)(SAMPLING_TIME_NS) * 168uL)/1000uL) 
#define TNOISE (u16)((((u16)(TNOISE_NS)) * 168uL)/1000uL)
#define TRISE (u16)((((u16)(TRISE_NS)) * 168uL)/1000uL)
#define TDEAD (u16)((DEADTIME_NS * 168uL)/1000uL)

#if (TNOISE_NS > TRISE_NS)
  #define MAX_TNTR_NS TNOISE_NS
#else
  #define MAX_TNTR_NS TRISE_NS
#endif

//函数声明

//数学变换
Curr_Components Clarke(Curr_Components Curr_Input);
Trig_Components Trig_Functions(s16 hAngle);
Curr_Components Park(Curr_Components Curr_Input, s16 Theta);
Volt_Components Rev_Park(Volt_Components Volt_Input);
//SVPWM
void SVPWM_3ShuntCalcDutyCycles (Volt_Components Stat_Volt_Input);
//FOC核心
void FOC_Model(void);
//系统初始化
void motor_init(void);

复制代码

//变量定义部分
Trig_Components Vector_Components;
u8 bSector;
u8 PWM4Direction=PWM2_MODE;
s16 cnt = S16_MIN;//开环调试变量

//FOC相关
Trig_Components Vector_Components;
Curr_Components Stat_Curr_a_b;            
Curr_Components Stat_Curr_alfa_beta;       
Curr_Components Stat_Curr_q_d;             
Curr_Components Stat_Curr_q_d_ref_ref;   //电流环的给定值，用于电流环Id,Iq和前馈电流控制的给定值
Volt_Components Stat_Volt_q_d;             
Volt_Components Stat_Volt_alfa_beta; 
PID_Struct_t PID_Torque_InitStructure;
PID_Struct_t PID_Flux_InitStructure;
PID_Struct_t PID_Speed_InitStructure;

void motor_init(void)
{
	//PWM初始化
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_2);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_3);
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2,0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3,0);
  //通道4触发ADC采样
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4,1000);//初始占空比应该多少？
//	//开启ADC注入转换
//	HAL_ADCEx_InjectedStart_IT(&hadc1);
//	//使能ABZ编码器
//	HAL_TIM_Encoder_Start_IT(&htim2,TIM_CHANNEL_ALL);
//  HAL_TIM_Base_Start_IT(&htim2);
//	//初始化PID控制器
//	PID_Init(&PID_Torque_InitStructure,&PID_Flux_InitStructure,&PID_Speed_InitStructure);
  
//	State = START;
}

void FOC_Model(void)	  //电流环处理函数
{
//	Stat_Curr_a_b = SVPWM_3ShuntGetPhaseCurrentValues();			//读取2相的电流值
//	Stat_Curr_alfa_beta = Clarke(Stat_Curr_a_b);	//Ia,Ib通过Clark变换得到Ialpha和Ibeta  
//	Stat_Curr_q_d = Park( Stat_Curr_alfa_beta,ENC_Get_Electrical_Angle());  //输入电角度、Ialpha和Ibeta，经过Park变换得到Iq、Id							
//	Stat_Volt_q_d.qV_Component1 = PID_Regulator(hTorque_Reference,Stat_Curr_q_d.qI_Component1, &PID_Torque_InitStructure);
//	Stat_Volt_q_d.qV_Component2 = PID_Regulator(hFlux_Reference,Stat_Curr_q_d.qI_Component2, &PID_Flux_InitStructure);  	
//	RevPark_Circle_Limitation();  	//归一化
	
	
	//开环调试
	Stat_Volt_q_d.qV_Component1 = 0;
	Stat_Volt_q_d.qV_Component2 = 3000;
	cnt+=500;
	if(cnt>S16_MAX)
		cnt=S16_MIN;
	Vector_Components = Trig_Functions(cnt);
	Stat_Volt_alfa_beta = Rev_Park(Stat_Volt_q_d);    //反Park变换
	SVPWM_3ShuntCalcDutyCycles(Stat_Volt_alfa_beta);  //svpwm实现函数，实际的电流输出控制	
}

//SVPWM
void SVPWM_3ShuntCalcDutyCycles (Volt_Components Stat_Volt_Input)
{
   s32 wX, wY, wZ, wUAlpha, wUBeta;
   u16  hTimePhA=0, hTimePhB=0, hTimePhC=0, hTimePhD=0;
   u16  hDeltaDuty;
    
   wUAlpha = Stat_Volt_Input.qV_Component1 * T_SQRT3 ;
   wUBeta = -(Stat_Volt_Input.qV_Component2 * T);

wX = wUBeta;
   wY = (wUBeta + wUAlpha)/2;
   wZ = (wUBeta - wUAlpha)/2;
   
  // Sector calculation from wX, wY, wZ
   if (wY<0)
   {
      if (wZ<0)
      {
        bSector = SECTOR_5;
      }
      else // wZ >= 0
        if (wX<=0)
        {
          bSector = SECTOR_4;
        }
        else // wX > 0
        {
          bSector = SECTOR_3;
        }
   }
   else // wY > 0
   {
     if (wZ>=0)
     {
       bSector = SECTOR_2;
     }
     else // wZ < 0
       if (wX<=0)
       {  
         bSector = SECTOR_6;
       }
       else // wX > 0
       {
         bSector = SECTOR_1;
       }
    }
   
   /* Duty cycles computation */
  PWM4Direction=PWM2_MODE;
    
  switch(bSector)
  {  
    case SECTOR_1:
                hTimePhA = (T/8) + ((((T + wX) - wZ)/2)/131072);
				hTimePhB = hTimePhA + wZ/131072;
				hTimePhC = hTimePhB - wX/131072;
                
                // ADC Syncronization setting value             
                if ((u16)(PWM_PERIOD-hTimePhA) > TW_AFTER)
                {
                  hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (u16)(hTimePhA - hTimePhB);
                  
				  // Definition of crossing point
                  if (hDeltaDuty > (u16)(PWM_PERIOD-hTimePhA)*2) 
                  {
                      hTimePhD = hTimePhA - TW_BEFORE; // Ts before Phase A 
                  }
                  else
                  {
                      hTimePhD = hTimePhA + TW_AFTER; // DT + Tn after Phase A
                     
                    if (hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=PWM1_MODE;
                      
                      hTimePhD = (2 * PWM_PERIOD) - hTimePhD-1;
                    }
                  }
                }
                            
                break;
    case SECTOR_2:
                hTimePhA = (T/8) + ((((T + wY) - wZ)/2)/131072);
				hTimePhB = hTimePhA + wZ/131072;
				hTimePhC = hTimePhA - wY/131072;
                
                // ADC Syncronization setting value
                if ((u16)(PWM_PERIOD-hTimePhB) > TW_AFTER)
                {
                  hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (u16)(hTimePhB - hTimePhA);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > (u16)(PWM_PERIOD-hTimePhB)*2) 
                  {
                    hTimePhD = hTimePhB - TW_BEFORE; // Ts before Phase B 
                  }
                  else
                  {
                    hTimePhD = hTimePhB + TW_AFTER; // DT + Tn after Phase B
                    
                    if (hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=PWM1_MODE;
                      
                      hTimePhD = (2 * PWM_PERIOD) - hTimePhD-1;
                    }
                  }
                }
                
                break;

case SECTOR_3:
                hTimePhA = (T/8) + ((((T - wX) + wY)/2)/131072);
				        hTimePhC = hTimePhA - wY/131072;
                hTimePhB = hTimePhC + wX/131072;
		
                // ADC Syncronization setting value
                if ((u16)(PWM_PERIOD-hTimePhB) > TW_AFTER)
                {
                  hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (u16)(hTimePhB - hTimePhC);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > (u16)(PWM_PERIOD-hTimePhB)*2) 
                  {
                    hTimePhD = hTimePhB - TW_BEFORE; // Ts before Phase B 
                  }
                  else
                  {
                    hTimePhD = hTimePhB + TW_AFTER; // DT + Tn after Phase B
                    
                    if (hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=PWM1_MODE;
                      
                      hTimePhD = (2 * PWM_PERIOD) - hTimePhD-1;
                    }
                  }
                }
                break;
    
    case SECTOR_4:
                hTimePhA = (T/8) + ((((T + wX) - wZ)/2)/131072);
                hTimePhB = hTimePhA + wZ/131072;
                hTimePhC = hTimePhB - wX/131072;
                
                // ADC Syncronization setting value
                if ((u16)(PWM_PERIOD-hTimePhC) > TW_AFTER)
                {
                  hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (u16)(hTimePhC - hTimePhB);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > (u16)(PWM_PERIOD-hTimePhC)*2)
                  {
                    hTimePhD = hTimePhC - TW_BEFORE; // Ts before Phase C 
                  }
                  else
                  {
                    hTimePhD = hTimePhC + TW_AFTER; // DT + Tn after Phase C
                    
                    if (hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=PWM1_MODE;
                      
                      hTimePhD = (2 * PWM_PERIOD) - hTimePhD-1;
                    }
                  }
                }
                break;  
    
    case SECTOR_5:
                hTimePhA = (T/8) + ((((T + wY) - wZ)/2)/131072);
				hTimePhB = hTimePhA + wZ/131072;
				hTimePhC = hTimePhA - wY/131072;
                
                // ADC Syncronization setting value
                if ((u16)(PWM_PERIOD-hTimePhC) > TW_AFTER)
                {
                  hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (u16)(hTimePhC - hTimePhA);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > (u16)(PWM_PERIOD-hTimePhC)*2) 
                  {
                    hTimePhD = hTimePhC - TW_BEFORE; // Ts before Phase C 
                  }
                  else
                  {
                    hTimePhD = hTimePhC + TW_AFTER; // DT + Tn after Phase C
                    
                    if (hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=PWM1_MODE;
                      
                      hTimePhD = (2 * PWM_PERIOD) - hTimePhD-1;
                    }
                  }
                }

break;
                
    case SECTOR_6:
                hTimePhA = (T/8) + ((((T - wX) + wY)/2)/131072);
				hTimePhC = hTimePhA - wY/131072;
				hTimePhB = hTimePhC + wX/131072;
                
                // ADC Syncronization setting value
                if ((u16)(PWM_PERIOD-hTimePhA) > TW_AFTER)
                {
                  hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (u16)(hTimePhA - hTimePhC);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > (u16)(PWM_PERIOD-hTimePhA)*2) 
                  {
                    hTimePhD = hTimePhA - TW_BEFORE; // Ts before Phase A 
                  }
                  else
                  {
                    hTimePhD = hTimePhA + TW_AFTER; // DT + Tn after Phase A
                    
                    if (hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=PWM1_MODE;
                      
                      hTimePhD = (2 * PWM_PERIOD) - hTimePhD-1;
                    }
                  }
                }

break;
    default:
		break;
   }
  
  if (PWM4Direction == PWM2_MODE)
  {
    //Set Polarity of CC4 High
    TIM1->CCER &= 0xDFFF;    
  }
  else
  {
    //Set Polarity of CC4 Low
    TIM1->CCER |= 0x2000;
  }
  
  /* Load compare registers values */ 
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,hTimePhA);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2,hTimePhB);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3,hTimePhC);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4,hTimePhD);
}

//数学函数
const s16 hSin_Cos_Table[256] = {
0x0000,0x00C9,0x0192,0x025B,0x0324,0x03ED,0x04B6,0x057F,
0x0648,0x0711,0x07D9,0x08A2,0x096A,0x0A33,0x0AFB,0x0BC4,
0x0C8C,0x0D54,0x0E1C,0x0EE3,0x0FAB,0x1072,0x113A,0x1201,
0x12C8,0x138F,0x1455,0x151C,0x15E2,0x16A8,0x176E,0x1833,
0x18F9,0x19BE,0x1A82,0x1B47,0x1C0B,0x1CCF,0x1D93,0x1E57,
0x1F1A,0x1FDD,0x209F,0x2161,0x2223,0x22E5,0x23A6,0x2467,
0x2528,0x25E8,0x26A8,0x2767,0x2826,0x28E5,0x29A3,0x2A61,
0x2B1F,0x2BDC,0x2C99,0x2D55,0x2E11,0x2ECC,0x2F87,0x3041,
0x30FB,0x31B5,0x326E,0x3326,0x33DF,0x3496,0x354D,0x3604,
0x36BA,0x376F,0x3824,0x38D9,0x398C,0x3A40,0x3AF2,0x3BA5,
0x3C56,0x3D07,0x3DB8,0x3E68,0x3F17,0x3FC5,0x4073,0x4121,
0x41CE,0x427A,0x4325,0x43D0,0x447A,0x4524,0x45CD,0x4675,
0x471C,0x47C3,0x4869,0x490F,0x49B4,0x4A58,0x4AFB,0x4B9D,
0x4C3F,0x4CE0,0x4D81,0x4E20,0x4EBF,0x4F5D,0x4FFB,0x5097,
0x5133,0x51CE,0x5268,0x5302,0x539B,0x5432,0x54C9,0x5560,
0x55F5,0x568A,0x571D,0x57B0,0x5842,0x58D3,0x5964,0x59F3,
0x5A82,0x5B0F,0x5B9C,0x5C28,0x5CB3,0x5D3E,0x5DC7,0x5E4F,
0x5ED7,0x5F5D,0x5FE3,0x6068,0x60EB,0x616E,0x61F0,0x6271,
0x62F1,0x6370,0x63EE,0x646C,0x64E8,0x6563,0x65DD,0x6656,
0x66CF,0x6746,0x67BC,0x6832,0x68A6,0x6919,0x698B,0x69FD,
0x6A6D,0x6ADC,0x6B4A,0x6BB7,0x6C23,0x6C8E,0x6CF8,0x6D61,
0x6DC9,0x6E30,0x6E96,0x6EFB,0x6F5E,0x6FC1,0x7022,0x7083,
0x70E2,0x7140,0x719D,0x71F9,0x7254,0x72AE,0x7307,0x735E,
0x73B5,0x740A,0x745F,0x74B2,0x7504,0x7555,0x75A5,0x75F3,
0x7641,0x768D,0x76D8,0x7722,0x776B,0x77B3,0x77FA,0x783F,
0x7884,0x78C7,0x7909,0x794A,0x7989,0x79C8,0x7A05,0x7A41,
0x7A7C,0x7AB6,0x7AEE,0x7B26,0x7B5C,0x7B91,0x7BC5,0x7BF8,
0x7C29,0x7C59,0x7C88,0x7CB6,0x7CE3,0x7D0E,0x7D39,0x7D62,
0x7D89,0x7DB0,0x7DD5,0x7DFA,0x7E1D,0x7E3E,0x7E5F,0x7E7E,
0x7E9C,0x7EB9,0x7ED5,0x7EEF,0x7F09,0x7F21,0x7F37,0x7F4D,
0x7F61,0x7F74,0x7F86,0x7F97,0x7FA6,0x7FB4,0x7FC1,0x7FCD,
0x7FD8,0x7FE1,0x7FE9,0x7FF0,0x7FF5,0x7FF9,0x7FFD,0x7FFE};

Curr_Components Clarke(Curr_Components Curr_Input)         
{
  Curr_Components Curr_Output; 
  s32 qIa_divSQRT3_tmp;
  s32 qIb_divSQRT3_tmp;    //定义32位有符号数，用来暂存Q30格式  
  s16 qIa_divSQRT3;
  s16 qIb_divSQRT3 ;
 
  Curr_Output.qI_Component1 = Curr_Input.qI_Component1;     //Ialpha = Ia
 
  qIa_divSQRT3_tmp = divSQRT_3 * Curr_Input.qI_Component1;  //计算Ia/√3
  qIa_divSQRT3_tmp /=32768;                                 //两个Q15数相乘，会变成Q30，因此要右移15位，变回Q15	    
  qIb_divSQRT3_tmp = divSQRT_3 * Curr_Input.qI_Component2;  //计算Ib/√3
  qIb_divSQRT3_tmp /=32768;
  
  qIa_divSQRT3=((s16)(qIa_divSQRT3_tmp));	                //s32赋值给s16	 		
  qIb_divSQRT3=((s16)(qIb_divSQRT3_tmp));				
     
  Curr_Output.qI_Component2=(-(qIa_divSQRT3)-(qIb_divSQRT3)-(qIb_divSQRT3));  //Ibeta = -(2*Ib+Ia)/sqrt(3) 
  return(Curr_Output); 
}
 
	
/*******************************************************************************
* Function Name  : Trig_Functions 
* Description    : 本函数返回输入角度的cos和sin函数值
* Input          : angle in s16 format
* Output         : Cosine and Sine in s16 format
*******************************************************************************/
Trig_Components Trig_Functions(s16 hAngle)  //hAngle=0,转子电角度=0度。hAngle=S16_MAX,转子电角度=180度。hAngle=S16_MIN,转子电角度=-180度
{
  u16 hindex;
  Trig_Components Local_Components;
  
  /* 10 bit index computation  */  
  hindex = (u16)(hAngle + 32768);  
  hindex /= 64;      
  
  switch (hindex & SIN_MASK) 
  {
  case U0_90:
    Local_Components.hSin = hSin_Cos_Table[(u8)(hindex)];
    Local_Components.hCos = hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
    break;
  
  case U90_180:  
     Local_Components.hSin = hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
     Local_Components.hCos = -hSin_Cos_Table[(u8)(hindex)];
    break;
  
  case U180_270:
     Local_Components.hSin = -hSin_Cos_Table[(u8)(hindex)];
     Local_Components.hCos = -hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
    break;
  
  case U270_360:
     Local_Components.hSin =  -hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
     Local_Components.hCos =  hSin_Cos_Table[(u8)(hindex)]; 
    break;
  default:
    break;
  }
  return (Local_Components);
}

/**********************************************************************************************************
Park变换，输入电角度、Ialpha和Ibeta，经过Park变换得到Iq、Id
**********************************************************************************************************/ 
Curr_Components Park(Curr_Components Curr_Input, s16 Theta)       
{ 
  Curr_Components Curr_Output;
  s32 qId_tmp_1, qId_tmp_2;
  s32 qIq_tmp_1, qIq_tmp_2;     
  s16 qId_1, qId_2;  
  s16 qIq_1, qIq_2;  
  
  Vector_Components = Trig_Functions(Theta);                      //计算电角度的cos和sin
  
  qIq_tmp_1 = Curr_Input.qI_Component1 * Vector_Components.hCos;  //计算Ialpha*cosθ	
  qIq_tmp_1 /= 32768;
  qIq_tmp_2 = Curr_Input.qI_Component2 *Vector_Components.hSin;   //计算Ibeta*sinθ
  qIq_tmp_2 /= 32768;
 
  qIq_1 = ((s16)(qIq_tmp_1));
  qIq_2 = ((s16)(qIq_tmp_2));
  Curr_Output.qI_Component1 = ((qIq_1)-(qIq_2));	//Iq=Ialpha*cosθ- Ibeta*sinθ
  
  qId_tmp_1 = Curr_Input.qI_Component1 * Vector_Components.hSin;  //计算Ialpha*sinθ
  qId_tmp_1 /= 32768;
  qId_tmp_2 = Curr_Input.qI_Component2 * Vector_Components.hCos;  //计算Ibeta*cosθ
  qId_tmp_2 /= 32768;
  
  qId_1 = (s16)(qId_tmp_1);		 
  qId_2 = (s16)(qId_tmp_2);					
  Curr_Output.qI_Component2 = ((qId_1)+(qId_2));   //Id=Ialpha*sinθ+ Ibeta*cosθ
  
  return (Curr_Output);
}
 
/**********************************************************************************************************
反park变换，输入Uq、Ud得到Ualpha、Ubeta
**********************************************************************************************************/ 
Volt_Components Rev_Park(Volt_Components Volt_Input)
{ 	
  s32 qValpha_tmp1,qValpha_tmp2,qVbeta_tmp1,qVbeta_tmp2;
  s16 qValpha_1,qValpha_2,qVbeta_1,qVbeta_2;
  Volt_Components Volt_Output;
   
  qValpha_tmp1 = Volt_Input.qV_Component1 * Vector_Components.hCos;  //Uq*cosθ
  qValpha_tmp1 /= 32768; 
  qValpha_tmp2 = Volt_Input.qV_Component2 * Vector_Components.hSin;  //Ud*sinθ
  qValpha_tmp2 /= 32768;
		
  qValpha_1 = (s16)(qValpha_tmp1);		
  qValpha_2 = (s16)(qValpha_tmp2);			
  Volt_Output.qV_Component1 = ((qValpha_1)+(qValpha_2));             //Ualpha=Uq*cosθ+ Ud*sinθ
  
  qVbeta_tmp1 = Volt_Input.qV_Component1 * Vector_Components.hSin;   //Uq*sinθ
  qVbeta_tmp1 /= 32768;  
  qVbeta_tmp2 = Volt_Input.qV_Component2 * Vector_Components.hCos;   //Ud*cosθ
  qVbeta_tmp2 /= 32768;
 
  qVbeta_1 = (s16)(qVbeta_tmp1);				
  qVbeta_2 = (s16)(qVbeta_tmp2);  				
  Volt_Output.qV_Component2 = -(qVbeta_1)+(qVbeta_2);                //Ubeta=Ud*cosθ- Uq*sinθ
 
  return(Volt_Output);
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536

//变量定义部分
Trig_Components Vector_Components;
u8 bSector;
u8 PWM4Direction=PWM2_MODE;
s16 cnt = S16_MIN;//开环调试变量

//FOC相关
Trig_Components Vector_Components;
Curr_Components Stat_Curr_a_b;            
Curr_Components Stat_Curr_alfa_beta;       
Curr_Components Stat_Curr_q_d;             
Curr_Components Stat_Curr_q_d_ref_ref;   //电流环的给定值，用于电流环Id,Iq和前馈电流控制的给定值
Volt_Components Stat_Volt_q_d;             
Volt_Components Stat_Volt_alfa_beta; 
PID_Struct_t PID_Torque_InitStructure;
PID_Struct_t PID_Flux_InitStructure;
PID_Struct_t PID_Speed_InitStructure;

void motor_init(void)
{
	//PWM初始化
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_2);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_3);
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2,0);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3,0);
  //通道4触发ADC采样
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4,1000);//初始占空比应该多少？
//	//开启ADC注入转换
//	HAL_ADCEx_InjectedStart_IT(&hadc1);
//	//使能ABZ编码器
//	HAL_TIM_Encoder_Start_IT(&htim2,TIM_CHANNEL_ALL);
//  HAL_TIM_Base_Start_IT(&htim2);
//	//初始化PID控制器
//	PID_Init(&PID_Torque_InitStructure,&PID_Flux_InitStructure,&PID_Speed_InitStructure);
  
//	State = START;
}

void FOC_Model(void)	  //电流环处理函数
{
//	Stat_Curr_a_b = SVPWM_3ShuntGetPhaseCurrentValues();			//读取2相的电流值
//	Stat_Curr_alfa_beta = Clarke(Stat_Curr_a_b);	//Ia,Ib通过Clark变换得到Ialpha和Ibeta  
//	Stat_Curr_q_d = Park( Stat_Curr_alfa_beta,ENC_Get_Electrical_Angle());  //输入电角度、Ialpha和Ibeta，经过Park变换得到Iq、Id							
//	Stat_Volt_q_d.qV_Component1 = PID_Regulator(hTorque_Reference,Stat_Curr_q_d.qI_Component1, &PID_Torque_InitStructure);
//	Stat_Volt_q_d.qV_Component2 = PID_Regulator(hFlux_Reference,Stat_Curr_q_d.qI_Component2, &PID_Flux_InitStructure);  	
//	RevPark_Circle_Limitation();  	//归一化
	
	
	//开环调试
	Stat_Volt_q_d.qV_Component1 = 0;
	Stat_Volt_q_d.qV_Component2 = 3000;
	cnt+=500;
	if(cnt>S16_MAX)
		cnt=S16_MIN;
	Vector_Components = Trig_Functions(cnt);
	Stat_Volt_alfa_beta = Rev_Park(Stat_Volt_q_d);    //反Park变换
	SVPWM_3ShuntCalcDutyCycles(Stat_Volt_alfa_beta);  //svpwm实现函数，实际的电流输出控制	
}




//SVPWM
void SVPWM_3ShuntCalcDutyCycles (Volt_Components Stat_Volt_Input)
{
   s32 wX, wY, wZ, wUAlpha, wUBeta;
   u16  hTimePhA=0, hTimePhB=0, hTimePhC=0, hTimePhD=0;
   u16  hDeltaDuty;
    
   wUAlpha = Stat_Volt_Input.qV_Component1 * T_SQRT3 ;
   wUBeta = -(Stat_Volt_Input.qV_Component2 * T);

   wX = wUBeta;
   wY = (wUBeta + wUAlpha)/2;
   wZ = (wUBeta - wUAlpha)/2;
   
  // Sector calculation from wX, wY, wZ
   if (wY<0)
   {
      if (wZ<0)
      {
        bSector = SECTOR_5;
      }
      else // wZ >= 0
        if (wX<=0)
        {
          bSector = SECTOR_4;
        }
        else // wX > 0
        {
          bSector = SECTOR_3;
        }
   }
   else // wY > 0
   {
     if (wZ>=0)
     {
       bSector = SECTOR_2;
     }
     else // wZ < 0
       if (wX<=0)
       {  
         bSector = SECTOR_6;
       }
       else // wX > 0
       {
         bSector = SECTOR_1;
       }
    }
   
   /* Duty cycles computation */
  PWM4Direction=PWM2_MODE;
    
  switch(bSector)
  {  
    case SECTOR_1:
                hTimePhA = (T/8) + ((((T + wX) - wZ)/2)/131072);
				hTimePhB = hTimePhA + wZ/131072;
				hTimePhC = hTimePhB - wX/131072;
                
                // ADC Syncronization setting value             
                if ((u16)(PWM_PERIOD-hTimePhA) > TW_AFTER)
                {
                  hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (u16)(hTimePhA - hTimePhB);
                  
				  // Definition of crossing point
                  if (hDeltaDuty > (u16)(PWM_PERIOD-hTimePhA)*2) 
                  {
                      hTimePhD = hTimePhA - TW_BEFORE; // Ts before Phase A 
                  }
                  else
                  {
                      hTimePhD = hTimePhA + TW_AFTER; // DT + Tn after Phase A
                     
                    if (hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=PWM1_MODE;
                      
                      hTimePhD = (2 * PWM_PERIOD) - hTimePhD-1;
                    }
                  }
                }
                            
                break;
    case SECTOR_2:
                hTimePhA = (T/8) + ((((T + wY) - wZ)/2)/131072);
				hTimePhB = hTimePhA + wZ/131072;
				hTimePhC = hTimePhA - wY/131072;
                
                // ADC Syncronization setting value
                if ((u16)(PWM_PERIOD-hTimePhB) > TW_AFTER)
                {
                  hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (u16)(hTimePhB - hTimePhA);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > (u16)(PWM_PERIOD-hTimePhB)*2) 
                  {
                    hTimePhD = hTimePhB - TW_BEFORE; // Ts before Phase B 
                  }
                  else
                  {
                    hTimePhD = hTimePhB + TW_AFTER; // DT + Tn after Phase B
                    
                    if (hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=PWM1_MODE;
                      
                      hTimePhD = (2 * PWM_PERIOD) - hTimePhD-1;
                    }
                  }
                }
                
                break;

    case SECTOR_3:
                hTimePhA = (T/8) + ((((T - wX) + wY)/2)/131072);
				        hTimePhC = hTimePhA - wY/131072;
                hTimePhB = hTimePhC + wX/131072;
		
                // ADC Syncronization setting value
                if ((u16)(PWM_PERIOD-hTimePhB) > TW_AFTER)
                {
                  hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (u16)(hTimePhB - hTimePhC);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > (u16)(PWM_PERIOD-hTimePhB)*2) 
                  {
                    hTimePhD = hTimePhB - TW_BEFORE; // Ts before Phase B 
                  }
                  else
                  {
                    hTimePhD = hTimePhB + TW_AFTER; // DT + Tn after Phase B
                    
                    if (hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=PWM1_MODE;
                      
                      hTimePhD = (2 * PWM_PERIOD) - hTimePhD-1;
                    }
                  }
                }
                break;
    
    case SECTOR_4:
                hTimePhA = (T/8) + ((((T + wX) - wZ)/2)/131072);
                hTimePhB = hTimePhA + wZ/131072;
                hTimePhC = hTimePhB - wX/131072;
                
                // ADC Syncronization setting value
                if ((u16)(PWM_PERIOD-hTimePhC) > TW_AFTER)
                {
                  hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (u16)(hTimePhC - hTimePhB);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > (u16)(PWM_PERIOD-hTimePhC)*2)
                  {
                    hTimePhD = hTimePhC - TW_BEFORE; // Ts before Phase C 
                  }
                  else
                  {
                    hTimePhD = hTimePhC + TW_AFTER; // DT + Tn after Phase C
                    
                    if (hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=PWM1_MODE;
                      
                      hTimePhD = (2 * PWM_PERIOD) - hTimePhD-1;
                    }
                  }
                }
                break;  
    
    case SECTOR_5:
                hTimePhA = (T/8) + ((((T + wY) - wZ)/2)/131072);
				hTimePhB = hTimePhA + wZ/131072;
				hTimePhC = hTimePhA - wY/131072;
                
                // ADC Syncronization setting value
                if ((u16)(PWM_PERIOD-hTimePhC) > TW_AFTER)
                {
                  hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (u16)(hTimePhC - hTimePhA);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > (u16)(PWM_PERIOD-hTimePhC)*2) 
                  {
                    hTimePhD = hTimePhC - TW_BEFORE; // Ts before Phase C 
                  }
                  else
                  {
                    hTimePhD = hTimePhC + TW_AFTER; // DT + Tn after Phase C
                    
                    if (hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=PWM1_MODE;
                      
                      hTimePhD = (2 * PWM_PERIOD) - hTimePhD-1;
                    }
                  }
                }

		break;
                
    case SECTOR_6:
                hTimePhA = (T/8) + ((((T - wX) + wY)/2)/131072);
				hTimePhC = hTimePhA - wY/131072;
				hTimePhB = hTimePhC + wX/131072;
                
                // ADC Syncronization setting value
                if ((u16)(PWM_PERIOD-hTimePhA) > TW_AFTER)
                {
                  hTimePhD = PWM_PERIOD - 1;
                }
                else
                {
                  hDeltaDuty = (u16)(hTimePhA - hTimePhC);
                  
                  // Definition of crossing point
                  if (hDeltaDuty > (u16)(PWM_PERIOD-hTimePhA)*2) 
                  {
                    hTimePhD = hTimePhA - TW_BEFORE; // Ts before Phase A 
                  }
                  else
                  {
                    hTimePhD = hTimePhA + TW_AFTER; // DT + Tn after Phase A
                    
                    if (hTimePhD >= PWM_PERIOD)
                    {
                      // Trigger of ADC at Falling Edge PWM4
                      // OCR update
                      
                      //Set Polarity of CC4 Low
                      PWM4Direction=PWM1_MODE;
                      
                      hTimePhD = (2 * PWM_PERIOD) - hTimePhD-1;
                    }
                  }
                }

                break;
    default:
		break;
   }
  
  if (PWM4Direction == PWM2_MODE)
  {
    //Set Polarity of CC4 High
    TIM1->CCER &= 0xDFFF;    
  }
  else
  {
    //Set Polarity of CC4 Low
    TIM1->CCER |= 0x2000;
  }
  
  /* Load compare registers values */ 
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,hTimePhA);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2,hTimePhB);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3,hTimePhC);
	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4,hTimePhD);
}

//数学函数
const s16 hSin_Cos_Table[256] = {
0x0000,0x00C9,0x0192,0x025B,0x0324,0x03ED,0x04B6,0x057F,
0x0648,0x0711,0x07D9,0x08A2,0x096A,0x0A33,0x0AFB,0x0BC4,
0x0C8C,0x0D54,0x0E1C,0x0EE3,0x0FAB,0x1072,0x113A,0x1201,
0x12C8,0x138F,0x1455,0x151C,0x15E2,0x16A8,0x176E,0x1833,
0x18F9,0x19BE,0x1A82,0x1B47,0x1C0B,0x1CCF,0x1D93,0x1E57,
0x1F1A,0x1FDD,0x209F,0x2161,0x2223,0x22E5,0x23A6,0x2467,
0x2528,0x25E8,0x26A8,0x2767,0x2826,0x28E5,0x29A3,0x2A61,
0x2B1F,0x2BDC,0x2C99,0x2D55,0x2E11,0x2ECC,0x2F87,0x3041,
0x30FB,0x31B5,0x326E,0x3326,0x33DF,0x3496,0x354D,0x3604,
0x36BA,0x376F,0x3824,0x38D9,0x398C,0x3A40,0x3AF2,0x3BA5,
0x3C56,0x3D07,0x3DB8,0x3E68,0x3F17,0x3FC5,0x4073,0x4121,
0x41CE,0x427A,0x4325,0x43D0,0x447A,0x4524,0x45CD,0x4675,
0x471C,0x47C3,0x4869,0x490F,0x49B4,0x4A58,0x4AFB,0x4B9D,
0x4C3F,0x4CE0,0x4D81,0x4E20,0x4EBF,0x4F5D,0x4FFB,0x5097,
0x5133,0x51CE,0x5268,0x5302,0x539B,0x5432,0x54C9,0x5560,
0x55F5,0x568A,0x571D,0x57B0,0x5842,0x58D3,0x5964,0x59F3,
0x5A82,0x5B0F,0x5B9C,0x5C28,0x5CB3,0x5D3E,0x5DC7,0x5E4F,
0x5ED7,0x5F5D,0x5FE3,0x6068,0x60EB,0x616E,0x61F0,0x6271,
0x62F1,0x6370,0x63EE,0x646C,0x64E8,0x6563,0x65DD,0x6656,
0x66CF,0x6746,0x67BC,0x6832,0x68A6,0x6919,0x698B,0x69FD,
0x6A6D,0x6ADC,0x6B4A,0x6BB7,0x6C23,0x6C8E,0x6CF8,0x6D61,
0x6DC9,0x6E30,0x6E96,0x6EFB,0x6F5E,0x6FC1,0x7022,0x7083,
0x70E2,0x7140,0x719D,0x71F9,0x7254,0x72AE,0x7307,0x735E,
0x73B5,0x740A,0x745F,0x74B2,0x7504,0x7555,0x75A5,0x75F3,
0x7641,0x768D,0x76D8,0x7722,0x776B,0x77B3,0x77FA,0x783F,
0x7884,0x78C7,0x7909,0x794A,0x7989,0x79C8,0x7A05,0x7A41,
0x7A7C,0x7AB6,0x7AEE,0x7B26,0x7B5C,0x7B91,0x7BC5,0x7BF8,
0x7C29,0x7C59,0x7C88,0x7CB6,0x7CE3,0x7D0E,0x7D39,0x7D62,
0x7D89,0x7DB0,0x7DD5,0x7DFA,0x7E1D,0x7E3E,0x7E5F,0x7E7E,
0x7E9C,0x7EB9,0x7ED5,0x7EEF,0x7F09,0x7F21,0x7F37,0x7F4D,
0x7F61,0x7F74,0x7F86,0x7F97,0x7FA6,0x7FB4,0x7FC1,0x7FCD,
0x7FD8,0x7FE1,0x7FE9,0x7FF0,0x7FF5,0x7FF9,0x7FFD,0x7FFE};



Curr_Components Clarke(Curr_Components Curr_Input)         
{
  Curr_Components Curr_Output; 
  s32 qIa_divSQRT3_tmp;
  s32 qIb_divSQRT3_tmp;    //定义32位有符号数，用来暂存Q30格式  
  s16 qIa_divSQRT3;
  s16 qIb_divSQRT3 ;
 
  Curr_Output.qI_Component1 = Curr_Input.qI_Component1;     //Ialpha = Ia
 
  qIa_divSQRT3_tmp = divSQRT_3 * Curr_Input.qI_Component1;  //计算Ia/√3
  qIa_divSQRT3_tmp /=32768;                                 //两个Q15数相乘，会变成Q30，因此要右移15位，变回Q15	    
  qIb_divSQRT3_tmp = divSQRT_3 * Curr_Input.qI_Component2;  //计算Ib/√3
  qIb_divSQRT3_tmp /=32768;
  
  qIa_divSQRT3=((s16)(qIa_divSQRT3_tmp));	                //s32赋值给s16	 		
  qIb_divSQRT3=((s16)(qIb_divSQRT3_tmp));				
     
  Curr_Output.qI_Component2=(-(qIa_divSQRT3)-(qIb_divSQRT3)-(qIb_divSQRT3));  //Ibeta = -(2*Ib+Ia)/sqrt(3) 
  return(Curr_Output); 
}
 
	
/*******************************************************************************
* Function Name  : Trig_Functions 
* Description    : 本函数返回输入角度的cos和sin函数值
* Input          : angle in s16 format
* Output         : Cosine and Sine in s16 format
*******************************************************************************/
Trig_Components Trig_Functions(s16 hAngle)  //hAngle=0,转子电角度=0度。hAngle=S16_MAX,转子电角度=180度。hAngle=S16_MIN,转子电角度=-180度
{
  u16 hindex;
  Trig_Components Local_Components;
  
  /* 10 bit index computation  */  
  hindex = (u16)(hAngle + 32768);  
  hindex /= 64;      
  
  switch (hindex & SIN_MASK) 
  {
  case U0_90:
    Local_Components.hSin = hSin_Cos_Table[(u8)(hindex)];
    Local_Components.hCos = hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
    break;
  
  case U90_180:  
     Local_Components.hSin = hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
     Local_Components.hCos = -hSin_Cos_Table[(u8)(hindex)];
    break;
  
  case U180_270:
     Local_Components.hSin = -hSin_Cos_Table[(u8)(hindex)];
     Local_Components.hCos = -hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
    break;
  
  case U270_360:
     Local_Components.hSin =  -hSin_Cos_Table[(u8)(0xFF-(u8)(hindex))];
     Local_Components.hCos =  hSin_Cos_Table[(u8)(hindex)]; 
    break;
  default:
    break;
  }
  return (Local_Components);
}
 

/**********************************************************************************************************
Park变换，输入电角度、Ialpha和Ibeta，经过Park变换得到Iq、Id
**********************************************************************************************************/ 
Curr_Components Park(Curr_Components Curr_Input, s16 Theta)       
{ 
  Curr_Components Curr_Output;
  s32 qId_tmp_1, qId_tmp_2;
  s32 qIq_tmp_1, qIq_tmp_2;     
  s16 qId_1, qId_2;  
  s16 qIq_1, qIq_2;  
  
  Vector_Components = Trig_Functions(Theta);                      //计算电角度的cos和sin
  
  qIq_tmp_1 = Curr_Input.qI_Component1 * Vector_Components.hCos;  //计算Ialpha*cosθ	
  qIq_tmp_1 /= 32768;
  qIq_tmp_2 = Curr_Input.qI_Component2 *Vector_Components.hSin;   //计算Ibeta*sinθ
  qIq_tmp_2 /= 32768;
 
  qIq_1 = ((s16)(qIq_tmp_1));
  qIq_2 = ((s16)(qIq_tmp_2));
  Curr_Output.qI_Component1 = ((qIq_1)-(qIq_2));	//Iq=Ialpha*cosθ- Ibeta*sinθ
  
  qId_tmp_1 = Curr_Input.qI_Component1 * Vector_Components.hSin;  //计算Ialpha*sinθ
  qId_tmp_1 /= 32768;
  qId_tmp_2 = Curr_Input.qI_Component2 * Vector_Components.hCos;  //计算Ibeta*cosθ
  qId_tmp_2 /= 32768;
  
  qId_1 = (s16)(qId_tmp_1);		 
  qId_2 = (s16)(qId_tmp_2);					
  Curr_Output.qI_Component2 = ((qId_1)+(qId_2));   //Id=Ialpha*sinθ+ Ibeta*cosθ
  
  return (Curr_Output);
}
 
/**********************************************************************************************************
反park变换，输入Uq、Ud得到Ualpha、Ubeta
**********************************************************************************************************/ 
Volt_Components Rev_Park(Volt_Components Volt_Input)
{ 	
  s32 qValpha_tmp1,qValpha_tmp2,qVbeta_tmp1,qVbeta_tmp2;
  s16 qValpha_1,qValpha_2,qVbeta_1,qVbeta_2;
  Volt_Components Volt_Output;
   
  qValpha_tmp1 = Volt_Input.qV_Component1 * Vector_Components.hCos;  //Uq*cosθ
  qValpha_tmp1 /= 32768; 
  qValpha_tmp2 = Volt_Input.qV_Component2 * Vector_Components.hSin;  //Ud*sinθ
  qValpha_tmp2 /= 32768;
		
  qValpha_1 = (s16)(qValpha_tmp1);		
  qValpha_2 = (s16)(qValpha_tmp2);			
  Volt_Output.qV_Component1 = ((qValpha_1)+(qValpha_2));             //Ualpha=Uq*cosθ+ Ud*sinθ
  
  qVbeta_tmp1 = Volt_Input.qV_Component1 * Vector_Components.hSin;   //Uq*sinθ
  qVbeta_tmp1 /= 32768;  
  qVbeta_tmp2 = Volt_Input.qV_Component2 * Vector_Components.hCos;   //Ud*cosθ
  qVbeta_tmp2 /= 32768;
 
  qVbeta_1 = (s16)(qVbeta_tmp1);				
  qVbeta_2 = (s16)(qVbeta_tmp2);  				
  Volt_Output.qV_Component2 = -(qVbeta_1)+(qVbeta_2);                //Ubeta=Ud*cosθ- Uq*sinθ
 
  return(Volt_Output);
}

加入上面代码后，电机应该就能转动了，如果不转动，适当改变cnt的值，或者加入几毫秒的延迟，因为此刻我们并未将FOC放在ADC中断中，Stat_Volt_q_d.qV_Component2 即Id不要设置的太大，尽量保持在一个安全等级范围内，所以这样来看，使电机转起来只需要PWM模块与反Park变换和SVPWM模块，基本外设我们此时只用到了PWM，其实还是挺简单的哈。但是此刻是开环运行，我们无法得知电机真实的运行状态，所以需要引入电流闭环。

3、测量电角度！（编码器）

本次使用的是ABZ1250线的编码器，通过配置定时器的编码器模式，并设置为4倍频，可以准确的测量出当前电角度，具体配置见下图。注意选择好自己对应的编码器引脚，打开定时器中断，设置优先级为2 。
在这里插入图片描述
然后在初始化中，开启编码器模式，通过串口打印出电角度（可参考：串口使用printf），用手转动电机轴，观察信息是否正确，一圈范围为：-32768—+32768。或者借助步骤2，让电机转起来，然后查看电角度波形，如下图所示。有了电角度后，我们就可以让电机飞了！
在这里插入图片描述

4、测量电流吧！（三电阻采样）

按图示配置好ADC外设（只用了ADC1），并开启ADC中断，等级设置为1.生成代码。
在这里插入图片描述
可以使用步骤1测试ADC的正确性，每次导通一相，读取一次ADC值，增大占空比，看看AD值是否增加。然后，首先需要对初始ADC进行校准，也就是在关闭各个桥臂的情况下，读出3个注入通道的ADC值，作为初始电流偏置值，或者成为零电流值。具体可参考FOC和SVPWM的C语言代码实现。本文处理比较粗糙，直接多次读取后，进行赋值，不建议这种做法。然后通过下面的代码读出3相电流值。然后仍然可以使用开环SVPWM让电机转起来，然后看电流波形是否为正弦波，或者接近正弦波。
要计算出实际电流值：

实际电流值 = (ADC值>>4)/4096*(3.3-1.65)/Amp/R ；
Amp为放大倍数，R为采样电阻值。

复制代码

//3电阻采样电流值
Curr_Components SVPWM_3ShuntGetPhaseCurrentValues(void)
{
  Curr_Components Local_Stator_Currents;
  s32 wAux;

switch (bSector)
   {
   case 4:
   case 5: //Current on Phase C not accessible     
             
            wAux = (s32)(hPhaseA_OffSet)- (HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1)<<1);                
            if (wAux < S16_MIN)
            {
              Local_Stator_Currents.qI_Component1= S16_MIN;
            }  
            else  if (wAux > S16_MAX)
                  { 
                    Local_Stator_Currents.qI_Component1= S16_MAX;
                  }
                  else
                  {
                    Local_Stator_Currents.qI_Component1= wAux;
                  }
                     
           // Ib = (hPhaseBOffset)-(ADC Channel 12 value)
            wAux = (s32)(hPhaseB_OffSet)-(HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2)<<1);
           // Saturation of Ib
            if (wAux < S16_MIN)
            {
              Local_Stator_Currents.qI_Component2= S16_MIN;
            }  
            else  if (wAux > S16_MAX)
                  { 
                    Local_Stator_Currents.qI_Component2= S16_MAX;
                  }
                  else
                  {
                    Local_Stator_Currents.qI_Component2= wAux;
                  }
           break;
           
   case 6:
   case 1:  //Current on Phase A not accessible     
            // Ib = (hPhaseBOffset)-(ADC Channel 12 value)
            wAux = (s32)(hPhaseB_OffSet)-(HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2)<<1);
            //Saturation of Ib 
            if (wAux < S16_MIN)
            {
              Local_Stator_Currents.qI_Component2= S16_MIN;
            }  
            else  if (wAux > S16_MAX)
                  { 
                    Local_Stator_Currents.qI_Component2= S16_MAX;
                  }
                  else
                  {
                    Local_Stator_Currents.qI_Component2= wAux;
                  }
            // Ia = -Ic -Ib 
            wAux = (HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_3)<<1)-hPhaseC_OffSet-
                                            Local_Stator_Currents.qI_Component2;
            //Saturation of Ia
            if (wAux> S16_MAX)
            {
               Local_Stator_Currents.qI_Component1 = S16_MAX;
            }
            else  if (wAux <S16_MIN)
                  {
                   Local_Stator_Currents.qI_Component1 = S16_MIN;
                  }
                  else
                  {  
                    Local_Stator_Currents.qI_Component1 = wAux;
                  }
           break;
           
   case 2:
   case 3:  // Current on Phase B not accessible
            // Ia = (hPhaseAOffset)-(ADC Channel 11 value)     
            wAux = (s32)(hPhaseA_OffSet)-(HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1)<<1);
            //Saturation of Ia 
            if (wAux < S16_MIN)
            {
              Local_Stator_Currents.qI_Component1= S16_MIN;
            }  
            else  if (wAux > S16_MAX)
                  { 
                    Local_Stator_Currents.qI_Component1= S16_MAX;
                  }
                  else
                  {
                    Local_Stator_Currents.qI_Component1= wAux;
                  }
     
            // Ib = -Ic-Ia;
            wAux = (HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_3)<<1) - hPhaseC_OffSet - 
                                            Local_Stator_Currents.qI_Component1;
            // Saturation of Ib
            if (wAux> S16_MAX)
            {
              Local_Stator_Currents.qI_Component2=S16_MAX;
            }
            else  if (wAux <S16_MIN)
                  {  
                    Local_Stator_Currents.qI_Component2 = S16_MIN;
                  }
                  else  
                  {
                    Local_Stator_Currents.qI_Component2 = wAux;
                  }                     
           break;

default:
           break;
   } 
  
  return(Local_Stator_Currents); 
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
//3电阻采样电流值
Curr_Components SVPWM_3ShuntGetPhaseCurrentValues(void)
{
  Curr_Components Local_Stator_Currents;
  s32 wAux;

  switch (bSector)
   {
   case 4:
   case 5: //Current on Phase C not accessible     
             
            wAux = (s32)(hPhaseA_OffSet)- (HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1)<<1);                
            if (wAux < S16_MIN)
            {
              Local_Stator_Currents.qI_Component1= S16_MIN;
            }  
            else  if (wAux > S16_MAX)
                  { 
                    Local_Stator_Currents.qI_Component1= S16_MAX;
                  }
                  else
                  {
                    Local_Stator_Currents.qI_Component1= wAux;
                  }
                     
           // Ib = (hPhaseBOffset)-(ADC Channel 12 value)
            wAux = (s32)(hPhaseB_OffSet)-(HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2)<<1);
           // Saturation of Ib
            if (wAux < S16_MIN)
            {
              Local_Stator_Currents.qI_Component2= S16_MIN;
            }  
            else  if (wAux > S16_MAX)
                  { 
                    Local_Stator_Currents.qI_Component2= S16_MAX;
                  }
                  else
                  {
                    Local_Stator_Currents.qI_Component2= wAux;
                  }
           break;
           
   case 6:
   case 1:  //Current on Phase A not accessible     
            // Ib = (hPhaseBOffset)-(ADC Channel 12 value)
            wAux = (s32)(hPhaseB_OffSet)-(HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2)<<1);
            //Saturation of Ib 
            if (wAux < S16_MIN)
            {
              Local_Stator_Currents.qI_Component2= S16_MIN;
            }  
            else  if (wAux > S16_MAX)
                  { 
                    Local_Stator_Currents.qI_Component2= S16_MAX;
                  }
                  else
                  {
                    Local_Stator_Currents.qI_Component2= wAux;
                  }
            // Ia = -Ic -Ib 
            wAux = (HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_3)<<1)-hPhaseC_OffSet-
                                            Local_Stator_Currents.qI_Component2;
            //Saturation of Ia
            if (wAux> S16_MAX)
            {
               Local_Stator_Currents.qI_Component1 = S16_MAX;
            }
            else  if (wAux <S16_MIN)
                  {
                   Local_Stator_Currents.qI_Component1 = S16_MIN;
                  }
                  else
                  {  
                    Local_Stator_Currents.qI_Component1 = wAux;
                  }
           break;
           
   case 2:
   case 3:  // Current on Phase B not accessible
            // Ia = (hPhaseAOffset)-(ADC Channel 11 value)     
            wAux = (s32)(hPhaseA_OffSet)-(HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1)<<1);
            //Saturation of Ia 
            if (wAux < S16_MIN)
            {
              Local_Stator_Currents.qI_Component1= S16_MIN;
            }  
            else  if (wAux > S16_MAX)
                  { 
                    Local_Stator_Currents.qI_Component1= S16_MAX;
                  }
                  else
                  {
                    Local_Stator_Currents.qI_Component1= wAux;
                  }
     
            // Ib = -Ic-Ia;
            wAux = (HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_3)<<1) - hPhaseC_OffSet - 
                                            Local_Stator_Currents.qI_Component1;
            // Saturation of Ib
            if (wAux> S16_MAX)
            {
              Local_Stator_Currents.qI_Component2=S16_MAX;
            }
            else  if (wAux <S16_MIN)
                  {  
                    Local_Stator_Currents.qI_Component2 = S16_MIN;
                  }
                  else  
                  {
                    Local_Stator_Currents.qI_Component2 = wAux;
                  }                     
           break;

   default:
           break;
   } 
  
  return(Local_Stator_Currents); 
}

5、让电机飞！（电流闭环）

下面就是PID相关代码，包括初始化函数和PID函数，初始化函数加入到电机初始化函数中，然后将FOC函数中的开环调试部分注释，其它的打开（电压限幅函数在下面），将电角度设置为0，将q轴参考值，PID参数全部设置为0，d轴参考值设置为1000（具体由板子与电机决定，一定要在安全范围内），然后开始调节PI参数（由小忘大调），可以通过串口曲线绘制，观察PI效果，调好后，将q轴PI参数设置为相同，然后加入启动函数（主要是电角度对齐），将电角度设置成读取电角度函数，设置好Iq参考值，电机会一直开始转，如果不加限幅，会加速到最大。
（工程代码见文末）

复制代码

volatile s16 hTorque_Reference;   //q轴设定值
volatile s16 hFlux_Reference;     //d轴设定值
volatile s16 hSpeed_Reference;    //速度环设定值
/****************************** 扭矩的PID参数，即q轴 *******************************************************/
#define PID_TORQUE_REFERENCE   (s16)000  //q轴的设定值，PID的目的就是要让测量的q轴值与设定值误差为0   
#define PID_TORQUE_KP_DEFAULT  (s16)2.35  //Kp默认值
#define PID_TORQUE_KI_DEFAULT  (s16)880   //Ki默认值
#define PID_TORQUE_KD_DEFAULT  (s16)0   //Kd默认值
 
/****************************** 转子磁通的PID参数，即d轴 *******************************************************/
#define PID_FLUX_REFERENCE   (s16)000       //d轴的设定值
#define PID_FLUX_KP_DEFAULT  (s16)2.35  
#define PID_FLUX_KI_DEFAULT  (s16)880  
#define PID_FLUX_KD_DEFAULT  (s16)0
 
/****************************** q轴和d轴PID参数的放大倍数 *******************************************************/
#define TF_KPDIV ((u16)(1024))    //因为Kp、Ki、Kd值很小，而我们需要整数计算，所以需要放大。得出计算结果之后，再缩小。1024
#define TF_KIDIV ((u16)(16384))//16384
#define TF_KDDIV ((u16)(8192))
 
/****************************** 速度环的PID参数 *******************************************************/
#define PID_SPEED_REFERENCE_RPM   (s16)1000         //电机的设定转速
#define PID_SPEED_REFERENCE       (u16)(PID_SPEED_REFERENCE_RPM/6)  //电机转速和速度环的设定值一般都不相等，电机不同，它们的关系也不同
#define PID_SPEED_KP_DEFAULT      (s16)50
#define PID_SPEED_KI_DEFAULT      (s16)10
#define PID_SPEED_KD_DEFAULT      (s16)0
#define NOMINAL_CURRENT           (s16)18000            //motor nominal current (0-pk)，3倍的额定电流
#define IQMAX                     NOMINAL_CURRENT	   //速度环输出最大值
 
/****************************** 速度环PID参数的放大倍数 *******************************************************/
#define SP_KPDIV ((u16)(16))
#define SP_KIDIV ((u16)(256))
#define SP_KDDIV ((u16)(16))

void PID_Init (PID_Struct_t *PID_Torque, PID_Struct_t *PID_Flux, PID_Struct_t *PID_Speed)
{
    hTorque_Reference = PID_TORQUE_REFERENCE;          //q轴设定值初始化
/******************************************* 下面是控制扭矩的PID参数，即q轴大小 **************************************************************/
    PID_Torque->hKp_Gain    = PID_TORQUE_KP_DEFAULT;   //Kp参数，放大了hKp_Divisor倍。调节结果除以hKp_Divisor才是真实结果
    PID_Torque->hKp_Divisor = TF_KPDIV;                //Kp参数分数因子
    PID_Torque->hKi_Gain = PID_TORQUE_KI_DEFAULT;      //Ki参数
    PID_Torque->hKi_Divisor = TF_KIDIV;                //Ki参数分数因子
    PID_Torque->hKd_Gain = PID_TORQUE_KD_DEFAULT;      //Kd参数
    PID_Torque->hKd_Divisor = TF_KDDIV;                //Kd参数分数因子
    PID_Torque->wPreviousError = 0;                    //上次计算的误差值，用于D调节
    PID_Torque->hLower_Limit_Output=S16_MIN;           //PID输出下限幅
    PID_Torque->hUpper_Limit_Output= S16_MAX;          //PID输出上限幅
    PID_Torque->wLower_Limit_Integral = S16_MIN * TF_KIDIV;  //I调节的下限福
    PID_Torque->wUpper_Limit_Integral = S16_MAX * TF_KIDIV;  //I调节的上限幅
    PID_Torque->wIntegral = 0;                         //I调节的结果，因为是积分，所以要一直累积
/******************************************* 上面是控制扭矩的PID参数，即q轴大小 **************************************************************/
 
    hFlux_Reference = PID_FLUX_REFERENCE;              //对于SM-PMSM电机，Id = 0
/******************************************* 下面是控制转子磁通的PID参数，即d轴大小 **************************************************************/
    PID_Flux->hKp_Gain    = PID_FLUX_KP_DEFAULT;
    PID_Flux->hKp_Divisor = TF_KPDIV;
    PID_Flux->hKi_Gain = PID_FLUX_KI_DEFAULT;
    PID_Flux->hKi_Divisor = TF_KIDIV;
    PID_Flux->hKd_Gain = PID_FLUX_KD_DEFAULT;
    PID_Flux->hKd_Divisor = TF_KDDIV;
    PID_Flux->wPreviousError = 0;
    PID_Flux->hLower_Limit_Output=S16_MIN;   
    PID_Flux->hUpper_Limit_Output= S16_MAX;   
    PID_Flux->wLower_Limit_Integral = S16_MIN * TF_KIDIV;
    PID_Flux->wUpper_Limit_Integral = S16_MAX * TF_KIDIV;
    PID_Flux->wIntegral = 0;
/******************************************* 上面是控制转子磁通的PID参数，即d轴大小 **************************************************************/
 
    hSpeed_Reference = PID_SPEED_REFERENCE;
/******************************************* 下面是速度环的PID参数 **************************************************************/
    PID_Speed->hKp_Gain    = PID_SPEED_KP_DEFAULT;
    PID_Speed->hKp_Divisor = SP_KPDIV;
    PID_Speed->hKi_Gain = PID_SPEED_KI_DEFAULT;
    PID_Speed->hKi_Divisor = SP_KIDIV;
    PID_Speed->hKd_Gain = PID_SPEED_KD_DEFAULT;
    PID_Speed->hKd_Divisor = SP_KDDIV;
    PID_Speed->wPreviousError = 0;
    PID_Speed->hLower_Limit_Output= -IQMAX;   
    PID_Speed->hUpper_Limit_Output= IQMAX;   
    PID_Speed->wLower_Limit_Integral = -IQMAX * SP_KIDIV;
    PID_Speed->wUpper_Limit_Integral = IQMAX * SP_KIDIV;
    PID_Speed->wIntegral = 0;
/******************************************* 上面是速度环的PID参数 **************************************************************/
}
 
//#define DIFFERENTIAL_TERM_ENABLED    //不使用PID的D调节
typedef signed long long s64;
s16 PID_Regulator(s16 hReference, s16 hPresentFeedback, PID_Struct_t *PID_Struct)
{
    s32 wError, wProportional_Term,wIntegral_Term, houtput_32;
    s64 dwAux;
#ifdef DIFFERENTIAL_TERM_ENABLED                         //如果使能了D调节
	s32 wDifferential_Term;
#endif    
	
    wError= (s32)(hReference - hPresentFeedback);		 //设定值-反馈值，取得需要误差量delta_e
    wProportional_Term = PID_Struct->hKp_Gain * wError;	 //PID的P调节，即比例放大调节：wP = Kp * delta_e
 
    if (PID_Struct->hKi_Gain == 0)                       //下面进行PID的I调节，即误差的累积调节
    {
        PID_Struct->wIntegral = 0;                       //如果I参数=0，I调节就=0 
    }
    else
    {
        wIntegral_Term = PID_Struct->hKi_Gain * wError;		    //wI = Ki * delta_e	，本次积分项
        dwAux = PID_Struct->wIntegral + (s64)(wIntegral_Term);	//积分累积的调节量 = 以前的积分累积量 + 本次的积分项
 
        if (dwAux > PID_Struct->wUpper_Limit_Integral)		    //对PID的I调节做限幅
        {
            PID_Struct->wIntegral = PID_Struct->wUpper_Limit_Integral;	//上限
        }
        else if (dwAux < PID_Struct->wLower_Limit_Integral)				//下限
        {
            PID_Struct->wIntegral = PID_Struct->wLower_Limit_Integral;
        }
        else
        {
            PID_Struct->wIntegral = (s32)(dwAux);		          //不超限, 更新积分累积项为dwAux
        }
    }
#ifdef DIFFERENTIAL_TERM_ENABLED						          //如果使能了D调节
	{
	s32 wtemp;
  
	wtemp = wError - PID_Struct->wPreviousError;			      //取得上次和这次的误差之差
	wDifferential_Term = PID_Struct->hKd_Gain * wtemp;	          //D调节结果，wD = Kd * delta_d
	PID_Struct->wPreviousError = wError;    				      //更新上次误差，用于下次运算	
 
	}
	houtput_32 = (wProportional_Term/PID_Struct->hKp_Divisor+     //输出总的调节量 = 比例调节量/分数因子 +
                  PID_Struct->wIntegral/PID_Struct->hKi_Divisor + //				 + 积分调节量/分数因子
                  wDifferential_Term/PID_Struct->hKd_Divisor); 	  //				 + 微分调节量/分数因子
 
#else  	
	//把P调节和I调节结果除以分数因子再相加，得到PI控制的结果
    houtput_32 = (wProportional_Term/PID_Struct->hKp_Divisor + PID_Struct->wIntegral/PID_Struct->hKi_Divisor);       
#endif
    if (houtput_32 >= PID_Struct->hUpper_Limit_Output)	   //PI控制结果限幅
    {
        return(PID_Struct->hUpper_Limit_Output);
    }
    else if (houtput_32 < PID_Struct->hLower_Limit_Output) //下限
    {
        return(PID_Struct->hLower_Limit_Output);
    }
    else
    {
        return((s16)(houtput_32)); 						   //不超限。输出结果 houtput_32
    }
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
volatile s16 hTorque_Reference;   //q轴设定值
volatile s16 hFlux_Reference;     //d轴设定值
volatile s16 hSpeed_Reference;    //速度环设定值
/****************************** 扭矩的PID参数，即q轴 *******************************************************/
#define PID_TORQUE_REFERENCE   (s16)000  //q轴的设定值，PID的目的就是要让测量的q轴值与设定值误差为0   
#define PID_TORQUE_KP_DEFAULT  (s16)2.35  //Kp默认值
#define PID_TORQUE_KI_DEFAULT  (s16)880   //Ki默认值
#define PID_TORQUE_KD_DEFAULT  (s16)0   //Kd默认值
 
/****************************** 转子磁通的PID参数，即d轴 *******************************************************/
#define PID_FLUX_REFERENCE   (s16)000       //d轴的设定值
#define PID_FLUX_KP_DEFAULT  (s16)2.35  
#define PID_FLUX_KI_DEFAULT  (s16)880  
#define PID_FLUX_KD_DEFAULT  (s16)0
 
/****************************** q轴和d轴PID参数的放大倍数 *******************************************************/
#define TF_KPDIV ((u16)(1024))    //因为Kp、Ki、Kd值很小，而我们需要整数计算，所以需要放大。得出计算结果之后，再缩小。1024
#define TF_KIDIV ((u16)(16384))//16384
#define TF_KDDIV ((u16)(8192))
 
/****************************** 速度环的PID参数 *******************************************************/
#define PID_SPEED_REFERENCE_RPM   (s16)1000         //电机的设定转速
#define PID_SPEED_REFERENCE       (u16)(PID_SPEED_REFERENCE_RPM/6)  //电机转速和速度环的设定值一般都不相等，电机不同，它们的关系也不同
#define PID_SPEED_KP_DEFAULT      (s16)50
#define PID_SPEED_KI_DEFAULT      (s16)10
#define PID_SPEED_KD_DEFAULT      (s16)0
#define NOMINAL_CURRENT           (s16)18000            //motor nominal current (0-pk)，3倍的额定电流
#define IQMAX                     NOMINAL_CURRENT	   //速度环输出最大值
 
/****************************** 速度环PID参数的放大倍数 *******************************************************/
#define SP_KPDIV ((u16)(16))
#define SP_KIDIV ((u16)(256))
#define SP_KDDIV ((u16)(16))

void PID_Init (PID_Struct_t *PID_Torque, PID_Struct_t *PID_Flux, PID_Struct_t *PID_Speed)
{
    hTorque_Reference = PID_TORQUE_REFERENCE;          //q轴设定值初始化
/******************************************* 下面是控制扭矩的PID参数，即q轴大小 **************************************************************/
    PID_Torque->hKp_Gain    = PID_TORQUE_KP_DEFAULT;   //Kp参数，放大了hKp_Divisor倍。调节结果除以hKp_Divisor才是真实结果
    PID_Torque->hKp_Divisor = TF_KPDIV;                //Kp参数分数因子
    PID_Torque->hKi_Gain = PID_TORQUE_KI_DEFAULT;      //Ki参数
    PID_Torque->hKi_Divisor = TF_KIDIV;                //Ki参数分数因子
    PID_Torque->hKd_Gain = PID_TORQUE_KD_DEFAULT;      //Kd参数
    PID_Torque->hKd_Divisor = TF_KDDIV;                //Kd参数分数因子
    PID_Torque->wPreviousError = 0;                    //上次计算的误差值，用于D调节
    PID_Torque->hLower_Limit_Output=S16_MIN;           //PID输出下限幅
    PID_Torque->hUpper_Limit_Output= S16_MAX;          //PID输出上限幅
    PID_Torque->wLower_Limit_Integral = S16_MIN * TF_KIDIV;  //I调节的下限福
    PID_Torque->wUpper_Limit_Integral = S16_MAX * TF_KIDIV;  //I调节的上限幅
    PID_Torque->wIntegral = 0;                         //I调节的结果，因为是积分，所以要一直累积
/******************************************* 上面是控制扭矩的PID参数，即q轴大小 **************************************************************/
 
    hFlux_Reference = PID_FLUX_REFERENCE;              //对于SM-PMSM电机，Id = 0
/******************************************* 下面是控制转子磁通的PID参数，即d轴大小 **************************************************************/
    PID_Flux->hKp_Gain    = PID_FLUX_KP_DEFAULT;
    PID_Flux->hKp_Divisor = TF_KPDIV;
    PID_Flux->hKi_Gain = PID_FLUX_KI_DEFAULT;
    PID_Flux->hKi_Divisor = TF_KIDIV;
    PID_Flux->hKd_Gain = PID_FLUX_KD_DEFAULT;
    PID_Flux->hKd_Divisor = TF_KDDIV;
    PID_Flux->wPreviousError = 0;
    PID_Flux->hLower_Limit_Output=S16_MIN;   
    PID_Flux->hUpper_Limit_Output= S16_MAX;   
    PID_Flux->wLower_Limit_Integral = S16_MIN * TF_KIDIV;
    PID_Flux->wUpper_Limit_Integral = S16_MAX * TF_KIDIV;
    PID_Flux->wIntegral = 0;
/******************************************* 上面是控制转子磁通的PID参数，即d轴大小 **************************************************************/
 
    hSpeed_Reference = PID_SPEED_REFERENCE;
/******************************************* 下面是速度环的PID参数 **************************************************************/
    PID_Speed->hKp_Gain    = PID_SPEED_KP_DEFAULT;
    PID_Speed->hKp_Divisor = SP_KPDIV;
    PID_Speed->hKi_Gain = PID_SPEED_KI_DEFAULT;
    PID_Speed->hKi_Divisor = SP_KIDIV;
    PID_Speed->hKd_Gain = PID_SPEED_KD_DEFAULT;
    PID_Speed->hKd_Divisor = SP_KDDIV;
    PID_Speed->wPreviousError = 0;
    PID_Speed->hLower_Limit_Output= -IQMAX;   
    PID_Speed->hUpper_Limit_Output= IQMAX;   
    PID_Speed->wLower_Limit_Integral = -IQMAX * SP_KIDIV;
    PID_Speed->wUpper_Limit_Integral = IQMAX * SP_KIDIV;
    PID_Speed->wIntegral = 0;
/******************************************* 上面是速度环的PID参数 **************************************************************/
}
 
//#define DIFFERENTIAL_TERM_ENABLED    //不使用PID的D调节
typedef signed long long s64;
s16 PID_Regulator(s16 hReference, s16 hPresentFeedback, PID_Struct_t *PID_Struct)
{
    s32 wError, wProportional_Term,wIntegral_Term, houtput_32;
    s64 dwAux;
#ifdef DIFFERENTIAL_TERM_ENABLED                         //如果使能了D调节
	s32 wDifferential_Term;
#endif    
	
    wError= (s32)(hReference - hPresentFeedback);		 //设定值-反馈值，取得需要误差量delta_e
    wProportional_Term = PID_Struct->hKp_Gain * wError;	 //PID的P调节，即比例放大调节：wP = Kp * delta_e
 
    if (PID_Struct->hKi_Gain == 0)                       //下面进行PID的I调节，即误差的累积调节
    {
        PID_Struct->wIntegral = 0;                       //如果I参数=0，I调节就=0 
    }
    else
    {
        wIntegral_Term = PID_Struct->hKi_Gain * wError;		    //wI = Ki * delta_e	，本次积分项
        dwAux = PID_Struct->wIntegral + (s64)(wIntegral_Term);	//积分累积的调节量 = 以前的积分累积量 + 本次的积分项
 
        if (dwAux > PID_Struct->wUpper_Limit_Integral)		    //对PID的I调节做限幅
        {
            PID_Struct->wIntegral = PID_Struct->wUpper_Limit_Integral;	//上限
        }
        else if (dwAux < PID_Struct->wLower_Limit_Integral)				//下限
        {
            PID_Struct->wIntegral = PID_Struct->wLower_Limit_Integral;
        }
        else
        {
            PID_Struct->wIntegral = (s32)(dwAux);		          //不超限, 更新积分累积项为dwAux
        }
    }
#ifdef DIFFERENTIAL_TERM_ENABLED						          //如果使能了D调节
	{
	s32 wtemp;
  
	wtemp = wError - PID_Struct->wPreviousError;			      //取得上次和这次的误差之差
	wDifferential_Term = PID_Struct->hKd_Gain * wtemp;	          //D调节结果，wD = Kd * delta_d
	PID_Struct->wPreviousError = wError;    				      //更新上次误差，用于下次运算	
 
	}
	houtput_32 = (wProportional_Term/PID_Struct->hKp_Divisor+     //输出总的调节量 = 比例调节量/分数因子 +
                  PID_Struct->wIntegral/PID_Struct->hKi_Divisor + //				 + 积分调节量/分数因子
                  wDifferential_Term/PID_Struct->hKd_Divisor); 	  //				 + 微分调节量/分数因子
 
#else  	
	//把P调节和I调节结果除以分数因子再相加，得到PI控制的结果
    houtput_32 = (wProportional_Term/PID_Struct->hKp_Divisor + PID_Struct->wIntegral/PID_Struct->hKi_Divisor);       
#endif
    if (houtput_32 >= PID_Struct->hUpper_Limit_Output)	   //PI控制结果限幅
    {
        return(PID_Struct->hUpper_Limit_Output);
    }
    else if (houtput_32 < PID_Struct->hLower_Limit_Output) //下限
    {
        return(PID_Struct->hLower_Limit_Output);
    }
    else
    {
        return((s16)(houtput_32)); 						   //不超限。输出结果 houtput_32
    }
}