MC_FOC_Driver.c 3.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110
  1. #include "MC_FOC_Driver.h"
  2. #include "MC_type.h"
  3. #include "MC_Globals.h"
  4. #include "MC_PID_regulators.h"
  5. #include "MC_Clarke_park.h"
  6. #include "stm32f10x_svpwm_3shunt.h"
  7. #define FluxWeak_Control 0
  8. //全局变量定义
  9. int32_t IqFdbFlt = 0;
  10. int32_t IdFdbFlt = 0;
  11. int32_t VoltSquareFlt = 0;
  12. int32_t UqVoltFlt = 0;
  13. int32_t UdVoltFlt = 0;
  14. void FOC_Model(int16_t Ref, int16_t MotorSpeed, uint16_t SVM_Angle) //电流环处理函数,严格按照框图理解
  15. {
  16. int32_t UqVoltTmp;
  17. int16_t UdVoltTmp;
  18. static int16_t IqFdb = 0;
  19. static int16_t IdFdb = 0;
  20. #if FluxWeak_Control
  21. int32_t VoltSquare = 0; //弱磁控制预留
  22. int32_t VoltMax = 0;
  23. #endif
  24. int32_t IdFluxLessRef = 0;
  25. int32_t UqCal = 0;
  26. Curr_Components Stat_Curr_a_b; // Stator currents Ia,Ib
  27. Curr_Components Stat_Curr_alfa_beta; // Ialpha & Ibeta, Clarke's transformations of Ia & Ib
  28. Volt_Components Stat_Volt_alfa_beta; // Valpha & Vbeta, RevPark transformations of Vq & Vd
  29. /**********STARTS THE VECTOR CONTROL *********************** */
  30. Stat_Curr_a_b = SVPWM_3ShuntGetPhaseCurrentValues(); //读取2相的电流值
  31. Stat_Curr_alfa_beta = Clarke(Stat_Curr_a_b); // 得到Ialpha和Ibeta,Clark变换
  32. Stat_Curr_q_d = Park(Stat_Curr_alfa_beta, SVM_Angle); // Stat_Curr_q_d为当前的Id和Iq值//SVM_Angle
  33. // 给定值为 Stat_Curr_q_d_ref_ref
  34. IqFdbFlt += (((int32_t)(Stat_Curr_q_d.qI_Component1 << 10)) - IqFdbFlt)>>10; // KFlt = 2ms
  35. IqFdb = IqFdbFlt >> 10;
  36. IdFdbFlt += (((int32_t)(Stat_Curr_q_d.qI_Component2 << 10)) - IdFdbFlt)>>10; // KFlt = 2ms
  37. IdFdb = IdFdbFlt >> 10;
  38. #if FluxWeak_Control // Debug 没调好,暂时关闭
  39. /* 增加弱磁处理 add Bike 20171007 */
  40. Tmp = ((int32_t)Stat_Volt_q_d.qV_Component1 * Stat_Volt_q_d.qV_Component1) + ((int32_t)Stat_Volt_q_d.qV_Component2 * Stat_Volt_q_d.qV_Component2); //2*Q30
  41. Tmp = Tmp >> 10; //Q20
  42. VoltSquareFlt += ((Tmp<<4) - VoltSquareFlt) >> 12;
  43. VoltSquare = VoltSquareFlt >> 4;
  44. VoltMax = 314641;
  45. IdFluxLessRef = PID_Regulator(VoltMax, VoltSquare, &PID_Weak_InitStructure) >> 10;
  46. #else
  47. IdFluxLessRef = 0;
  48. #endif
  49. UqVoltTmp = PID_Regulator(Ref,\
  50. IqFdb,\
  51. &PID_Torque_InitStructure); // 电流闭环输出q轴电压
  52. UqVoltFlt += ((UqVoltTmp << 9) - UqVoltFlt) >> 3;
  53. /*
  54. UqCal = DbSpdMotor*VMax*0.7/1020
  55. = DbSpdMotor*VMax/(1020/0.7)
  56. = DbSpdMotor*VMax/1020
  57. */
  58. #if 1 //加入前馈
  59. uint16_t Cal_K;
  60. Cal_K = (MC_MotorParam.Rate_Speed * 183) >> 7; //电机转速 * 1.43
  61. UqCal = ((int32_t)MotorSpeed * MAX_MODULE) / ((Cal_K < 1000) ? 1000 : Cal_K);
  62. UqVoltTmp = UqCal + (UqVoltFlt >> 9);
  63. #else //去掉前馈
  64. UqVoltTmp = UqVoltFlt >> 9;
  65. #endif
  66. UqVoltTmp = (UqVoltTmp > ((int32_t)MAX_MODULE)) ? MAX_MODULE : UqVoltTmp;
  67. Stat_Volt_q_d.qV_Component1 = UqVoltTmp;
  68. UdVoltTmp = PID_Regulator(IdFluxLessRef,\
  69. IdFdb,\
  70. &PID_Flux_InitStructure); // 电流闭环输出d轴电压
  71. UdVoltFlt += ((UdVoltTmp << 9) - UdVoltFlt) >> 3;
  72. Stat_Volt_q_d.qV_Component2 = UdVoltFlt >> 9;
  73. //circle limitation
  74. RevPark_Circle_Limitation(&Stat_Volt_q_d); // 电压极限圈限制? 会不会出现波动情况?
  75. /*Performs the Reverse Park transformation,
  76. i.e transforms stator voltages Vqs and Vds into Valpha and Vbeta on a
  77. stationary reference frame*/
  78. Stat_Volt_alfa_beta = Rev_Park(Stat_Volt_q_d);
  79. /*Valpha and Vbeta finally drive the power stage*/
  80. SVPWM_3ShuntCalcDutyCycles(Stat_Volt_alfa_beta); //实际的电流输出控制
  81. }
  82. void FOC_Enable(void)
  83. {
  84. FOC_Status = FOC_Status_RUN;
  85. }
  86. void FOC_Disable(void)
  87. {
  88. FOC_Status = FOC_Status_WAIT;
  89. Stat_Curr_q_d.qI_Component1 = 0;
  90. Stat_Curr_q_d.qI_Component2 = 0;
  91. Stat_Volt_q_d.qV_Component1 = 0;
  92. Stat_Volt_q_d.qV_Component2 = 0;
  93. }