motor_ctl-2/User/Src/MC_FOC_Driver.c

#include "MC_FOC_Driver.h"
#include "MC_type.h"
#include "MC_Globals.h"
#include "MC_PID_regulators.h"
#include "MC_Clarke_park.h"
#include "stm32f10x_svpwm_3shunt.h"
#include "hall_sensor.h"
#include "motor_control.h"
#define FluxWeak_Control 0

//全局变量定义

int32_t IqFdbFlt = 0;
int32_t IdFdbFlt = 0;
int32_t VoltSquareFlt = 0;
int32_t UqVoltFlt = 0;
int32_t UdVoltFlt = 0;

void FOC_Model(int16_t Ref, int16_t MotorSpeed, uint16_t SVM_Angle)	  //电流环处理函数，严格按照框图理解
{  
	int32_t UqVoltTmp;
	int16_t UdVoltTmp;
	static int16_t IqFdb = 0;
	static int16_t IdFdb = 0;
	#if FluxWeak_Control
	int32_t VoltSquare = 0;    //弱磁控制预留
	int32_t VoltMax = 0; 
	#endif
	static int32_t IdFluxLessRef = 0, IqFluxLessRef = 0;
	int32_t UqCal = 0;
	static int32_t FOC_IqLim = 0;
	Curr_Components Stat_Curr_a_b;       // Stator currents Ia,Ib
	Curr_Components Stat_Curr_alfa_beta; // Ialpha & Ibeta, Clarke's transformations of Ia & Ib
	Volt_Components Stat_Volt_alfa_beta; // Valpha & Vbeta, RevPark transformations of Vq & Vd

  /**********STARTS THE VECTOR CONTROL *********************** */ 		
	Stat_Curr_a_b = SVPWM_3ShuntGetPhaseCurrentValues();			//读取2相的电流值
  Stat_Curr_alfa_beta = Clarke(Stat_Curr_a_b);	// 得到Ialpha和Ibeta,Clark变换
 	Stat_Curr_q_d = Park(Stat_Curr_alfa_beta, SVM_Angle);  // Stat_Curr_q_d为当前的Id和Iq值//SVM_Angle
																                         // 给定值为 Stat_Curr_q_d_ref_ref
	
	IqFdbFlt += (((int32_t)(Stat_Curr_q_d.qI_Component1 << 10)) - IqFdbFlt)>>10;  // KFlt = 2ms
	IqFdb = IqFdbFlt >> 10;
	
	IdFdbFlt += (((int32_t)(Stat_Curr_q_d.qI_Component2 << 10)) - IdFdbFlt)>>10;   // KFlt = 2ms
	IdFdb = IdFdbFlt >> 10;

	if((MC_HallSensorData.BackwardFlag == TRUE) && (MC_CalParam.AssistRunMode == MC_AssistRunMode_GAS))
	{
		if( ADC1_Result[ADC1_RANK_VIN] < 3000) //53853mV  17.951  3000
		{
			IqFluxLessRef = Ref;
			if(IdFluxLessRef < (-4))
			{
				IdFluxLessRef += 4;
			}
			else IdFluxLessRef = 0;			
		}
		else if( ADC1_Result[ADC1_RANK_VIN] < 3210 ) //57622mV 3210
		{
			FOC_IqLim = 1050 - (ADC1_Result[ADC1_RANK_VIN] - 3000) * 5;
			IqFluxLessRef = Ref < FOC_IqLim ? Ref : FOC_IqLim;
			IdFluxLessRef -= 2;
			if(IdFluxLessRef < -420) IdFluxLessRef = -420;
		}
		else
		{
			IqFluxLessRef = 0;
			IdFluxLessRef = -420;		
		}	
	}
	else
	{
		if( ADC1_Result[ADC1_RANK_VIN] < 3100) //55648mV  17.951  3100
		{
			IqFluxLessRef = Ref;
			if(IdFluxLessRef < (-4))
			{
				IdFluxLessRef += 4;
			}
			else IdFluxLessRef = 0;			
		}
		else if( ADC1_Result[ADC1_RANK_VIN] < 3310 ) //59417mV 3310
		{
			FOC_IqLim = 1050 - (ADC1_Result[ADC1_RANK_VIN] - 3100) * 5;
			IqFluxLessRef = Ref < FOC_IqLim ? Ref : FOC_IqLim;
			IdFluxLessRef -= 2;
			if(IdFluxLessRef < -420) IdFluxLessRef = -420;
		}
		else
		{
			IqFluxLessRef = 0;
			IdFluxLessRef = -420;		
		}	
	}
	
	UqVoltTmp =  PID_Regulator(IqFluxLessRef,\
														 IqFdb,\
			                       &PID_Torque_InitStructure); // 电流闭环输出q轴电压
	UqVoltFlt += ((UqVoltTmp << 9) - UqVoltFlt) >> 3;	

	/* 
		UqCal = DbSpdMotor*VMax*0.7/1020
		      = DbSpdMotor*VMax/(1020/0.7)
					= DbSpdMotor*VMax/1020
	*/
	#if 1    //加入前馈
	uint16_t Cal_K;
	Cal_K = (MC_MotorParam.Rate_Speed * 183) >> 7; //电机转速 * 1.43
	UqCal = ((int32_t)MotorSpeed * MAX_MODULE) / ((Cal_K < 1000) ? 1000 : Cal_K);
	UqVoltTmp = UqCal + (UqVoltFlt >> 9);
	#else    //去掉前馈
	UqVoltTmp = UqVoltFlt >> 9;
	#endif
	
	UqVoltTmp = (UqVoltTmp > ((int32_t)MAX_MODULE)) ? MAX_MODULE : UqVoltTmp;
	Stat_Volt_q_d.qV_Component1 = UqVoltTmp;

	UdVoltTmp = PID_Regulator(IdFluxLessRef,\
														IdFdb,\
	                          &PID_Flux_InitStructure);	// 电流闭环输出d轴电压
	
	UdVoltFlt	+= ((UdVoltTmp << 9) - UdVoltFlt) >> 3;		
	Stat_Volt_q_d.qV_Component2 =  UdVoltFlt >> 9;
																							
  //circle limitation
  RevPark_Circle_Limitation(&Stat_Volt_q_d); 		// 电压极限圈限制？ 会不会出现波动情况?
  /*Performs the Reverse Park transformation,
  i.e transforms stator voltages Vqs and Vds into Valpha and Vbeta on a 
  stationary reference frame*/
  Stat_Volt_alfa_beta = Rev_Park(Stat_Volt_q_d);
  /*Valpha and Vbeta finally drive the power stage*/ 
  SVPWM_3ShuntCalcDutyCycles(Stat_Volt_alfa_beta); //实际的电流输出控制  
}

void FOC_Enable(void)
{
  FOC_Status = FOC_Status_RUN;
}

void FOC_Disable(void)
{
  FOC_Status = FOC_Status_WAIT;
	Stat_Curr_q_d.qI_Component1 = 0;
	Stat_Curr_q_d.qI_Component2 = 0;
	Stat_Volt_q_d.qV_Component1 = 0;
	Stat_Volt_q_d.qV_Component2 = 0;
}