mtc.c

该程序是ST7MC驱动BLDC120

	if (MotorStatus & AUTO_SWITCH)
	{
	    if ( (!ValBit(Flag_MTC,SWITCH_TO_VOLTAGE_PWMON)) && (Get_CurrentTargetSpeed() >= Freq_Switch_Mode) )
	    {
	    	SetBit(Flag_MTC,SWITCH_TO_VOLTAGE_PWMON);
	    //	Adjust_INT_TERM_voltage();
	    	
	    // 	ToCMPxL(MCPUL,(u16)Init_Duty_Voltage); // initial duty cycle
		//	ToCMPxH(MCPUH,(u16)Init_Duty_Voltage);	    	
	    //	MCPVL = mem_MCPVL;    	// current limitation - Voltage mode
		//	MCPVH = mem_MCPVH;
	    //	ClrBit(MCRA,3);  		// switch to voltage mode now!
	    	
	    	SET_MTC_PAGE(1);
	    	MCONF = mem_MCONF_Ontime;
	    	SET_MTC_PAGE(0);	    	
			MCRC = mem_MCRC_Ontime;			
			PDDDR |= 0x0d; 		//PD0,PD2,PD3 push pull output
			PDOR  |= 0x0d; 	
			PDDR  &= 0xf2; 		//PD0,PD2,PD3 = 0
			
		//	Rising_bemf  = Rising_Table[Freq_Switch_Mode - Start_Freq];
		//	Falling_bemf = Falling_Table[Freq_Switch_Mode - Start_Freq];	    		    	
	    }
	    else if ( (ValBit(Flag_MTC,SWITCH_TO_VOLTAGE_PWMON)) && (Get_CurrentTargetSpeed() < Freq_Switch_Mode) ) 
	    {
	    	ClrBit(Flag_MTC,SWITCH_TO_VOLTAGE_PWMON);
	    //	Adjust_INT_TERM_current();
	    		    	
	    //	ToCMPxL(MCPVL,(u16)Init_Duty_Current); 	// initial duty cycle for reference current
		//	ToCMPxH(MCPVH,(u16)Init_Duty_Current);	    	
	    //	SetBit(MCRA,3);  			// switch to voltage mode now!
	    	
	    	SET_MTC_PAGE(1);
	    	MCONF = mem_MCONF;
	    	SET_MTC_PAGE(0);	    	
			MCRC = mem_MCRC;			
			PDDDR &= 0xf2; 			//PD0,PD2,PD3 floating Input
			PDOR  &= 0xf2; 	
	    	
	    //	Rising_bemf  = Rising_Table[Freq_Switch_Mode - Start_Freq -1];
		//	Falling_bemf = Falling_Table[Freq_Switch_Mode - Start_Freq -1];
	    }           
	               
	    if (MZREG>=0x5)   // check MZREG value to keep good accuracy
            {
            Step_Z[Step_Z_Counter].Ratio = (u8)(MPRSR & 0x0f);   // update Z steptime buffer for PI regulation
            Step_Z[Step_Z_Counter].StepTime = MZREG;             // & motor frequency computation
           
            if (Step_Z_Counter >= STEP_Z_BUFFER_SIZE-1) Step_Z_Counter = 0;
            else Step_Z_Counter++;
      	    }	
      	    
      	if (Step_counter != 0)	Step_counter --;       
    }
  	else    // Forced switched mode/accel phase
	    {
		BemfCounter++;
		if (BemfCounter == bemf_valid)	//Transition switched->autoswitched
		{
			if (MZREG <= (U8_MAX/2))	// No overflow when multiply
			{
				// Center last zerocrossing in the middle of two commutations
				MCOMP = (u8)(MZREG * 2);
			}
			else	MCOMP = U8_MAX-1;		// Not correct: step not centered
			MotorStatus |= LAST_FORCED_SWITCH;	// Last switched mode step
		}	// End of switched->autoswitched transition 
  	}
}	// End of Z event processing


// RRRRRRRRRRRRRRRRRRRRRRRRRRRRR RPI event RRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRR
if (MISR & RPI_MSK)
    {
    MISR = ((u8)~(RMI_MSK + RPI_MSK));
    #if (DEMAG_TYPE == HSW)
    		RP_counter ++;	
        	if (RPICounter !=0)
        		{
        		RPICounter --;        
         	    
    	   		if (RPICounter == 0)   	MDREG = (u8) SoftDemagTime;
	        	
            	}
    #elif (DEMAG_TYPE == SW)
	
	   	if (RPICounter !=0)
			{
   			RPICounter --;        
   			if (RPICounter == 0)   	MDREG = (u8) SoftDemagTime;
		   	}
    #endif

	}

// RRRRRRRRRRRRRRRRRRRRRRRRRRRRR RMI event RRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRRR
if (MISR & RMI_MSK)
    {
    MISR = ((u8)~(RMI_MSK + RPI_MSK));
    SetBit(Flag_it,RM_EVT);
    }
}        



#if defined ( __HIWARE__)		/* test for HIWARE Compiler */
#pragma TRAP_PROC SAVE_REGS	/* additional registers will be saved */
#elif defined ( __CSMC__ )			/* test for Cosmic Compiler */
@interrupt			/* Cosmic interrupt handling */
#endif
/*-----------------------------------------------------------------------------
ROUTINE Name : MCES_SE_IT

Description:	Emergency Stop and Speed Error interrupt service routine.
        	As this routine is only entered in case of HW problem, the 
	        motor is stopped and trace of this interrupt is keeped with a 
		MotorStatus flag.

Comments:	The Speed Error Flag is only used for induction motor drive.
-----------------------------------------------------------------------------*/
void MCES_SE_IT(void)
{
	MISR = ((u8)~EI_MSK);	// Clr Emergency Stop Flag
	
	MTC_StopMotor();
	MotorStatus = EMERGENCY_STOP;
}  


/*-----------------------------------------------------------------------------
ROUTINE Name : Set_Duty

Description:	This is the update routine of the PWM duty cycle -> feed MCPUH/L
		(voltage mode) or MCPVH/L (current mode) registers. 
		
Comments:	None
-----------------------------------------------------------------------------*/
void Set_Duty(u16 duty)
{
#if (DRIVING_MODE == VOLTAGE_MODE)
	ToCMPxL(MCPUL,(u16)duty);
	ToCMPxH(MCPUH,(u16)duty);
#elif (DRIVING_MODE == CURRENT_MODE)
	ToCMPxL(MCPVL,(u16)duty);
	ToCMPxH(MCPVH,(u16)duty);
#endif
}      
 

/*-----------------------------------------------------------------------------
ROUTINE Name : Set_Target_Electrical_Frequency

Description:	This is the update routine of the PWM duty cycle/PWM current
				limitation in closed loop mode -> feed MCPUH/L in voltage mode
				or MCPVH/L in current mode. 
				The target electrical frequency (0.1 Hz resolution, eg 1000 for 100Hz)
				is input in the the regulation routine;	result (10 bits value)
				is then used to calculate the new duty cycle.
		
Comments:	see mtc.h to for rising/falling bemf,Ki/Kp coefficient calculation.
-----------------------------------------------------------------------------*/
void Set_Target_Electrical_Frequency(u16 target_freq)
{
	u32 temp; 
	u8  temp1;
/*
  	if (target_freq<=Freq_Min)       // target_freq<=Freq_Min ??
    {
		Rising_bemf = Rising_Fmin ;
		Falling_bemf = Falling_Fmin;
		Ki = Ki_Fmin;      // integral term
		Kp = Kp_Fmin;      // proportionnal term
    }
    else if (target_freq<=F_1)       // Freq_Min<=target_freq<=F_1 ??
	{
		Rising_bemf = (u8)(Rising_Fmin + (s32)(alpha_Rising_1*(target_freq-Freq_Min)/1024)); 
		Falling_bemf = (u8)(Falling_Fmin + (s32)(alpha_Falling_1*(target_freq-Freq_Min)/1024));
		Ki = (u8)(Ki_Fmin + (s32)(alpha_Ki_1*(target_freq-Freq_Min)/1024));
		Kp = (u8)(Kp_Fmin + (s32)(alpha_Kp_1*(target_freq-Freq_Min)/1024));
	}
	else if (target_freq<=F_2)  	// F_1<target_freq<=F_2 ??
	{
		Rising_bemf = (u8)(Rising_F_1 + (s16)(alpha_Rising_2*(target_freq-F_1)/1024)); 
		Falling_bemf = (u8)(Falling_F_1 + (s16)(alpha_Falling_2*(target_freq-F_1)/1024));
		Ki = (u8)(Ki_F_1 + (s32)(alpha_Ki_2*(target_freq-F_1)/1024));
		Kp = (u8)(Kp_F_1 + (s32)(alpha_Kp_2*(target_freq-F_1)/1024));
	}
	else if (target_freq<=F_3)  	// F_2<target_freq<=F_3 ??
	{
		Rising_bemf = (u8)(Rising_F_2 + (s16)(alpha_Rising_3*(target_freq-F_2)/1024)); 
		Falling_bemf = (u8)(Falling_F_2 + (s16)(alpha_Falling_3*(target_freq-F_2)/1024));
		Ki = (u8)(Ki_F_2 + (s32)(alpha_Ki_3*(target_freq-F_2)/1024));
		Kp = (u8)(Kp_F_2 + (s32)(alpha_Kp_3*(target_freq-F_2)/1024));
	}	
	else if (target_freq<=Freq_Max)  // F_3<target_freq<=Freq_Max ??
	{                       
		Rising_bemf = (u8)(Rising_F_3 + (u16)(alpha_Rising_4*(target_freq-F_3)/1024)); 
		Falling_bemf = (u8)(Falling_F_3 + (u16)(alpha_Falling_4*(target_freq-F_3)/1024));
		Ki = (u8)(Ki_F_3 + (u16)(alpha_Ki_4*(target_freq-F_3)/1024));
		Kp = (u8)(Kp_F_3 + (u16)(alpha_Kp_4*(target_freq-F_3)/1024));
	}
	else                            // Freq_Max>Freq_Motor ??
	{
		Rising_bemf = Rising_Fmax ;
		Falling_bemf = Falling_Fmax;
		Ki = Ki_Fmax;      // integral term
		Kp = Kp_Fmax;      // proportionnal term
	}						
*/  
	
	temp1 = (u8)((u16)(target_freq - Freq_Min)/(Pole_Pair_Num * 10));          //mod by lf 05.9.26
	Rising_bemf  = Rising_Table[temp1];
	Falling_bemf = Falling_Table[temp1];
	
	Ki = Ki_Fmin;      // integral term
	Kp = Kp_Fmin;      // proportionnal term
		
	temp = PWM_FREQUENCY;              // temp =  (PWM_FREQUENCY * PIoutput)/1024
	temp *= regul_PI((u16)(target_freq));
	temp >>= 10;
	if (temp > Max_Duty) temp = Max_Duty;
	
/*	
	if (!ValBit(Flag_MTC,SWITCH_TO_VOLTAGE_PWMON))
	{
		if (temp > Max_Duty) temp = Max_Duty;	 // current & PWM OFF mode
		ToCMPxL(MCPVL,(u16)temp);
		ToCMPxH(MCPVH,(u16)temp);

	}	
	else 
	{
		if (temp < Min_Duty) temp = Min_Duty;    // voltage & PWM ON mode
		ToCMPxL(MCPUL,(u16)temp);
		ToCMPxH(MCPUH,(u16)temp);
	}	
*/	

#if (DRIVING_MODE == VOLTAGE_MODE)
	ToCMPxL(MCPUL,(u16)temp);
	ToCMPxH(MCPUH,(u16)temp);
#elif (DRIVING_MODE == CURRENT_MODE)
	ToCMPxL(MCPVL,(u16)temp);
	ToCMPxH(MCPVH,(u16)temp);
#endif
  
}      

/*-----------------------------------------------------------------------------
ROUTINE Name : active_brake

Description:	braking routine 
Input/Output:	duty cycle, duration in ms (u16/u16)
Comments:	Return from this routine immediately if duty 
			is equal to 0x00. 
-----------------------------------------------------------------------------*/
BOOL active_brake(u16 duty, u16 time)   // duration = time * 1ms  eg: 1000 = 1sec
{
if (duty == 0) 
	{
	MCRA &= (u8)(~SWA_MSK);	// Disable Autoswitched mode and...
	MIMR = 0;
	return(TRUE);
	}
else
	{
	switch(BrakeState)
		{
		case BRAKE_START:
		default:
			MCRA &= (u8)(~SWA_MSK);	// Disable Autoswitched mode and...
			MIMR = 0;
			MTC_EnableDirectAccess();
			MPHST = BREAK_POSITION;	// Alignment step pattern
			MTC_DisableDirectAccess();

			#if (DRIVING_MODE == VOLTAGE_MODE)
			ToCMPxL(MCPUL,(u16)(duty));
			ToCMPxH(MCPUH,(u16)(duty));
			#else
			ToCMPxL(MCPVL,(u16)(duty));
			ToCMPxH(MCPVH,(u16)(duty));
			#endif
			
			Timer_1ms = time;
			BrakeState = BRAKE_ONGOING;
			return(FALSE);
			break;
		case BRAKE_ONGOING:
			if (Timer_1ms == 0)	BrakeState = BRAKE_FINISHED;
			return(FALSE);
			break;
		case BRAKE_FINISHED:
			return(TRUE);
			break;
		}
	}			  
return(FALSE);
}


/*-----------------------------------------------------------------------------
ROUTINE Name : Chk_Motor_Stalled

Description:	 
		
Comments:	None
-----------------------------------------------------------------------------*/
void Chk_Motor_Stalled(void)
{
if ((MPRSR & 0x0F) == RATIO_MAX) 
    {
            if(  MotorStatus != EMERGENCY_STOP)
            {
           	MotorStatus |= MOTOR_STALLED;
    		SetBit(Power_Motor_Status,MotorStalled);
    	    }     
     }
}


/*** (c) 2004  STMicroelectronics ****************** END OF FILE ***/