说明
本文为无刷电机或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下，然后按照下面的图配置好基本外设。
#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_31.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);//SVPWMvoid 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实现函数，实际的电流输出控制}//SVPWMvoid 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_dPID_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 }}速度环跟踪曲线图
阶跃信号
正弦信号
工程链接：PMSM电机FOC简易驱动程序