//********************************************************************************
//								     Include
//********************************************************************************
#include "MDSF05_SFR.h"
#include "XSFR_GLOBAL.h"
#include "Allhex.h"
#include <math.h>

//MOTOR_COMPONENT xdata Motor_Component;
//PMOTOR_COMPONENT xdata pMotor_Component =& Motor_Component;

//********************************************************************************
//								       RAM
//********************************************************************************
//The variables defined by data are placed in the first 128 bytes (0x00~0x7F) address space
//The variables defined by idata are placed in the (0x00~0xFF) address space
//The variable defined by xdata are placed in the external extended RAM  (generally refers to the external 0xF000~0xF1FF space, the specific space size varies with the MCU , and some MCU do not have external RAM)

//The variable defined by data is the fastest, followed by idata, and xdata is slower than the first two
//Variables that are often used are declared with data/idata; variables that are not commonly used are declared with xdata.

struct{
	unsigned char IR_Data_Decode_Finish:1;
	unsigned char IR_Decode_Header:1;
	unsigned char External1_TRIG:1;
	unsigned char Current_offset:1;			//1 = Current correction completed.	
	unsigned char CloseLoopFlag:1;
	unsigned char ZeroPointFlag:1;
	unsigned char ZeroPointFlagOld:1;
	unsigned char MOTOR_PWM_ENABLE:1;		//1 = PWM output
	unsigned char MOTOR_OFF_FLAG:1;			//1 = Order the motor to stop
	unsigned char Break_flag:1;
	unsigned char Counter_Function_sw:1;    //1 = Enable the timing function
	unsigned char Smart_Function_sw:1;		//1 = Enable the SMART function
	unsigned char RF_Learning_sw:1;			//1 = RF study done
	unsigned char Buzzer_SW:1;
}FLAG;
				
uint8   SpeedNum = 0;
// uint8 	SystemState = 0;
uint8 	MotorState = 0;
uint8 	MotorFaultState = 0;				//Stall status; 1=Over speed, 2=Under speed, 3=IQ too big, 4=IQ too small
uint8 	MotorDir = 0;						//0=Stanby, 1=CW, 2=CCW
xdata 	uint8 	CCWFlag = CW_CCW;			//0=CW, 1=CCW
xdata 	uint8 	CCWFlagOld = 0;

//Speed
int16 			SpeedCmd = 0;
int16 			SpeedCmdTemp = 0;
int16			Speed_Ibus_limit = 0;
idata 	int16 	EstimatedSpeed;

//Current
int16 	CurrentCmd = 0;
int16 	CurrentCmdTemp = 0;
#if (CURRENT_CONTROL == 1)
	int16 	CurrentCmdLimit = 0;
	int16 	CurrentComp = 0;
	// int16 	CurrentCmdLimit = IQ_MAX_LIMIT_VALUE;
#endif

#if (CW_CCW_TEST == 1)
	uint16 	t_counter1;
	bit 	CW_CCW_state;
#endif

uint16 	IbusOffset = 0;
uint16 	IbusTemp = 0;
uint16 	Ibus_avg = 0;
idata	uint16	Ibus_avg_FILTED  = 0;
idata	float	Ibus_In_AMPS  = 0;	

//Temperature
uint16 TemperatureTemp = 0;
uint16 Temperature_avg = 0;

//VSP
uint16 VspTemp;
uint16 Vsp_avg;

//uint16 VbusOffset;
uint16 Vbus_avg = 0;

xdata uint16 	CloseLoopDelayCount = 0;

xdata int16 	LatestTheta = 0;
xdata int16 	LatestThetaTemp = 0;

xdata int16 	TailwindSpeed = 0;

xdata uint8 	AngleState = 0;
xdata uint8 	AngleStateOld = 0;
xdata int8 		AngleStateCehckCount = 0;

xdata uint16 	BreakPwmDuty = 0;

//Count
xdata uint16 	AlignmentCount 			= 0;
xdata uint16 	RampDelayCount 			= 0;
xdata uint16 	TailwindCount 			= 0;
xdata uint16 	TailwindTimeOut 		= 0;
xdata uint8  	MotorStartRetryCount 	= 0;
xdata uint16 	MotorStartDelayCount 	= 0;
xdata uint16 	BumplessCount 			= 0;


//IQ,ID
xdata int16 	IqFb;
xdata int16 	IqPiOut;
xdata int16 	IqOutTemp = 0;
xdata int16 	IdOutTemp = 0;

//BEMF
xdata int16 BmfV;
xdata int16 BmfW;

xdata uint16 BmfV_old;
xdata uint16 BmfW_old;

xdata int16 BmfVW_Sub;
xdata uint16 abs_BmfVW_Sub;
xdata int16 BmfVW_Sum_Pos_ZeroPoint;
xdata int16 BmfVW_Sum_Neg_ZeroPoint;
xdata uint16 BmfBreakCount = 0;
xdata int8 ZeroPointDebounceCnt;
xdata int8 ZeroPointCnt;
xdata int8 ZeroPointCheck = 0;

//PLL
xdata int16 PllOutTemp = 0;

xdata uint16 check_break_time_cnt=0;
// xdata uint16 Ramp_Cnt = 0;

//RF
idata uint8 RF_Channel_temp = 0;
idata uint8 SpeedNum_temp = 0;

idata uint8 RF_Channel_old = 0;
idata uint8 SpeedNum_old = 0;

//DCR
#if (DCR_Strategy == 1)
	xdata int16 	vq_offset_ramp_values = 0;
	xdata int16 	theta_ramp_values = 0;
#endif

//SQUARE
#if (Square_Parking == 1)
	xdata uint8 	chkflow = 0;
	xdata uint16 	chkcnt = 0;
#endif

#if (POWER_LIMIT  == 1)
	xdata uint16 PowerLimitDelayCount = 0;
#endif

#if (POWER_CONTROL == 1)
	xdata uint16 PowerControlDelayCount = 0;
#endif

#if (POWER_CONTROL_USER_PI == 1)
	xdata uint16 UserPI_PowerControlDelayCount = 0;
#endif

#if ((POWER_CONTROL == 1) || (POWER_LIMIT == 1) || (POWER_CONTROL_USER_PI == 1))
	xdata uint16 Watt;
	xdata uint16 WattTemp;
	xdata uint16 Watt_InRef = WATT_LIMIT_VALUE;
	xdata uint16 WattCount;
#endif

xdata	uint16	ABS_break_time = 0;
xdata	uint16	do_vf_time = 0;
xdata	uint16	Motor_standstill_judgment_cnt = 0;
//********************************************************************************
//								   Motor parameter Init
//********************************************************************************
void MotorInit_Fun (void){
	
//IQ Init
	SFR_PAGE = 0;
	PI_CMD = 0;
	PI_FB = 0;
	PI_KP = 0;
	PI_KI = 0;
	PI_KT = Iq_Kt;
	PI_TR = 0;
	PI_UI = 0;
	PI_MAX = Iq_MaxLimit;
	PI_MIN = Iq_MinLimit;
	PI_OUT = 0;
	
//ID Init	
	SFR_PAGE = 1;
	PI_CMD = 0;
	PI_FB = 0;
	PI_KP = 0;
	PI_KI = 0;
	PI_KT = Id_Kt;
	PI_TR = 0;
	PI_UI = 0;
	PI_MAX = Id_MaxLimit;
	PI_MIN = Id_MinLimit;
	PI_OUT = 0;

//SPEED Init		
	SP_CYC = Spd_Cyc;	// Set to calculate CloseLoop after execute PWM cycle xx times. 
	SFR_PAGE = 2;
	PI_CMD = 0;
	PI_FB = 0;
	PI_KP = 0;
	PI_KI = 0;
	PI_KT = Spd_Kt;
	PI_TR = 0;
	PI_UI = 0;
	PI_MAX = Spd_MaxLimit;
	PI_MIN = -Spd_MinLimit;
	PI_OUT = 0;
	
//PLL Init	
	SFR_PAGE = 3;
	PI_CMD = 0;
	PI_FB = 0;
	PI_KP = Pll_Kp_TailWind;
	PI_KI = Pll_Ki;
	PI_KT = Pll_Kt;
	PI_TR = 0;
	PI_UI = 0;
	PI_MAX = Pll_MaxLimit;
	PI_MIN = Pll_MinLimit;
	PI_OUT = 0;
	
	MOTOR_CONT3 = MOTOR_CONT3_REGS;
	FG_CTRL = FG_CTRL_REGS;
	
//Motor SPEC Init	
	FS_MACRO(F);									//F
	GS_MACRO(G);									//G
	Z_CORR_MACRO(ZCorr);							//Z
	BAN_GAIN_MACRO(BanGain);						//BAN GAIN
	SMO_KSLIDE_MACRO(Kslide);						//SMO GAIN
	MAXSMC_ERR_MACRO(MaxSmcError);					//SMO ERR MAX
	
	SMO_FILTER_MACRO(Kslf);							//SMO FILTER
	ANG_BASE_MACRO(AngleBase << AngleBaseShift);	//ANG_BASE
	ANG_SHIFT_MACRO(0);								//ANG SHIFT

	SVPWM_AMP_MACRO(SVPWM_AMP_REGS); 				//SVPWM AMP
	
	PI_TMSR = 0x00;									// Enable Anti-Windup Mode
	
	FOCCONT &= ~(PLLEN);		// FOCCONT |= (PLLEN);
	MOTOR_CONT1 |= (FOCANGSEL);	// Enable SMO_Ang
	FOCCONT &= ~(SPEEDEN);
	
	#if ((CURRENT_CONTROL == 1)&&(SPEED_CONTROL == 0))
		MOTOR_CONT1 &= ~(IQINSEL);					// Disable SPEED_PI_OUT
		// CurrentCmd = IQ_CMD_VALUE;				//Final target current
		CurrentCmdTemp = IQ_FINAL_START_VALUE;		//Openloop end current
	#endif
	
	#if (SPEED_CONTROL == 1)
		MOTOR_CONT1 |= IQINSEL;						// Use SPEED_PI_OUT
	#endif
	
	#if (POWER_CONTROL == 1)
		InitPI(&PIParm_Watt);
	#endif
	
	MOTOR_CONT2 &= ~(0x10);
	
	IqOutTemp = 0;
	IdOutTemp = 0;
	
	BmfBreakCount = 4000;//by leo
	
	MotorDir = 0;
	FLAG.Break_flag = 0;
	ZeroPointCheck = 0;
	FLAG.ZeroPointFlag = 0;
	BmfVW_Sub = 0;

	ZeroPointCnt = 0;
	ZeroPointDebounceCnt = 0;
	BmfVW_Sum_Pos_ZeroPoint = 0;
	BmfVW_Sum_Neg_ZeroPoint = 0;
	
	
	LatestTheta = 0;
	RampDelayCount = 0;
	AngleStateCehckCount = 0;
	TailwindCount = 0;
	TailwindTimeOut = 0;
	AlignmentCount = 0;
	BreakPwmDuty = 0;
	FLAG.CloseLoopFlag = 0;
	BumplessCount = 0;
	
	FLAG.Break_flag = 0;
	MotorState = M_TAILWIND;
	#if (DCR_Strategy == 1)
		theta_ramp_values = 0;
	#endif
	BmfV_old = 0;
	BmfW_old = 0;
	ABS_break_time = 0;
}
//********************************************************************************
//								   	Speed command
//********************************************************************************	
#if ((RF_Control == 1)||(IR_Control == 1))
	void RF_IR_Fun(void){
		switch(RF_Channel){
			case R_OFF:
				if(MotorState==M_RUN){				//Has entered the outer ring
					FLAG.MOTOR_OFF_FLAG = 1;		//Execution stops,but pwm continues to output
				}
				else{
					FLAG.MOTOR_PWM_ENABLE = 0;		//PWM disable
				}
				#if (SPEED_CONTROL == 1)
					SpeedCmd = 0;
				#endif
					
				#if (CURRENT_CONTROL == 1)
					CurrentCmd = 0;
				#endif
					
				SPEED_UI_MACRO(Spd_MinLimit);	
				SpeedNum = 0;
				FLAG.Counter_Function_sw = 0;		// Timing function disable
				FLAG.Smart_Function_sw   = 0;		// SMART function disable
				Timer1Count_1s           = 0;
				Timer1Smart_1s           = 0;
				Timer1Count_1h			 = 0;
				Timer1Smart_2h           = 0;
				Timer1Count_user_timer   = 0;
				
				#if (dIPWM_MODE == 1)
					Buzzer_TIMES = IPWM_TIMES_OFF;
					IPWM_RUN_TIME = IPWM_RUN_TIME_OFF;
					IPWM_STOP_TIME = IPWM_STOP_TIME_OFF;
				#endif
			break;
			
			case R_1_SPEED:
				#if (SPEED_CONTROL == 1)
						SpeedCmd = IR_SPEED_1*POLE_PAIRS;
						if(SpeedNum_temp!=SpeedNum_old){
							SPEED_UI_MACRO(IR_SPEED_UI_1);
						}
				#endif
				
				#if (CURRENT_CONTROL == 1)
					CurrentCmd = IR_Current_1_VALUE;
				#endif
				
				SpeedNum = 1;
				FLAG.MOTOR_PWM_ENABLE = 1;			// Enable PWM output
				FLAG.MOTOR_OFF_FLAG = 0;
				
				#if (dIPWM_MODE == 1)
					Buzzer_TIMES = IPWM_TIMES_Others;
					IPWM_RUN_TIME = IPWM_RUN_TIME_Others;
					IPWM_STOP_TIME = IPWM_STOP_TIME_Others;
				#endif
				
				if(FLAG.Smart_Function_sw==1){
					Timer1Smart_1s = 0;
					Timer1Smart_2h = 0;
				}
			break;
			
			case R_2_SPEED:
				#if (SPEED_CONTROL == 1)
						SpeedCmd = IR_SPEED_2*POLE_PAIRS;
						if(SpeedNum_temp!=SpeedNum_old){
							SPEED_UI_MACRO(IR_SPEED_UI_2);
						}
				#endif
				
				#if (CURRENT_CONTROL == 1)
					CurrentCmd = IR_Current_2_VALUE;
				#endif
				
				SpeedNum = 2;
				FLAG.MOTOR_PWM_ENABLE = 1;
				FLAG.MOTOR_OFF_FLAG = 0;
				
				#if (dIPWM_MODE == 1)
					Buzzer_TIMES = IPWM_TIMES_Others;
					IPWM_RUN_TIME = IPWM_RUN_TIME_Others;
					IPWM_STOP_TIME = IPWM_STOP_TIME_Others;
				#endif
				
				if(FLAG.Smart_Function_sw==1){
					Timer1Smart_1s = 0;
					Timer1Smart_2h = 0;
				}
			break;
			
			case R_3_SPEED:
				#if (SPEED_CONTROL == 1)
						SpeedCmd = IR_SPEED_3*POLE_PAIRS;
						if(SpeedNum_temp!=SpeedNum_old){
							SPEED_UI_MACRO(IR_SPEED_UI_3);
						}
				#endif
				
				#if (CURRENT_CONTROL == 1)
					CurrentCmd = IR_Current_3_VALUE;
				#endif
				SpeedNum = 3;
				FLAG.MOTOR_PWM_ENABLE = 1;
				FLAG.MOTOR_OFF_FLAG = 0;
				
				#if (dIPWM_MODE == 1)
					Buzzer_TIMES = IPWM_TIMES_Others;
					IPWM_RUN_TIME = IPWM_RUN_TIME_Others;
					IPWM_STOP_TIME = IPWM_STOP_TIME_Others;
				#endif
				if(FLAG.Smart_Function_sw==1){
					Timer1Smart_1s = 0;
					Timer1Smart_2h = 0;
				}
			break;
			
			case R_4_SPEED:
				#if (SPEED_CONTROL == 1)
						SpeedCmd = IR_SPEED_4*POLE_PAIRS;
						if(SpeedNum_temp!=SpeedNum_old){
							SPEED_UI_MACRO(IR_SPEED_UI_4);
						}
				#endif
				
				#if (CURRENT_CONTROL == 1)
					CurrentCmd = IR_Current_4_VALUE;
				#endif
				SpeedNum = 4;
				FLAG.MOTOR_PWM_ENABLE = 1;
				FLAG.MOTOR_OFF_FLAG = 0;
				
				#if (dIPWM_MODE == 1)
					Buzzer_TIMES = IPWM_TIMES_Others;
					IPWM_RUN_TIME = IPWM_RUN_TIME_Others;
					IPWM_STOP_TIME = IPWM_STOP_TIME_Others;
				#endif
				if(FLAG.Smart_Function_sw==1){
					Timer1Smart_1s = 0;
					Timer1Smart_2h = 0;
				}
			break;
			
			case R_SMART:
				if(FLAG.MOTOR_PWM_ENABLE==1){
					FLAG.Smart_Function_sw = 1;
				}
				#if (dIPWM_MODE == 1)
					Buzzer_TIMES = IPWM_TIMES_Smart;
					IPWM_RUN_TIME = IPWM_RUN_TIME_Smart;
					IPWM_STOP_TIME = IPWM_STOP_TIME_Smart;
				#endif
			break;
			
			case R_3H:
				if(FLAG.MOTOR_PWM_ENABLE==1){
					FLAG.Counter_Function_sw = 1;
					Counter_timer = 3;
					Timer1Count_1s           = 0;
					Timer1Count_1h			 = 0;
					Timer1Count_user_timer   = 0;
				}
				#if (dIPWM_MODE == 1)
					Buzzer_TIMES = IPWM_TIMES_OFF;
					IPWM_RUN_TIME = IPWM_RUN_TIME_OFF;
					IPWM_STOP_TIME = IPWM_STOP_TIME_OFF;
				#endif
			break;
			
			case R_6H:
				if(FLAG.MOTOR_PWM_ENABLE==1){
					FLAG.Counter_Function_sw = 1;
					Counter_timer = 6;
					Timer1Count_1s           = 0;
					Timer1Count_1h			 = 0;
					Timer1Count_user_timer   = 0;
				}
				#if (dIPWM_MODE == 1)
					Buzzer_TIMES = IPWM_TIMES_OFF;
					IPWM_RUN_TIME = IPWM_RUN_TIME_OFF;
					IPWM_STOP_TIME = IPWM_STOP_TIME_OFF;
				#endif
			break;
			
			case R_9H:
				if(FLAG.MOTOR_PWM_ENABLE==1){
					FLAG.Counter_Function_sw = 1;
					Counter_timer = 9;
					Timer1Count_1s           = 0;
					Timer1Count_1h			 = 0;
					Timer1Count_user_timer   = 0;
				}
				#if (dIPWM_MODE == 1)
					Buzzer_TIMES = IPWM_TIMES_OFF;
					IPWM_RUN_TIME = IPWM_RUN_TIME_OFF;
					IPWM_STOP_TIME = IPWM_STOP_TIME_OFF;
				#endif
			break;
		}
		SpeedNum_temp = SpeedNum;
		RF_Channel_temp = RF_Channel;
		RF_Channel = 0;
	}
#endif
#if (VSP_TRI == 1)
	void Vsp_Fun (void){
		#if (dSlope_control == 1)
			if(Vsp_avg > CMD_START_VAL){
		#endif
		
		#if (dStage_control == 1)
			if(Vsp_avg > 102){
		#endif
		
			if(FLAG.MOTOR_PWM_ENABLE==0)
				FLAG.MOTOR_PWM_ENABLE = 1;
				
			#if (CURRENT_CONTROL == 1)
				CurrentCmd = (int)((float) Vsp_avg * IQ_GAIN);
			#endif
			
			#if (SPEED_CONTROL == 1)
				#if (dStage_control == 1)
					if(Vsp_avg > 102 && Vsp_avg<=240){
						SpeedCmd = VSP_SPEED_1*POLE_PAIRS;
						SpeedNum = 1;
					}
					else if(Vsp_avg>=250 && Vsp_avg<=490){
						SpeedCmd = VSP_SPEED_2*POLE_PAIRS;
						SpeedNum = 2;
					}
					else if(Vsp_avg>=500 && Vsp_avg<=740){
						SpeedCmd = VSP_SPEED_3*POLE_PAIRS;
						SpeedNum = 3;
					}
					else if(Vsp_avg>=750 && Vsp_avg<=1000){
						SpeedCmd = VSP_SPEED_4*POLE_PAIRS;
						SpeedNum = 4;
					}
					SpeedNum_temp = SpeedNum;
				#endif
				
				#if (dSlope_control == 1)
					SpeedCmd = ((SPEED_STEP * (Vsp_avg - CMD_START_VAL)) / 1023) + SPEED_MIN_LIMIT_VALUE;	
				#endif
			#endif
		}
		else{
			#if (dSlope_control == 1)
				if(Vsp_avg < CMD_STOP_VAL){
			#endif
		
			#if (dStage_control == 1)
				if(Vsp_avg < 80){
			#endif
		
				if(MotorState == M_RUN){
					#if (CURRENT_CONTROL == 1)
						if(CurrentCmd > 100)
							CurrentCmd -= 10;
					#endif
					
					#if (SPEED_CONTROL == 1)
						if(SpeedCmd > SPEED_MIN_LIMIT_VALUE)
							SpeedCmd -= 10;
					#endif
					
					else{
						FLAG.MOTOR_PWM_ENABLE = 0; // Motor Stop
						MotorErrorState = 0;
						CurrentCmd = 0;
						SpeedCmd = 0;
						SpeedNum = 0;
					}
				}
				else{
					FLAG.MOTOR_PWM_ENABLE = 0;
					MotorErrorState = 0;
					CurrentCmd = 0;
					SpeedCmd = 0;
					SpeedNum = 0;
				}
			}
		}
	}
#endif
//********************************************************************************
//								   Motor_Control
//********************************************************************************
void Motor_Control (void){
	#if (CW_CCW_TEST == 1)	//Forward and reverse test
		switch(CW_CCW_state){
			
			case 0:
				 t_counter1++;
				 if(t_counter1==Reversing_TIMES){			//Running time after starting in a different direction
					 t_counter1 = 0;
					 PWM_SetAllOff();
					 ResetMOC();
					 CW_CCW_state = 1;
				 }
			break;
			
			case 1:
				t_counter1++;
				if(t_counter1==STOP_TIMES_TO_CW){			//Stop this time before connecting to the downwind
					t_counter1 = 0;
					MotorState = M_INIT;
					CW_CCW_state = 0;
				}
			break;
			
			case 2:
				t_counter1++;
				if(t_counter1==CW_RUN_TIME){				//Operation time after connecting with downwind
					t_counter1 = 0;
					CCWFlag^=1;
					CW_CCW_state = 0;
				}
			break;
		}
	#endif
	
	if(CCWFlag != CCWFlagOld)				//When there is a forward and reverse rotation change, switch the motor state back to Init and restart
		MotorState = M_INIT;
	CCWFlagOld = CCWFlag;
	
	if(MotorErrorState){						//Wrong
		if(MotorState != M_OFF)
			ResetMOC();							//Clear register value of PI-Control.
		PWM_SetAllOff();						//PWM ALL OFF.
		MotorStartRetry_Flow();
		
		// #if (dIPWM_MODE == 1)
			// FLAG.Buzzer_SW = 1;
			// Buzzer_TIMES = 1;
			// IPWM_RUN_TIME = IPWM_RUN_TIME_Err;
			// IPWM_STOP_TIME = IPWM_STOP_TIME_Err;
		// #endif
		
		#if (dLED_MODE == 1)
			LED_Flashing();
		#endif
		
		#if ((FG_CTRL_REGS & 0x80) == 0x80)
			FG_DISABLE;
		#endif
	}
	else if(FLAG.MOTOR_PWM_ENABLE==1){		// Start
		MotorStart_Flow();
		#if (dLED_MODE == 1)
			LED_OUT = 0;
		#endif
	}
	else{
		if(MotorState != M_OFF)
			ResetMOC();
		
		PWM_SetAllOff();
		MotorStartRetryCount = 0;
		MotorErrorState = Clear;
		MotorState = M_OFF;
		#if ((FG_CTRL_REGS & 0x80) == 0x80)
			FG_DISABLE;
		#endif
	}
}
//********************************************************************************
//							 MotorStartRetry_Flow
//********************************************************************************
void MotorStartRetry_Flow (void){
	if(													//This error determines that there is a limit on the number of restarts
		#if (AOCP_Retry_ENABLE == 1)
			((MotorErrorState & AOCP) == AOCP)||
		#endif
		
		#if (POCP_Retry_ENABLE == 1)
			((MotorErrorState & POCP) == POCP)||
		#endif
		
		#if (OVP_Retry_ENABLE == 1)
			((MotorErrorState & OverVbus) == OverVbus)||
		#endif
		
		#if (LVP_Retry_ENABLE == 1)
			((MotorErrorState & UnderVbus) == UnderVbus)||
		#endif
		
		#if (OTP_Retry_ENABLE == 1)
			((MotorErrorState & OverTemperature) == OverTemperature)||
		#endif
		
		#if (FaultLock_Retry_ENABLE == 1)
			((MotorErrorState & FaultLock) == FaultLock)||
		#endif
		0)
	{
		MotorStartDelayCount++;
		if(MotorStartDelayCount >= RESTART_DURATIONT){
			MotorStartDelayCount = 0;
			MotorStartRetryCount++;
			if(MotorStartRetryCount >= RETRY_COUNT){
				MotorState = M_ERROR;	//Need to power off and restart				
			}
			else{
				MotorState = M_OFF;						
				MotorErrorState = Clear;
			}
		}
	}	
	else{														
		MotorState = M_ERROR;	//Need to power off and restart	
	}
}
//********************************************************************************
//								   MotorStart_Flow
//********************************************************************************
void MotorStart_Flow (void){
	unsigned int dIQ_OUT;
	#if (SPEED_CONTROL == 1)
		unsigned int SPEED_UI;
	#endif
	
	switch (MotorState){
		case M_INIT:
			PWM_SetBreakForce();
			PWM_SetAllOff();
			ResetMOC();
			MotorInit_Fun();
			if(CCWFlag)
				MOTOR_CONT2 |= 0x08;		//CCW
			else
				MOTOR_CONT2 &= ~(0x08);		//CW
		break;

		case M_TAILWIND:
			#if (BEMF_TAILWIND_RES_FUNCTION == 1) || (BEMF_TAILWIND_DIODE_FUNCTION == 1) 
				#if (BEMF_TAILWIND_SOP == 2)
				//if(MotorDir == 0)
				{
					if(TailwindTimeOut > TAILWIND_DURATION){
						TailwindTimeOut = 0;
						MotorDir = 0;
						MotorState = M_IPD;
					}
					else
						TailwindTimeOut++;
				}
				#endif
			#endif
			
			#if (TAILWIND_ESTIMATED_FUNCTION == 1)			
				Break_Fun();
				TailWindDetect_Fun();
			#endif
			
			#if ((TAILWIND_ESTIMATED_FUNCTION == 0) && (BEMF_TAILWIND_RES_FUNCTION == 0) && (BEMF_TAILWIND_DIODE_FUNCTION == 0))
				MotorState = M_IPD;
			#endif
			
		break;
			
		case M_IPD:
			#if (IPD_FUNCTION == 1)
				IPDDetect_Fun();
			#else
				if(MotorDir == 0){				//The motor is judged to be stationary or low-speed forward/backward
					if(FLAG.Break_flag==1){		//Confirm that the motor is not stationary
						ABS_break_time++;
						if(ABS_break_time<=1000)
							PWM_SetBreakForce();
						else if(ABS_break_time<=2000){
							PWM_SetAllOff();
							BmfVW_Sub = BmfV - BmfW; 
							abs_BmfVW_Sub = abs(BmfVW_Sub);
							if(abs_BmfVW_Sub<3)
								Motor_standstill_judgment_cnt+=1;
							else
								Motor_standstill_judgment_cnt = 0;
						}
						else{
							if(Motor_standstill_judgment_cnt>=dMotor_standstill_judgment_cnt){
								Motor_standstill_judgment_cnt = 0;
								#if (DCR_Strategy == 1)||(Square_Parking == 1)
									MotorState = M_Parking;
								#else
									MotorState = M_START;
								#endif
								LatestTheta = 0<<6;		//Initial Angle(0~383)
							}
							else{
								BmfV_old = 0;
								BmfW_old = 0;
								ABS_break_time = 0;
							}
						}
					}
					else{	//Stanby
						#if (DCR_Strategy == 1)||(Square_Parking == 1)
							MotorState = M_Parking;
						#else
							MotorState = M_START;
						#endif
						LatestTheta = 0<<6;		//Initial Angle(0~383)
					}
				}
				else{
					FLAG.Break_flag = 0;
					LatestTheta = 0<<6;		//Initial Angle(0~383)
				}
			#endif
		break;
		
		case M_BMF_BREAK:
			if(BmfBreakCount > 1){
				BmfBreakCount--;
				PWM_SetActive();
			}
			else{
				PWM_SetAllOff();
				MotorState = M_TAILWIND;
			}
		break;
		
		case M_Parking:
			#if (DCR_Strategy == 1)
				if(MotorDir == 0){
					FOCCONT &= ~(PLLEN);	//PLL Disable.
					PLL_KP_MACRO(0);
					PLL_KI_MACRO(0);
					PLL_UI_MACRO(0);
					PLL_OUT_MACRO(0);
					
					IQ_KI_MACRO(0);
					IQ_KP_MACRO(0);
					IQ_UI_MACRO(0);
					IQ_OUT_MACRO(0);
						
					ID_KI_MACRO(0);
					ID_KP_MACRO(0);
					ID_UI_MACRO(0);
					ID_OUT_MACRO(0);
						
					VD_OFFSET_MACRO(0);

					if(theta_ramp_values<=12256){//0~180deg
						vq_offset_ramp_values = LERP(theta_ramp_values,0,dDCR_V_min_values,24512,dDCR_V_max_values);
							
						VQ_OFFSET_MACRO(vq_offset_ramp_values);
						FOC_ANG_MACRO(theta_ramp_values);
						CPU_ANG_MACRO(theta_ramp_values>>6);
							
						theta_ramp_values += dDCR_Zone1_percentage;
						PWM_SetActive();					
					}
					else if(theta_ramp_values<=24512){//180~360deg
						vq_offset_ramp_values = LERP(theta_ramp_values,0,dDCR_V_min_values,24512,dDCR_V_max_values);
							
						VQ_OFFSET_MACRO(vq_offset_ramp_values);
						FOC_ANG_MACRO(theta_ramp_values);
						CPU_ANG_MACRO(theta_ramp_values>>6);
							
						theta_ramp_values += dDCR_Zone2_percentage;
					}
					else{
						theta_ramp_values = 24513;
						vq_offset_ramp_values = 0;
						VQ_OFFSET_MACRO(0);
						FOC_ANG_MACRO(0);
						CPU_ANG_MACRO(0);
						PWM_SetAllOff();
						MotorState = M_START;
					}	
				}
			#endif
			
			#if (Square_Parking == 1)
				switch(chkflow){
					case 0:
						if(check_break_time_cnt<300){
							check_break_time_cnt++;
							PWM_SetBreakForce();
						}
						else{
							PWM_SetAllOff();
							check_break_time_cnt=0;
							chkflow = 1;
						}
					break;
										 
					case 1:
						MOTOR_CONT1 &= ~(MPWMSEL);	//User Mode.
						chkflow = 2;
					break;
									
					case 2:
						SwitchPhase(2);
						PWMSet(2);
						chkflow = 3;
					break;
										
					case 3:
						if(++chkcnt > USER_START_DELAY)
							chkflow = 4;
					break;
										
					case 4:
						SwitchPhase(8);
						PWMSet(8);
						PWM_SetAllOff();
						chkcnt=0;
						chkflow=0;
						check_break_time_cnt=0;
						MotorState = M_START;
					break;
				}  					
			#endif
		break;
		case M_START:
			if(MotorDir == 0)
				IPDStart_Fun();
			else
				TailWindStart_Fun();
		break;
			
		case M_RUN:
			#if (CURRENT_CONTROL == 1)
				#if (POWER_CONTROL_USER_PI == 0)
					// GET_IQ_OUT_MACRO(dIQ_OUT);
					
					// if(dIQ_OUT>25000){
						// IQ_KI_MACRO(20);
						// ID_KI_MACRO(20);
					// }
					
					CurrentCmdTemp = CurrentCmd;
					CurrentComp = CurrentCmdTemp + CurrentCmdLimit;
					if(CurrentComp>=2000)
						CurrentComp = 2000;
					else if(CurrentComp<=0)
						CurrentComp = 0;
					IQ_CMD_MACRO(CurrentComp);
					
	//				RampDelayCount++;
	//				if(RampDelayCount >= 10){
	//					RampDelayCount = 0;
	//					IdOutTemp = Ramp_Fun(ID_FINAL_START_VALUE,IdOutTemp,32767,-32767,1);
	//					ID_CMD_MACRO(IdOutTemp);
	//				}
					// #if (RF_Control == 1)
						// if(SpeedNum_temp!=SpeedNum_old){
							// SpeedNum_old = SpeedNum_temp;	
						// }
					// #endif
				#endif
			#endif
			
			#if (SPEED_CONTROL == 1)
				#if (BUMPLESS_FUNCTION == 1)
					if(MotorDir != 1){
						if(BumplessCount < BUMPLESS_DURATION){
							BumplessCount++;
							if(BumplessCount == BUMPLESS_DURATION){
								PI_TMSR &= ~(SPEEDTMSEL);
								PI_TMSR &= ~(SPEEDTMEN);
							}
						}	
					}
				#endif
				
				//Preventing SPEED_ UI = SPEED_ limit_ At min, the UI will saturate
				GET_SPEED_UI_MACRO(SPEED_UI);
				if(SPEED_UI==Spd_MinLimit){
					SPEED_UI_MACRO(Spd_MinLimit+1);
				}
				
				#if (BUMPLESS_FUNCTION == 1)
					if(BumplessCount == BUMPLESS_DURATION||(MotorDir!= 0))
				#endif
				
				{
					
					if(SpeedNum_temp!=SpeedNum_old){
						SpeedNum_old = SpeedNum_temp;
						// SPEED_UI_MACRO(0);
						#if (RF_Control == 1)
							if(SpeedNum==1){
								SPEED_UI_MACRO(IR_SPEED_UI_1);
							}
							else if(SpeedNum==2){
								SPEED_UI_MACRO(IR_SPEED_UI_2);
							}
							else if(SpeedNum==3){
								SPEED_UI_MACRO(IR_SPEED_UI_3);
							}
							else if(SpeedNum==4){
								SPEED_UI_MACRO(IR_SPEED_UI_4);	
							}
							else if(SpeedNum==0){
								SPEED_UI_MACRO(Spd_MinLimit);
							}								
						#endif
						
						#if (VSP_TRI == 1)
							#if (dStage_control == 1)
								if(SpeedNum==1){
									SPEED_UI_MACRO(VSP_SPEED_UI_1);
								}
								else if(SpeedNum==2){
									SPEED_UI_MACRO(VSP_SPEED_UI_2);
								}
								else if(SpeedNum==3){
									SPEED_UI_MACRO(VSP_SPEED_UI_3);
								}
								else if(SpeedNum==4){
									SPEED_UI_MACRO(VSP_SPEED_UI_4);
								}									
								else if(SpeedNum==0){
									SPEED_UI_MACRO(Spd_MinLimit);
								}									
							#endif
						#endif
					}
				}
				RampDelayCount++;
				if(RampDelayCount >= SPEED_RAMP){
					RampDelayCount = 0;
					if(SpeedCmdTemp <= SpeedCmd)
						SpeedCmdTemp += SPEED_ACC;
					else
						SpeedCmdTemp -= SPEED_DEC;
					
					if(SpeedCmdTemp >= SPEED_MAX_LIMIT_VALUE)
						SpeedCmdTemp = SPEED_MAX_LIMIT_VALUE;	
					else if(SpeedCmdTemp<=SPEED_MIN_LIMIT_VALUE)
						SpeedCmdTemp = SPEED_MIN_LIMIT_VALUE;	
						
					IdOutTemp = Ramp_Fun(ID_FINAL_START_VALUE,IdOutTemp,32767,-32767,8);
					ID_CMD_MACRO(IdOutTemp);
				}
				Speed_Ibus_limit = 0;
				SPEED_CMD_MACRO(SpeedCmdTemp + Speed_Ibus_limit);
			#endif
				// Motor Stop Fun
				if(FLAG.MOTOR_OFF_FLAG==1){		//Command to close the motor, but do not close the PWM output at this time
					if(EstimatedSpeed < STOP_SPEED_VALUE){
						// SystemState = 0;
						// ResetMOC();
						// PWM_SetAllOff();
						MotorState = M_INIT;
						FLAG.MOTOR_OFF_FLAG = 0;
						FLAG.MOTOR_PWM_ENABLE = 0;		//Disable PWM output
					}
				}
		break;
		default:
			ResetMOC();
			PWM_SetAllOff();
			MotorState = M_INIT;
		break;
	}
}
//********************************************************************************
//								     Closeloop
//********************************************************************************
void MotorCloseLoop_Fun (void){
	#if (SPEED_CONTROL == 1)
		short Estimated_Spd;
	#endif
	// #if(SPEED_CONTROL == 1)
		// #if(BUMPLESS_FUNCTION == 1)
			// short Temp1,RealSpeed;
		// #endif
	// #endif
	
	if(FLAG.CloseLoopFlag == 1){
		PLL_KP_MACRO(Pll_Kp_Final);
		PLL_KI_MACRO(Pll_Ki);
		IQ_KP_MACRO(Iq_Kp);
		// IQ_KI_MACRO(Iq_Ki);
		ID_KP_MACRO(Id_Kp);
		// ID_KI_MACRO(Id_Ki);
		
		IQ_KI_MACRO(Iq_Ki);
		ID_KI_MACRO(Id_Ki);
		
		#if (CURRENT_CONTROL == 1)
			IQ_KP_MACRO(8000);
			ID_KP_MACRO(8000);
			IQ_KI_MACRO(160);
			ID_KI_MACRO(160);
		#endif
		#if (SPEED_CONTROL == 1)
			#if (BUMPLESS_FUNCTION == 1)
				if(MotorDir != 1){
					SFR_PAGE = 2; PI_TR = IQ_FINAL_START_VALUE;// IQ to Speed
					
	//				SFR_PAGE = 0; Temp1 = PI_FB;// IQ
	//				SFR_PAGE = 2; PI_TR = Temp1;// IQ to Speed
	//				SFR_PAGE = 4; RealSpeed = SMO_D2;// SMO_Speed 
	//				SFR_PAGE = 3; PI_UI = RealSpeed;// SMO_Speed to PLL_Speed_UI
					PI_TMSR |= SPEEDTMSEL;
					PI_TMSR |= SPEEDTMEN;
				}
			#else
				SPEED_UI_MACRO(IQ_FINAL_START_VALUE);
				SPEED_OUT_MACRO(IQ_FINAL_START_VALUE);
				// SPEED_UI_MACRO(0);
				// SPEED_OUT_MACRO(0);
				//ID_UI_MACRO(0);
				//ID_OUT_MACRO(0);
			#endif
			
			FOCCONT |= SPEEDEN;
			MOTOR_CONT1 |= FOCANGSEL;// Use SMO_Ang
			MOTOR_CONT1 |= IQINSEL;// Use SPEED_PI_OUT
			SPEED_KP_MACRO(Spd_Kp_Final);
			SPEED_KI_MACRO(Spd_Ki);
			// SpeedCmdTemp = SPEED_FINAL_VALUE;
			if(MotorDir==1){
				// if(CaptureTotalTemp<4000)
					// SpeedCmdTemp = SPEED_FINAL_START_VALUE2;	//250
				// else if(CaptureTotalTemp<8000)
					// SpeedCmdTemp = SPEED_FINAL_START_VALUE1;	//200
				// else
					// SpeedCmdTemp = SPEED_FINAL_START_VALUE;
			}
			else{					//Because the current will drop to 0 during the connection of the TR route in the open-loop, the SPD_ CMD gives an initial value
				GET_SPEED_MACRO(Estimated_Spd);
				SpeedCmdTemp = Estimated_Spd + 300;		//300 is the best connection value tested
			}
			SPEED_CMD_MACRO(SpeedCmdTemp);
		#endif
		
		FOCCONT |= PLLEN;
		ID_CMD_MACRO(ID_FINAL_START_VALUE);
		MOTOR_CONT2 |= MOTOR_CONT2_REGS;
		MotorState = M_RUN;
	}
}
//********************************************************************************
//							  	The motor judges the wind
//********************************************************************************
#if (BEMF_TAILWIND_RES_FUNCTION == 1) || (BEMF_TAILWIND_DIODE_FUNCTION == 1)
void TailWindDetect_TwoBEMF_Fun(void){
	//First, determine whether the current motor is following or facing the wind
	if(!ZeroPointCheck){
		BmfVW_Sub = BmfV - BmfW; 
		abs_BmfVW_Sub = abs(BmfVW_Sub);
		
		// if(BmfV>BEMF_BREAK_VALUE+2||BmfW>BEMF_BREAK_VALUE+2){
			// FLAG.Break_flag = 1;									//The motor is not in a stationary state
		// }
		if(abs_BmfVW_Sub>10)
			FLAG.Break_flag = 1;
		
		if((BmfVW_Sub > -10) && (FLAG.ZeroPointFlag == 0)){			//Two phase subtraction, positive edge zero crossing
			if(++ZeroPointDebounceCnt > 10){
				ZeroPointDebounceCnt = 0;
				//if(FLAG.ZeroPointFlag == 0)
					FLAG.ZeroPointFlag = 1;
			}
		}
		else if((BmfVW_Sub < -10) && (FLAG.ZeroPointFlag == 1)){	//Two phase subtraction, negative edge zero crossing
			if(++ZeroPointDebounceCnt > 10){
				ZeroPointDebounceCnt = 0;
				//if(FLAG.ZeroPointFlag == 1)
					FLAG.ZeroPointFlag = 0;
			}
		}		
		// HallUEdgeCnt++;
		// if(HallUEdgeCnt >= PWM_Frequency){					// 1Hz = 20000 * 50us 
			// HallUEdgeCnt = PWM_Frequency;
			// RecHallUEdgeCnt = HallUEdgeCnt;
			// ZeroPointCnt = 0;
		// }
		
		if(FLAG.ZeroPointFlag != FLAG.ZeroPointFlagOld){
			TailwindTimeOut = 0;
			if(FLAG.ZeroPointFlag == 1){					//Zero crossing of positive edge
				SFR_PAGE = 1; CAPCONT |= 0x10;
				SFR_PAGE = 1; CaptureTotal = (CAPT_H * 256) + CAPT_L;	//Open Capture to calculate speed
				CaptureTotalTemp = CaptureTotal;
				BmfVW_Sum_Pos_ZeroPoint = BmfV + BmfW;					//BmfVW_Sum_Pos_ZeroPoint = When the waveform of the two phase subtraction passes through the positive edge, the two phase addition value at this time
				if(BmfVW_Sum_Pos_ZeroPoint > BmfVW_Sum_Neg_ZeroPoint){
					ZeroPointCnt++;
					if(ZeroPointCnt > 4){			//Judge times
						ZeroPointCnt = 0;

						if (CCWFlag == 1)
							MotorDir = 1;			//CW
						else
							MotorDir = 2;			//CCW
							
						TailwindTimeOut = 0;
					}
				}
				else if(BmfVW_Sum_Pos_ZeroPoint < BmfVW_Sum_Neg_ZeroPoint){
					ZeroPointCnt--;
					if(ZeroPointCnt < -4){			//Judge times
						ZeroPointCnt = 0;

						if (CCWFlag == 1)
							MotorDir = 2;			//CCW
						else
							MotorDir = 1;			//CW
							
						TailwindTimeOut = 0;
					}
				}
			}
			else{//Negative edge zero crossing								
				SFR_PAGE = 1; CAPCONT &= ~(0x10);
				// RecHallUEdgeCnt = HallUEdgeCnt;
				// HallUEdgeCnt = 1;
				BmfVW_Sum_Neg_ZeroPoint = BmfV + BmfW;				//BmfVW_Sum_Neg_ZeroPoint = When the waveform of the two phase subtraction passes through the negative edge, the two phase addition value at this time
//*************************************************************************************************************************************				
	//After confirming the wind direction, fill in the current electrical angle
				if(MotorDir == 1){	//Confirmed as tailwind			
					#if (BEMF_TAILWIND_DIODE_FUNCTION == 1)
						if (CCWFlag == 1)
							LatestTheta = (short)CW_TAILWIND_DIODE_ANG<<6;	//90 degree
						else
							LatestTheta = (short)CCW_TAILWIND_DIODE_ANG<<6;	//120 degree
					#endif
					#if (BEMF_TAILWIND_RES_FUNCTION == 1)
						if (CCWFlag == 1)
							LatestTheta = (short)CW_TAILWIND_RES_ANG<<6;	//0 degree
						else
							LatestTheta = (short)CCW_TAILWIND_RES_ANG<<6;	//60 degree
					#endif
	//Take the value from Capture and confirm the motor speed range to be connected to the downwind direction
	
					/*	CaptureTotalTemp formula:
						Capture clock = 48MHZ/512 = 93750Hz
						93750Hz/32767 = 2.8Hz(Minimum speed can be read)
					*/
					if((CaptureTotalTemp < DET_Tailspeed_min_speed)&&(CaptureTotalTemp>DET_Tailspeed_max_speed)){
					/* SPEED_VALUE formula
						SPEED_VALUE = (48MHZ/512)*60 = 93750*60
						F = 93750/CaptureTotalTemp
						rpm = PLL_OUT * POLE_PAIRS = (F*60)/POLE_PAIRS ; PLL_OUT = rpm/POLE_PAIRS
						PLL_OUT = [(93750/CaptureTotalTemp)*60 / POLE_PAIRS] / POLE_PAIRS
								= (93750/CaptureTotalTemp)*60
						
					*/
	//Convert the value obtained by Capture into angular velocity and fill it in PLL
						#if ((SOP_LEVEL == 1) || (SOP_LEVEL == 2))
							PllOutTemp = ((SPEED_VALUE / CaptureTotalTemp) >> AngleBaseShift);//Convert frequency to PLL speed
							ZeroPointCheck = 1;
						#endif
					}
				}
				else if(MotorDir == 2){	//Confirmed as Headwind
					#if (BEMF_TAILWIND_DIODE_FUNCTION == 1)
						if (CCWFlag == 1)
							LatestTheta = (short)288<<6;
						else
							LatestTheta = (short)320<<6;
					#endif
					#if (BEMF_TAILWIND_RES_FUNCTION == 1)
						if (CCWFlag == 1)
							LatestTheta = (short)192<<6;	
						else
							LatestTheta = (short)288<<6;	
					#endif
				
					if(CaptureTotalTemp > DET_Headspeed_max_speed){
						#if ((SOP_LEVEL == 1) || (SOP_LEVEL == 2))
							#if (Headwind_High_Speed_Start_Strategy==1) //PLL
								PllOutTemp = ((SPEED_VALUE / CaptureTotalTemp) >> AngleBaseShift);
								PllOutTemp = -PllOutTemp;
								ZeroPointCheck = 1;
							#endif
							#if (Headwind_High_Speed_Start_Strategy==2)	//ABS
								PWM_SetBreakForce();
							#endif
						#endif
					}
					else{
						
					}
					// else if((CaptureTotalTemp < 1000)){
						// BmfBreakCount = 100;
						// MotorState = M_BMF_BREAK;
					// }
				}
			}
		}
		FLAG.ZeroPointFlagOld = FLAG.ZeroPointFlag;
//****************************************************************************************************************************
	//Fill in the obtained electrical angle/angular velocity and other parameters, as well as initial values such as PI for connection
		if(ZeroPointCheck){
			if(MotorDir==1){
				PLL_KP_MACRO(Pll_Kp_Start);
				PLL_KI_MACRO(Pll_Ki);
				ID_KP_MACRO(Id_Kp);
				ID_KI_MACRO(Id_Ki);
			}
			else if(MotorDir==2){
				PLL_KP_MACRO(0);	//Like PLL_ If KP/KI is given, then PLL_ OUT will oscillate and rise upwards
				PLL_KI_MACRO(0);
				ID_KP_MACRO(0);
				ID_KI_MACRO(0);
			}
			IQ_KP_MACRO(Iq_Kp);
			IQ_KI_MACRO(Iq_Ki);

			MOTOR_CONT1 |= (FOCANGSEL);// Use SMO_Ang

			#if (CURRENT_CONTROL == 1)
				IQ_CMD_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
				//ID_CMD_MACRO(ID_FINAL_TAILWIND_START_VALUE);
				IQ_UI_MACRO(BEMF_TAILWIND_IQ_OUT_VALUE);
				IQ_OUT_MACRO(BEMF_TAILWIND_IQ_OUT_VALUE);
				ID_UI_MACRO(0);
				ID_OUT_MACRO(0);
			#endif
			
			#if (SPEED_CONTROL == 1)
				SPEED_UI_MACRO(IQ_FINAL_TAILWIND_START_VALUE);	//2000
				SPEED_OUT_MACRO(IQ_FINAL_TAILWIND_START_VALUE);	//2000
				//ID_CMD_MACRO(ID_FINAL_TAILWIND_START_VALUE);
				IQ_UI_MACRO(BEMF_TAILWIND_IQ_OUT_VALUE);	//8000
				IQ_OUT_MACRO(BEMF_TAILWIND_IQ_OUT_VALUE);	//8000
				ID_UI_MACRO(0);
				ID_OUT_MACRO(0);
			#endif
			
			RampDelayCount = 0;
			CloseLoopDelayCount = 0;
			FOCCONT |= PLLEN;
			MOTOR_CONT1 |= (MPWMSEL);// Enable PWM Smo Mode
			MOTOR_CONT2 |= MOTOR_CONT2_REGS;
			PWM_SetActive();
			FOC_ANG_MACRO(LatestTheta);
			CPU_ANG_MACRO(LatestTheta>>6);
			PLL_UI_MACRO(PllOutTemp);//PllOutTemp
			PLL_OUT_MACRO(PllOutTemp);
			MotorState = M_START;
		}
	}
}
#endif
//********************************************************************************
//								 Estimator detects BEMF
//********************************************************************************
#if (TAILWIND_ESTIMATED_FUNCTION == 1)		//Estimator detection
void TailWindDetect_Fun (void){
	/* Static downwind and headwind judgment */
	//SFR_PAGE = 4; TailwindSpeed = SMO_D2;
	SFR_PAGE = 3; TailwindSpeed = PI_OUT;	//PLL
	
	SFR_PAGE = 4; LatestTheta = SMO_D1;		//SMO-Angle
	LatestThetaTemp = LatestTheta;
	LatestTheta -= (191<<6);// Offer the estimated angle to -180~180 degrees
	
	#if (BEMF_TAILWIND_SOP == 2)
	if(TailwindTimeOut > (TAILWIND_DURATION << 1)){
		TailwindTimeOut = 0;
		MotorDir = 0;
		MotorState = M_IPD;
	}
	else
		TailwindTimeOut++;
	
	if(AngleStateOld == AngleState){
		TailwindCount++;
		if(TailwindCount >= TAILWIND_DURATION){
			TailwindCount = 0;
			TailwindTimeOut = 0;
			MotorDir = 0;
			MotorState = M_IPD;
		}
	}
	else{
		TailwindCount = 0;
	}
	#endif
	AngleStateOld = AngleState;
	
	// Determine tailwind status
	if(LatestTheta < (-160<<6)){// When less than -170 degrees
		AngleState = 1;
	}
	else if((LatestTheta > (-10+50<<6)) && (LatestTheta < (10+50<<6))){// At -10 to 10 degrees
		if((AngleState == 1) || (AngleState == 3)){
			if(AngleState == 1)
				MotorDir = 1;// Tailwind CW
			else if(AngleState == 3)
				MotorDir = 2;// Headwind CCW
			
			if((MotorDir == 2) && (abs(TailwindSpeed) > SPEED_TAILWIND_VALUE)){
				//TailwindCount = 0;// Headwind Status Reset TailwindCount = 0;
				if(AngleStateCehckCount < -2){// Perform tailwind start after 2 judgments
					AngleStateCehckCount = 0;
					//FOC_ANG_MACRO(LatestThetaTemp);// Compensation angle
					
					PllOutTemp = TailwindSpeed;
					PLL_KP_MACRO(Pll_Kp_Start);
					PLL_KI_MACRO(Pll_Ki);
					IQ_KP_MACRO(Iq_Kp);
					IQ_KI_MACRO(Iq_Ki);
					ID_KP_MACRO(Id_Kp);
					ID_KI_MACRO(Id_Ki);
					
					MOTOR_CONT1 |= (FOCANGSEL);// Use SMO_Ang

					#if (CURRENT_CONTROL == 1)
						//IQ_CMD_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
						IQ_CMD_MACRO(IQ_FINAL_START_VALUE);
						ID_CMD_MACRO(ID_FINAL_START_VALUE);
						IQ_UI_MACRO(0);
						IQ_OUT_MACRO(0);
						ID_UI_MACRO(0);
						ID_OUT_MACRO(0);
					#endif
					
					#if (SPEED_CONTROL == 1)
						//SPEED_UI_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
						//SPEED_OUT_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
						SPEED_UI_MACRO(IQ_FINAL_START_VALUE);
						SPEED_OUT_MACRO(IQ_FINAL_START_VALUE);
						IQ_UI_MACRO(0);
						IQ_OUT_MACRO(0);
						ID_UI_MACRO(0);
						ID_OUT_MACRO(0);
					#endif
					
					RampDelayCount = 0;
					CloseLoopDelayCount = 0;
					FOCCONT |= PLLEN;
					MOTOR_CONT1 |= (MPWMSEL);// Enable PWM Smo Mode
					MOTOR_CONT2 |= MOTOR_CONT2_REGS;
					PWM_SetActive();
					MotorState = M_START;
				}
				else
					AngleStateCehckCount--;
			}
			
			if((MotorDir == 1) && (abs(TailwindSpeed) > SPEED_TAILWIND_VALUE) && (abs(TailwindSpeed) < 350)){
				if(AngleStateCehckCount > 2){// Perform tailwind start after 2 judgments
					AngleStateCehckCount = 0;
					//FOC_ANG_MACRO(LatestThetaTemp);// Compensation angle
					
 					PllOutTemp = TailwindSpeed;
					PLL_KP_MACRO(Pll_Kp_Start);
					PLL_KI_MACRO(Pll_Ki);
					IQ_KP_MACRO(Iq_Kp);
					IQ_KI_MACRO(Iq_Ki);
					ID_KP_MACRO(Id_Kp);
					ID_KI_MACRO(Id_Ki);

					MOTOR_CONT1 |= (FOCANGSEL);// Use SMO_Ang

					#if (CURRENT_CONTROL == 1)
						//IQ_CMD_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
						IQ_CMD_MACRO(IQ_FINAL_START_VALUE);
						ID_CMD_MACRO(ID_FINAL_START_VALUE);
						IQ_UI_MACRO(0);
						IQ_OUT_MACRO(0);
						ID_UI_MACRO(0);
						ID_OUT_MACRO(0);
					#endif
					
					#if (SPEED_CONTROL == 1)
						//SPEED_UI_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
						//SPEED_OUT_MACRO(IQ_FINAL_TAILWIND_START_VALUE);
						SPEED_UI_MACRO(IQ_FINAL_START_VALUE);
						SPEED_OUT_MACRO(IQ_FINAL_START_VALUE);

						IQ_UI_MACRO(0);
						IQ_OUT_MACRO(0);
						ID_UI_MACRO(0);
						ID_OUT_MACRO(0);
					#endif
					
					RampDelayCount = 0;
					CloseLoopDelayCount = 0;
					FOCCONT |= PLLEN;
					MOTOR_CONT1 |= (MPWMSEL);// Enable PWM Smo Mode
					MOTOR_CONT2 |= MOTOR_CONT2_REGS;
					PWM_SetActive();
					MotorState = M_START;
				}
				else
					AngleStateCehckCount++;
			}
		}
		AngleState = 2;
	}
	else if(LatestTheta > (160<<6)){// When greater than 170 degrees
		AngleState = 3;
	}
}
#endif
//********************************************************************************
//								 Execute tailwind and headwind start
//********************************************************************************
void TailWindStart_Fun (void){
	if(MotorDir == 1){//Tailwind start
		#if 1
		RampDelayCount++;
		if(RampDelayCount >= SMO_RAMP){
			RampDelayCount = 0;

			PllOutTemp = Ramp_Fun(FREQ_FINAL_START_VALUE,PllOutTemp,32700,-32700,1);
		}
		#endif
		
		#if ((SOP_LEVEL == 1) || (SOP_LEVEL == 2))		

		// PLL_UI_MACRO(350);
		// PLL_OUT_MACRO(350);
		
		PLL_UI_MACRO(PllOutTemp);
		PLL_OUT_MACRO(PllOutTemp);
	
		#if (SOP_LEVEL == 2)
		if(PllOutTemp >= FREQ_FINAL_START_VALUE){
			CloseLoopDelayCount++;
			if(CloseLoopDelayCount > SMO_DELAY_DURATION){
				CloseLoopDelayCount = 0;
				FLAG.CloseLoopFlag = 1;
			}
		}
		#endif
	#endif
	}
	else if(MotorDir == 2){	//Headwind start
		#if((SOP_LEVEL == 1) || (SOP_LEVEL == 2))
		RampDelayCount++;
			if(RampDelayCount >= SMO_RAMP){
			RampDelayCount = 0;
			
			PllOutTemp = Ramp_Fun(BEMF_TAILWIND_SPEED_VALUE,PllOutTemp,32700,-32700,1);	//Pull this PLL_OUT against the wind
		}
			
		PLL_UI_MACRO(PllOutTemp);
		PLL_OUT_MACRO(PllOutTemp);
		
		#if (SOP_LEVEL == 2)
			if(PllOutTemp >= BEMF_TAILWIND_SPEED_VALUE)

			{
				CloseLoopDelayCount++;
				if(CloseLoopDelayCount > SMO_DELAY_DURATION){
					CloseLoopDelayCount = 0;
					FLAG.CloseLoopFlag = 1;
				}
			}
		#endif
	#endif
	}
	MotorCloseLoop_Fun();
}
//********************************************************************************
//								    IPD
//********************************************************************************
#if (IPD_FUNCTION == 1)
xdata uint8 IPD_Cnt = 0;
xdata uint8 IPD_Detect_State = 0;

void IPDDetect_Fun (void){
	#define IPDAdvanceAng 32
	switch (IPD_Detect_State){
		case 0:
			AOCPCONT = IPD_LEVEL;
			Break_Fun();
			IPD_Cnt++;
			if(IPD_Cnt > 30){
				IPD_Detect_State = 1;
				IPD_Cnt = 0;
			}
			break;
		case 1:
			//WatchDog_Disable(); // (note: IPD will triger WatchDog Reset , Disable WatchDog)
			IPD_Init();
			//WatchDog_Init();
			IPD_Detect_State = 2;
			break;
		case 2:
			if(IPDPattern == 4) LatestTheta = (64+IPDAdvanceAng)<<6;
			else if(IPDPattern == 5) LatestTheta = (140+IPDAdvanceAng)<<6;
			// else if(IPDPattern == 5) LatestTheta = (128+IPDAdvanceAng)<<6;
			else if(IPDPattern == 2) LatestTheta = (192+IPDAdvanceAng)<<6;
			else if(IPDPattern == 3) LatestTheta = (256+IPDAdvanceAng)<<6;
			else if(IPDPattern == 6) LatestTheta = (320+IPDAdvanceAng)<<6;
			else if(IPDPattern == 1) LatestTheta = (0+IPDAdvanceAng)<<6;
			else LatestTheta = (0+IPDAdvanceAng)<<6;
			MotorErrorState &= ~(AOCP);
			MotorState = M_START;
			IPD_Detect_State = 0;
			break;
		default:
			break;
	}
}
#endif
//********************************************************************************
//								    Motor Start
//********************************************************************************
void IPDStart_Fun (void){
	float f_Temp;
	if(AlignmentCount < 1){
		AlignmentCount++;
		PllOutTemp = 0;
		PLL_KP_MACRO(Pll_Kp_Start);
		PLL_KI_MACRO(Pll_Ki);
		// PLL_KP_MACRO(0);
		// PLL_KI_MACRO(0);
		SPEED_KP_MACRO(0);
		SPEED_KI_MACRO(0);
		IQ_KP_MACRO(Iq_Kp);
		IQ_KI_MACRO(Iq_Ki);
		// ID_KP_MACRO(Id_Kp);
		// ID_KI_MACRO(Id_Ki);
		
		ID_KP_MACRO(0);
		ID_KI_MACRO(0);

		MOTOR_CONT1 |= FOCANGSEL;// Use SMO_Ang

		#if (CURRENT_CONTROL == 1)
			IqOutTemp = IQ_INIT_START_VALUE;
			//IdOutTemp = 0;
			IQ_CMD_MACRO(IqOutTemp);
			//ID_CMD_MACRO(IdOutTemp);
		#endif
		
		#if (SPEED_CONTROL == 1)
			IqOutTemp = IQ_INIT_START_VALUE;
			//IdOutTemp = 0;
			SPEED_UI_MACRO(IqOutTemp);
			SPEED_OUT_MACRO(IqOutTemp);
			//ID_CMD_MACRO(IdOutTemp);
		#endif
		
		RampDelayCount = 0;
		CloseLoopDelayCount = 0;
		FOCCONT |= PLLEN;
		
		#if (IPD_FUNCTION == 1)
			FOC_ANG_MACRO(LatestTheta);
			CPU_ANG_MACRO(LatestTheta>>6);
		#else
			FOC_ANG_MACRO(LatestTheta);
			CPU_ANG_MACRO(LatestTheta>>6);
		#endif
		
		MOTOR_CONT1 |= (MPWMSEL);// Enable PWM Smo Mode
		PWM_SetActive();
		
		f_Temp = IQ_INIT_START_VALUE;
	}
	else if(AlignmentCount < I_ALIG_DURATION){ //Parking
		AlignmentCount++;
		f_Temp += I_ALIG_STEP_VALUE;
		if(f_Temp >= IQ_FIRST_START_VALUE)
			f_Temp = IQ_FIRST_START_VALUE;
		IqOutTemp = f_Temp;
		
		#if (CURRENT_CONTROL == 1)
			IQ_CMD_MACRO(IqOutTemp);
		#endif
		
		#if (SPEED_CONTROL == 1)	
			SPEED_UI_MACRO(IqOutTemp);
			SPEED_OUT_MACRO(IqOutTemp);
		#endif
		
		PllOutTemp = FREQ_FIRST_START_VALUE;
		PLL_UI_MACRO(PllOutTemp);
		PLL_OUT_MACRO(PllOutTemp);
	}
	else if(AlignmentCount >= I_ALIG_DURATION){
		//AlignmentCount = I_ALIG_DURATION;
		
		#if ((SOP_LEVEL == 1) || (SOP_LEVEL == 2))
			RampDelayCount++;
			if(RampDelayCount >= SMO_RAMP){
				RampDelayCount = 0;			

				PllOutTemp = Ramp_Fun(FREQ_FINAL_START_VALUE,PllOutTemp,FREQ_FINAL_START_VALUE,FREQ_FIRST_START_VALUE,PLL_ACC);
				
				IqOutTemp = Ramp_Fun(IQ_FINAL_START_VALUE,IqOutTemp,32767,-32767,16);
				//IdOutTemp = Ramp_Fun(ID_FINAL_START_VALUE,IdOutTemp,32767,-32767,4);
				
				#if (CURRENT_CONTROL == 1)
					IQ_CMD_MACRO(IqOutTemp);
				#endif
				
				#if (SPEED_CONTROL == 1)
					SPEED_UI_MACRO(IqOutTemp);
					SPEED_OUT_MACRO(IqOutTemp);
				#endif
				
				PLL_UI_MACRO(PllOutTemp);
				PLL_OUT_MACRO(PllOutTemp);
				
				//ID_CMD_MACRO(IdOutTemp);
			}		
			
			#if (SOP_LEVEL == 2)
				if((PllOutTemp >= FREQ_FINAL_START_VALUE)&&(IqOutTemp >= IQ_FINAL_START_VALUE)){
					CloseLoopDelayCount++;
					if(CloseLoopDelayCount > SMO_DELAY_DURATION){
						CloseLoopDelayCount = 0;
						FLAG.CloseLoopFlag = 1;
					}
				}
			#endif
		#endif
		MotorCloseLoop_Fun();
	}
	else{
		AlignmentCount++;
	}
}
//********************************************************************************
//								    brake
//********************************************************************************
void Break_Fun (void){
	#if (BREAK_FUNCTION == 1)
	  MOTOR_CONT1 &= ~(MPWMSEL);// Enable PWM User Mode
		PWM_SetBreak();
		if(BreakPwmDuty < BREAK_DUTY_VALUE) BreakPwmDuty++;
			PWM_Duty(BreakPwmDuty);
	#else
		MOTOR_CONT1 &= ~(MPWMSEL);// Enable PWM User Mode
		PWM_SetBreakForce();
		if(BreakPwmDuty < BREAK_DUTY_VALUE) BreakPwmDuty++;
			PWM_Duty(BreakPwmDuty);
	#endif
}
//********************************************************************************
//								Ibus Calculation
//********************************************************************************
#if (IBUS_LIMIT == 1)
void Ibus_Calculation(void){
	Ibus_avg_FILTED  = ((signed long)Ibus_avg * 31 + Ibus_avg) >> 5;
	Ibus_avg_FILTED  = Ibus_avg_FILTED << 6;
	if(MotorState == M_RUN)
		Ibus_In_AMPS = Ibus_avg_FILTED / (float)dIDC_GAIN_W;

}
#endif
//********************************************************************************
//								 Power Limit
//********************************************************************************
#if (POWER_LIMIT == 1)
bit power_test;
void PowerLimit_Fun (void){//1ms
	if(MotorState == M_RUN){
		if(SpeedNum==4){
			if(PowerLimitDelayCount > POWER_LIMIT_DELAY_DURATION){
				if(++WattCount > POWER_LIMIT_TIMER){
					WattCount = 0;
					
					#if (CURRENT_CONTROL == 1)
						if(Watt > Watt_InRef){
							// power_test = 1;
							if(CurrentCmdLimit>-32767){
								if(Watt > Watt_InRef+5)
									CurrentCmdLimit-=5;
								else
									CurrentCmdLimit--;
							}

							// if(CurrentCmdLimit < 100)
								// CurrentCmdLimit = 100;
						}
						else{
							// if(power_test==1){
								if(CurrentCmdLimit<32767)
									CurrentCmdLimit++;
							// if(CurrentCmdLimit > IQ_MAX_LIMIT_VALUE)
								// CurrentCmdLimit = IQ_MAX_LIMIT_VALUE;
							// }
						}
					#endif
					
					#if (SPEED_CONTROL == 1)
						if(Watt > Watt_InRef){
							CurrentCmdLimit--;
							if(CurrentCmdLimit < 100)
								CurrentCmdLimit = 100;
						}
						else{
							CurrentCmdLimit++;
							if(CurrentCmdLimit > IQ_MAX_LIMIT_VALUE)
								CurrentCmdLimit = IQ_MAX_LIMIT_VALUE;
						}
						SPEED_MAX_MACRO(CurrentCmdLimit);
					#endif
				}
			}
			else
				PowerLimitDelayCount++;
		}
		else{
			CurrentCmdLimit = 0;
			power_test = 0;
		}
	}
}
#endif
//********************************************************************************
//								Power Control
//********************************************************************************
#if (POWER_CONTROL == 1)
void PowerControl_Fun (void){
	if(MotorState == M_RUN){
		#if (POWER_CONTROL_SOP == 2)
			if(++PIParm_Watt.Cnt >= WattFreSet){ // WattFreSet
				PIParm_Watt.Cnt = 0;
				Watt = (int)((float)Vbus_avg * Ibus_avg * 0.0049);
				PIParm_Watt.InRef = WATT_LIMIT_VALUE;
				PIParm_Watt.InMeas = Watt;
				CalcPI(&PIParm_Watt);
				CurrentCmd = PIParm_Watt.Out;
				CurrentCmdTemp = CurrentCmd;
				IQ_CMD_MACRO(CurrentCmdTemp);
			}
		#else
			Watt = (int)((float)Vbus_avg * Ibus_avg * 0.0049);
			CurrentCmdTemp = CurrentCmd;
			IQ_CMD_MACRO(CurrentCmdTemp);
		#endif
	}
	else{
		//PowerControlDelayCount = 0;
		Watt = 0;
		PIParm_Watt.Sum = ((int32)POWER_PI_OUT_VALUE << 15);
	}
}
#endif
//********************************************************************************
//							Power Control(User PI)
//********************************************************************************
#if (POWER_CONTROL_USER_PI == 1)
void PowerControl_UserPI_Fun (void){
	if(MotorState == M_RUN){
		#if (POWER_CONTROL_USER_PI_SOP == 2)
			if(UserPI_PowerControlDelayCount > POWER_CONTROL_DELAY_DURATION){
				UserPI_PowerControlDelayCount = 0;
				//Watt = (int)((uint32)Vbus_avg * Ibus_avg * 2129 >> 15);
				Watt = (int)((float)Vbus_avg * Ibus_avg * dPOWER_GAIN);	//10640
				// WattTemp = (int)((float)Vbus_avg * Ibus_avg * dPOWER_GAIN);
				// Watt = LPF_Function(WattTemp,Watt,16383);	//6.2us

				USER_PI_ACTIVE;
				USER_CMD_MACRO(WATT_LIMIT_VALUE);
				USER_FB_MACRO(Watt);
				GET_USER_OUT_MACRO(CurrentCmd);
				//SFR_PAGE = 4; PI_CMD = WATT_LIMIT_VALUE;
				//SFR_PAGE = 4; PI_FB = Watt;
				//SFR_PAGE = 4; CurrentCmd = PI_OUT;
				CurrentCmdTemp = CurrentCmd;
				IQ_CMD_MACRO(CurrentCmdTemp);
			}
			else{
				UserPI_PowerControlDelayCount++;
			}
		#else
			Watt = (int)((float)Vbus_avg * Ibus_avg * dPOWER_GAIN);
			CurrentCmdTemp = CurrentCmd;
			IQ_CMD_MACRO(CurrentCmdTemp);
		#endif
	}
	else{
		UserPI_PowerControlDelayCount = 0;
		Watt = 0;
		SFR_PAGE = 4; PI_UI = POWER_PI_OUT_VALUE;
		SFR_PAGE = 4; PI_OUT = POWER_PI_OUT_VALUE;
	}
}
#endif
//********************************************************************************
//								    Reset MOC
//********************************************************************************
void ResetMOC (void){
	FOCCONT |= 0x80;			// Clear PI & SMO
}
//********************************************************************************
//								    MOC INIT
//********************************************************************************
void MOC_Init (void){
	MOTOR_CONT1 = MOTOR_CONT1_REGS;
	MOTOR_CONT2 &= ~(0x10);
	MOTOR_CONT3 = MOTOR_CONT3_REGS;
	
	// Motor PWM Compensate Factor Register
	MPWMCPSF = 0;
	
	FG_CTRL = FG_CTRL_REGS;
	
	FOCCONT = PICLEAR | ESTCR | INVADCD | ADCTRIG;// PI Clear
	
	PI_GAIN = PI_GAIN_REGS;
	PI_TMSR = 0x00;
}
