说明
本文为无刷电机或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
参考资料：
（1）ST电机库
（2）PMSM的FOC 矢量控制算法调试流程，新手上手流程
（3）PMSM矢量控制算法调试流程
（4）FOC和SVPWM的C语言代码实现
（5）上官致远–深入理解无刷直流电机矢量控制技术–科学出版社
0、系统配置
将下列值加入到Cube的User Constants下，然后按照下面的图配置好基本外设。
#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电压，然后用手转动电机一圈，感到有几次阻力，就是几对极。
/* 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中加入下面定义，这在后面都会用到。
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);
//变量定义部分
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);
}
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
}
}
速度环跟踪曲线图
阶跃信号
正弦信号
工程链接：STM32-HAL库的PMSM、BLDC的FOC简易程序