#include "foc_algorithm.h" #include "speed_pid.h" #include "main.h" MotorParam_t motor1 = { .direction = MOTOR_CW, .poles = 4, .sensor = MOTOR_SENSOR_HALL, .eleangle = 0.0f, .ctrl_state = CTRL_STATE_IDLE, }; MotorParam_t* get_motor(void) { return &motor1; } FOC_INTERFACE_STATES_DEF FOC_Interface_states; FOC_INPUT_DEF FOC_Input; FOC_OUTPUT_DEF FOC_Output; RT_MODEL rtM_; RT_MODEL *const rtM = &rtM_; #ifdef __cplusplus extern "C" { #endif extern void stm32_ekf_Start_wrapper(real_T *xD); extern void stm32_ekf_Outputs_wrapper(const real32_T *u, real32_T *y, const real_T *xD); extern void stm32_ekf_Update_wrapper(const real32_T *u, real32_T *y, real_T *xD); extern void stm32_ekf_Terminate_wrapper(real_T *xD); #ifdef __cplusplus } #endif #ifdef __cplusplus extern "C" { #endif extern void L_identification_Start_wrapper(real_T *xD); extern void L_identification_Outputs_wrapper(const real32_T *u, real32_T *y, const real_T *xD); extern void L_identification_Update_wrapper(const real32_T *u, real32_T *y, real_T *xD); extern void L_identification_Terminate_wrapper(real_T *xD); #ifdef __cplusplus } #endif #ifdef __cplusplus extern "C" { #endif extern void R_flux_identification_Start_wrapper(real_T *xD); extern void R_flux_identification_Outputs_wrapper(const real32_T *u, real32_T *y, const real_T *xD); extern void R_flux_identification_Update_wrapper(const real32_T *u, real32_T *y, real_T *xD); extern void R_flux_identification_Terminate_wrapper(real_T *xD); #ifdef __cplusplus } #endif static CURRENT_ABC_DEF Current_Iabc; static CURRENT_ALPHA_BETA_DEF Current_Ialpha_beta; static VOLTAGE_ALPHA_BETA_DEF Voltage_Alpha_Beta; static TRANSF_COS_SIN_DEF Transf_Cos_Sin; static CURRENT_DQ_DEF Current_Idq; static VOLTAGE_DQ_DEF Voltage_DQ; static CURRENT_PID_DEF Current_D_PID; static CURRENT_PID_DEF Current_Q_PID; FOC_INPUT_DEF* get_foc_input(void) { return &FOC_Input; } FOC_OUTPUT_DEF* get_foc_ouput(void) { return &FOC_Output; } CURRENT_PID_DEF* get_currd_pid(void) { return &Current_D_PID; } CURRENT_PID_DEF* get_currq_pid(void) { return &Current_Q_PID; } CURRENT_DQ_DEF* get_curr_dq(void) { return &Current_Idq; } /*************************************** * FOC算法初始化 * 功能：初始化FOC算法相关参数 * 描述：包括PID参数、EKF和参数识别模块的初始化 ***************************************/ void foc_algorithm_initialize(void) { // 初始化电流PID控制器参数 { Current_D_PID.P_Gain = D_PI_P; Current_D_PID.I_Gain = D_PI_I; Current_D_PID.B_Gain = D_PI_KB; Current_D_PID.Max_Output = D_PI_UP_LIMIT; Current_D_PID.Min_Output = D_PI_LOW_LIMIT; Current_D_PID.I_Sum = 0.0f; Current_Q_PID.P_Gain = Q_PI_P; Current_Q_PID.I_Gain = Q_PI_I; Current_Q_PID.B_Gain = Q_PI_KB; Current_Q_PID.Max_Output = Q_PI_UP_LIMIT; Current_Q_PID.Min_Output = Q_PI_LOW_LIMIT; Current_Q_PID.I_Sum = 0.0f; } // 初始化速度PID控制器 speed_pid_initialize(); // 速度PID初始化 // 初始化EKF stm32_ekf_Start_wrapper(&FOC_Interface_states.EKF_States[0]);// EKF初始化 // 初始化电感识别模块 L_identification_Start_wrapper(&FOC_Interface_states.L_Ident_States);// 电感识别初始化 // 初始化电阻和磁链识别模块 R_flux_identification_Start_wrapper(&FOC_Interface_states.R_flux_Ident_States);// 电阻和磁链识别初始化 // 初始化EKF状态 { real_T initVector[4] = { 0, 0, 0, 0 }; { int_T i1; real_T *dw_DSTATE = &FOC_Interface_states.EKF_States[0]; for (i1=0; i1 < 4; i1++) { dw_DSTATE[i1] = initVector[i1]; } } } // 初始化电感识别状态 { real_T initVector[1] = { 0 }; { int_T i1; for (i1=0; i1 < 1; i1++) { FOC_Interface_states.L_Ident_States = initVector[0]; } } } // 初始化电阻和磁链识别状态 { real_T initVector[1] = { 0 }; { int_T i1; for (i1=0; i1 < 1; i1++) { FOC_Interface_states.R_flux_Ident_States = initVector[0]; } } } } /*************************************** * FOC算法主函数 * 功能：执行完整的FOC控制算法 * 描述：包括坐标变换、电流控制、SVPWM计算和参数识别 ***************************************/ void motor_ekf_closeloop_run(void) { // 1. 读取三相电流 Current_Iabc.Ia = FOC_Input.ia; // 读取A相电流 Current_Iabc.Ib = FOC_Input.ib; // 读取B相电流 Current_Iabc.Ic = FOC_Input.ic; // 读取C相电流 // 2. 坐标变换 Clarke_Transf(Current_Iabc,&Current_Ialpha_beta); // Clarke变换：三相到两相静止 Angle_To_Cos_Sin(FOC_Input.theta,&Transf_Cos_Sin); // 计算角度的余弦和正弦值 Park_Transf(Current_Ialpha_beta,Transf_Cos_Sin,&Current_Idq); // Park变换：两相静止到两相旋转 // 3. 电流PID控制 Current_PID_Calc(FOC_Input.Id_ref,Current_Idq.Id,&Voltage_DQ.Vd,&Current_D_PID); // D轴电流PID控制 Current_PID_Calc(FOC_Input.Iq_ref,Current_Idq.Iq,&Voltage_DQ.Vq,&Current_Q_PID); // Q轴电流PID控制 // 4. 反Park变换 Rev_Park_Transf(Voltage_DQ,Transf_Cos_Sin,&Voltage_Alpha_Beta); // 反Park变换：两相旋转到两相静止 // 5. 准备EKF输入数据 FOC_Interface_states.EKF_Interface[0] = Voltage_Alpha_Beta.Valpha; // Alpha轴电压 FOC_Interface_states.EKF_Interface[1] = Voltage_Alpha_Beta.Vbeta; // Beta轴电压 FOC_Interface_states.EKF_Interface[2] = Current_Ialpha_beta.Ialpha; // Alpha轴电流 FOC_Interface_states.EKF_Interface[3] = Current_Ialpha_beta.Ibeta; // Beta轴电流 FOC_Interface_states.EKF_Interface[4] = FOC_Input.Rs; // 定子电阻 FOC_Interface_states.EKF_Interface[5] = FOC_Input.Ls; // 定子电感 FOC_Interface_states.EKF_Interface[6] = FOC_Input.flux; // 磁链 // 6. 执行EKF输出计算 stm32_ekf_Outputs_wrapper(&FOC_Interface_states.EKF_Interface[0], &FOC_Output.EKF[0], &FOC_Interface_states.EKF_States[0]); // 7. 准备电阻和磁链识别输入数据 FOC_Interface_states.R_flux_Ident_Interface[0] = Current_Idq.Iq; // Q轴电流 FOC_Interface_states.R_flux_Ident_Interface[1] = FOC_Input.speed_fdk; // 电机转速 FOC_Interface_states.R_flux_Ident_Interface[2] = Voltage_DQ.Vq; // Q轴电压 // 8. 准备电感识别输入数据 FOC_Interface_states.L_Ident_Interface[0] = -(Current_Idq.Iq * FOC_Input.speed_fdk); // 计算电感识别输入 FOC_Interface_states.L_Ident_Interface[1] = Voltage_DQ.Vd; // D轴电压 // 9. 执行电感识别 L_identification_Outputs_wrapper(&FOC_Interface_states.L_Ident_Interface[0], &FOC_Interface_states.L_Ident_Output, &FOC_Interface_states.L_Ident_States); // 10. 执行电阻和磁链识别 R_flux_identification_Outputs_wrapper(&FOC_Interface_states.R_flux_Ident_Interface[0], &FOC_Interface_states.R_flux_Ident_Output[0], &FOC_Interface_states.R_flux_Ident_States); // 11. 计算SVPWM占空比 SVPWM_Calc(Voltage_Alpha_Beta,FOC_Input.Udc,FOC_Input.Tpwm); // SVPWM计算 // 12. 更新EKF状态 stm32_ekf_Update_wrapper(&FOC_Interface_states.EKF_Interface[0], &FOC_Output.EKF[0], &FOC_Interface_states.EKF_States[0]); // 13. 更新电感识别状态 L_identification_Update_wrapper(&FOC_Interface_states.L_Ident_Interface[0], &FOC_Interface_states.L_Ident_Output, &FOC_Interface_states.L_Ident_States); // 14. 更新电阻和磁链识别状态 R_flux_identification_Update_wrapper(&FOC_Interface_states.R_flux_Ident_Interface[0], &FOC_Interface_states.R_flux_Ident_Output[0], &FOC_Interface_states.R_flux_Ident_States); // 15. 更新输出参数 FOC_Output.L_RF[0] = FOC_Interface_states.L_Ident_Output; // 识别的电感 FOC_Output.L_RF[1] = FOC_Interface_states.R_flux_Ident_Output[0]; // 识别的电阻 FOC_Output.L_RF[2] = FOC_Interface_states.R_flux_Ident_Output[1]; // 识别的磁链 } void motor_hall_close_run(void) { } void motor_foc_openloop_run(void) { // 1. 读取三相电流 Current_Iabc.Ia = FOC_Input.ia; // 读取A相电流 Current_Iabc.Ib = FOC_Input.ib; // 读取B相电流 Current_Iabc.Ic = FOC_Input.ic; // 读取C相电流 // 2. 坐标变换 Clarke_Transf(Current_Iabc,&Current_Ialpha_beta); // Clarke变换：三相到两相静止 Angle_To_Cos_Sin(FOC_Input.theta,&Transf_Cos_Sin); // 计算角度的余弦和正弦值 Park_Transf(Current_Ialpha_beta,Transf_Cos_Sin,&Current_Idq); // Park变换：两相静止到两相旋转 // 3. 电流PID控制 Current_PID_Calc(FOC_Input.Id_ref,Current_Idq.Id,&Voltage_DQ.Vd,&Current_D_PID); // D轴电流PID控制 Current_PID_Calc(FOC_Input.Iq_ref,Current_Idq.Iq,&Voltage_DQ.Vq,&Current_Q_PID); // Q轴电流PID控制 // 4. 反Park变换 Rev_Park_Transf(Voltage_DQ,Transf_Cos_Sin,&Voltage_Alpha_Beta); // 反Park变换：两相旋转到两相静止 SVPWM_Calc(Voltage_Alpha_Beta,FOC_Input.Udc,FOC_Input.Tpwm); // SVPWM计算 }