regul.c

该程序是ST7MC驱动BLDC120

/**************** (c) 2004 STMicroelectronics **********************

PROJECT  : ST7MC demokit
COMPILER : ST7 METROWERKS C (HIWARE) / COSMIC

MODULE  :  regul.c
LIBRARY VERSION  :  1.0.2

CREATION DATE : 07.2003 
AUTHOR :      Florent COSTE	/  Microcontroller Application Lab  / ST Hong Kong

-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-

DESCRIPTION :   routine for closed LOOP operation              

-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-

 ******************************************************************************
 THE SOFTWARE INCLUDED IN THIS FILE IS FOR GUIDANCE ONLY. ST MICROELECTRONICS
 SHALL NOT BE HELD LIABLE FOR ANY DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES
 WITH RESPECT TO ANY CLAIMS ARISING FROM USE OF THIS SOFTWARE.
 ******************************************************************************
              
-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-

******************************************************************************/

#include "version.h"
#include "lib.h"
#include "mtc.h"
#include "regul.h"
#include "ST7MC_hr.h"
#include "it_ST7MC.h"
#include "LinSCI.h"
#include "MTC_Settings_Sensorless.h" 


#define Error_slip_MAX   (s16)2048 
#define Error_slip_MIN   (s16)-2048  

#define MTIM_MAX_FREQ    MTC_CLOCK/8     // 16Mhz/8 -> 2Mhz
  
//--------------------------------------
// Variables
//--------------------------------------

static s32 VoltageIntegralTerm;
static BOOL MaxPiOut,MinPiOut;      
volatile Step_s Step_Z[STEP_Z_BUFFER_SIZE];    // buffer filled with the last 12 Z step times

 
/*-------------------Main program-------------------- */

/*-----------------------------------------------------------------------------
ROUTINE Name :  Period_To_Frequency

Description:	Convert Step_Z buffer information (electrical period) into 
				electrical frequency.
                
Input/Output:	none/u16 (electrical frequency, 0.1 Hz resolution)
Comments: 		None
-----------------------------------------------------------------------------*/

u16 Period_To_Frequency(void)
{
u32 result;
u16 MZ_Temp;
u8 Ratio_Min, i;

/********************** Compute average Motor Z Step Period ********************************/
// F = Fmtc/(MZREG.2^ratio)    T = (MZREG.2^ratio)/Fmtc
  
Ratio_Min = RATIO_MAX;    // init Ratio_Min with max ratio
MZ_Temp = 0;

for (i=0;i<=STEP_Z_BUFFER_SIZE-1;i++)  // check max ratio of buffer
    {
    if (Step_Z[i].Ratio < Ratio_Min) Ratio_Min = Step_Z[i].Ratio;
    }
// Ratio_Min contains now the min ratio of stored values in Step_Z buffer

for (i=0;i<=STEP_Z_BUFFER_SIZE-1;i++)     // Compute average period
    {                                  
    if (Step_Z[i].Ratio == Ratio_Min) MZ_Temp += (u8)(Step_Z[i].StepTime);
    else MZ_Temp += ((Step_Z[i].StepTime)<<((u8)(Step_Z[i].Ratio-Ratio_Min)));
    }

/***** Convert period to frequency *****/
result = (10*MTIM_MAX_FREQ)/MZ_Temp;
result >>= (u8)(Ratio_Min);           //divide by 2^ratio

return((u16)(result));
}

/*Initialisation of Integral term of PI*/
void Init_PI(void)
{
#if (DRIVING_MODE == VOLTAGE_MODE)
VoltageIntegralTerm = (((ramp_MCPUH<<8) + ramp_MCPUL)>>3)*65536;
VoltageIntegralTerm /= PWM_FREQUENCY;
#else
VoltageIntegralTerm = (((ramp_MCPVH<<8) + ramp_MCPVL)>>3)*65536;
VoltageIntegralTerm /= PWM_FREQUENCY;
#endif
}
  
void Adjust_INT_TERM_current(void)
{
	VoltageIntegralTerm = VoltageIntegralTerm/6;  // /4
}

void Adjust_INT_TERM_voltage(void)
{
	VoltageIntegralTerm = VoltageIntegralTerm*4;  // *3
}  

/*-----------------------------------------------------------------------------
ROUTINE Name :  regul_PI

Description:	Compute PI output (0 (PI min) to 1023 (PI max)) according to Ki,
				Kp, sampling time, and target electrical frequency.
                
Input/Output:	u16/u16 (PI output (10 bits value)/target electrical frequency, 0.1 Hz resolution)
Comments: 	None
-----------------------------------------------------------------------------*/
u16 regul_PI(u16 Target_Freq)      // return 10 bits value
{      
s32 Voltage_slip_s32,DeltaVoltage_slip_s32,Newpi_32;
s16 NewPIoutput, Error_slip,Error;
u16 output;


/********************** Compute PI output ***************************************/

Freq_Motor = (u16)Period_To_Frequency(); 

Error = (s16)(Target_Freq - Freq_Motor);  // Freq_Motor is actually the step time between 6 Z events
if (Error > (s16)(Error_slip_MAX)) 
	{
	Error_slip = Error_slip_MAX;
	}
else if (Error < (s16)(Error_slip_MIN))
	{
	Error_slip = Error_slip_MIN;
	}
else Error_slip = (s16)Error;



/********************** Compute Proportional term ********************************/

Voltage_slip_s32 = Kp * (s32)Error_slip; 

/********************** Compute Integral term ************************************/
// If modulation is maximum, integral term must be "frozen" 

//DeltaVoltage_slip_s32 = ( Ki * SAMPLING_TIME * (s32)Error_slip)/256;
  if (Get_CurrentTargetSpeed() <= (35*Pole_Pair_Num*10) ) 
 DeltaVoltage_slip_s32 = ( Ki * SAMPLING_TIME_LowSpeed * (s32)Error_slip)/256;
 else if(Get_CurrentTargetSpeed() > (35*Pole_Pair_Num*10)&&Get_CurrentTargetSpeed() < (80*Pole_Pair_Num*10) )
 DeltaVoltage_slip_s32 = ( Ki * SAMPLING_TIME_MidSpeed * (s32)Error_slip)/256;
 else 
 DeltaVoltage_slip_s32 = ( Ki * SAMPLING_TIME_HighSpeed * (s32)Error_slip)/256;  
 
if( ((Error_slip>0) && !MaxPiOut) || ((Error_slip<0) && !MinPiOut) )
	{
	if ((VoltageIntegralTerm >= 0) && (DeltaVoltage_slip_s32 >= 0))
		{
		if (( (u32)VoltageIntegralTerm + (u32)DeltaVoltage_slip_s32 ) > S32_MAX)
			VoltageIntegralTerm = S32_MAX;	// Avoid IntTerm Overflow
		else VoltageIntegralTerm += DeltaVoltage_slip_s32; // "integral" output
		}
	else if ((VoltageIntegralTerm < 0) && (DeltaVoltage_slip_s32 < 0))
		{
		if (( (u32)VoltageIntegralTerm + (u32)DeltaVoltage_slip_s32 ) <= S32_MAX)
			VoltageIntegralTerm = S32_MIN;	// Avoid IntTerm Overflow
		else VoltageIntegralTerm += DeltaVoltage_slip_s32; // "integral" output
		}
	 else
		VoltageIntegralTerm += DeltaVoltage_slip_s32; // "integral" output 
	}

    if ((VoltageIntegralTerm >= 0) && (Voltage_slip_s32 >= 0))
		{
		if (( (u32)VoltageIntegralTerm + (u32)Voltage_slip_s32 ) > S32_MAX)
			Newpi_32 = S32_MAX;	// Avoid IntTerm Overflow
		else Newpi_32 = (VoltageIntegralTerm + Voltage_slip_s32); //  output
		}
	else if ((VoltageIntegralTerm < 0) && (Voltage_slip_s32 < 0))
		{
		if (( (u32)VoltageIntegralTerm + (u32)Voltage_slip_s32 ) <= S32_MAX)
			Newpi_32 = S32_MIN;	// Avoid IntTerm Overflow
		else Newpi_32 = (VoltageIntegralTerm + Voltage_slip_s32); //  output
		}
	 else

		Newpi_32 = (VoltageIntegralTerm + Voltage_slip_s32); //  output 
	
#if (DRIVING_MODE == VOLTAGE_MODE)
    NewPIoutput = (s16)( Newpi_32 /64); 
#else
    NewPIoutput = (s16)( Newpi_32 /256);                                    
#endif

if ( NewPIoutput < 0 )
	{
	output = 0;
	MinPiOut = TRUE;
	}
else if ( NewPIoutput > 1024 )
	{
	output = 1024;
	MaxPiOut = TRUE;  // Set ClampFlag if modulation reaches maximum value 
	}
else
	{
	output = NewPIoutput;
	MinPiOut = FALSE;
	MaxPiOut = FALSE;
	}

return (output);  // return PI output
}
      
 
/*** (c) 2004  STMicroelectronics ****************** END OF FILE ***/