FOC_TEST/motor/motor_run.c

#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计算
	
}