ev.c

用ＴＭＳ３２０ＬＦ２４０７Ａ开发电机控制的源代码

/************************************************************************
Project name: 	motorTest.pjt
File name: 		ev.c
Author: 		胡天亮. 山东大学数控技术研究中心
				Tianliang Hu, CNC Research Center, Shandong University
Description:	Event manager funtion, include axes position detect of QEP circuit
History:		(1)
*************************************************************************/

//Include header file
#include "f2407_c.h"
#include "motor.h"
#include "pid.h"
#include "ev.h"



//extern Variable definition
extern PIDDataStruct pidDataAxisA;
extern PIDDataStruct pidDataAxisB;
extern PIDDataStruct pidDataAxisC;
extern PIDDataStruct pidDataAxisD;

extern int motorCtrlData[4];		//PID calculated motor control data
extern double motorCmd[4];

//Variable definition
long QepCNT = 0;

int	 motorAPos = 0;
long motorBPos = 0;
long QepBCycle = 0;
long motorCPos = 0;
long motorDPos = 0;

int TimerPIntFlag = 0;


//Variable for test
int countNum = 0;
float pidValue = 0.000;

/*---------------------------------------------------------------------------
Name: 			Qepinitial()
Description:	QEP circuit initialization
Input par:		none
Output par:		none
---------------------------------------------------------------------------*/
int inline Qepinit()
{
	*MCRA |= 0x0018;
	*MCRC |= 0x0180;
	*T2PR  = 0x0FFFF;
	*T4PR  = 0x0FFFF;
	*T2CON = 0x01870;
	*T4CON = 0x01870;
	*T2CNT = QEPA_BASE_VALUE;
	*T4CNT = QEPB_BASE_VALUE;
	*CAPCONA = 0x0E000;				//enable Qep
	*CAPCONB = 0x0E000;
}


/*---------------------------------------------------------------------------
Name: 			QepACap()
Description:	QEPA process;
				Update motor A position, Here use motor A as spindle motor
Input par:		none
Output par:		none
---------------------------------------------------------------------------*/
void inline QepACap()
{
	QepCNT = *T2CNT;
	
	//QepA is used for spindle motor	
	if(QepCNT > QEPA_BASE_VALUE +SPINDLE_RESOLUTION)
	{
		*T2CNT -= SPINDLE_RESOLUTION;
		QepCNT -= (QEPA_BASE_VALUE + SPINDLE_RESOLUTION);
		pidDataAxisA.Signals.uc -= SPINDLE_RESOLUTION/10000.0;
		
	}
	else if (QepCNT < QEPA_BASE_VALUE - SPINDLE_RESOLUTION)
	{
		*T2CNT += SPINDLE_RESOLUTION;
		QepCNT -= (QEPA_BASE_VALUE - SPINDLE_RESOLUTION);
		pidDataAxisA.Signals.uc += SPINDLE_RESOLUTION/10000.0;
	}
	else
	{
		QepCNT -= QEPA_BASE_VALUE;
	}
	motorAPos = QepCNT;
}		

/*---------------------------------------------------------------------------
Name: 			QepBCap()
Description:	QEPB process;
				Update motor B position
Input par:		none
Output par:		none
---------------------------------------------------------------------------*/
void inline QepBCap()
{
	QepCNT = *T4CNT;
	if((*EVBIFRB & 0x0004) && (*EVBIFRB & 0x0008))
	{
		if(QepCNT>0x7fff)
		{
			QepBCycle--;
		}
		else
		//(QepCNT>0x7fff)
		{
			QepBCycle++;			
		}
		*EVBIFRB |= 0x000C;
	}
	motorBPos = QepBCycle * FULLCNT_AB + QepCNT - QEPB_BASE_VALUE;;
}


/*---------------------------------------------------------------------------
Name: 			QepCCap()
Description:	QEPC process;
				Update motor C position, Here use ls7266 X channel as QEP decoder
Input par:		none
Output par:		none
---------------------------------------------------------------------------*/
void inline QepCCap()
{
	motorCPos = ls7266_SoftRdX() - QEPC_BASE_VALUE;
}


/*---------------------------------------------------------------------------
Name: 			QepDCap()
Description:	QEPD process;
				Update motor D position, Here use ls7266 Y channel as QEP decoder
Input par:		none
Output par:		none
---------------------------------------------------------------------------*/
void inline QepDCap()
{
	motorDPos = ls7266_SoftRdY() - QEPD_BASE_VALUE;
}

/*---------------------------------------------------------------------------
Name: 			Timer1int()
Description:	Initial timer 1 as inspect timer,
				to do PID motor serve when PID serve in com and T3 interrupt is not active
				or not active in 5ms
Input par:		none
Output par:		none
---------------------------------------------------------------------------*/
void inline Timer1int()
{
	*IMR |= 0x0002;
	
	*EVAIMRA = *EVAIMRA | 0X0080;		// 允许定时器1周期中断
	*EVAIFRA = *EVAIFRA & 0X0080;		// 清除定时器1周期中断标志
	*T1CON   = 0x0110C;					// Timer1为连续增计数模式，预分频值为2，使用内部时钟
	*T1PR    = 0x7530;					// Timer1的周期寄存器的值根据5ms延时和预分频值确定
//	*T1CON   = 0x0100C;					// Timer3为连续增计数模式，预分频值为1，使用内部时钟
//	*T1PR    = 0x9C40;					// Timer3的周期寄存器的值根据2ms延时和预分频值确定
	*T1CNT   = 0x00;					// Timer1的计数器清零
}



/*---------------------------------------------------------------------------
Name: 			Timer3int()
Description:	Initial timer 3 as GP timer
Input par:		none
Output par:		none
---------------------------------------------------------------------------*/
void inline Timer3int()
{
	*IMR |= 0x0002;
	
	*EVBIMRA = *EVBIMRA | 0X0080;		// 允许定时器3周期中断
	*EVBIFRA = *EVBIFRA & 0X0080;		// 清除定时器3周期中断标志
	*T3CON   = 0x0100C;					// Timer3为连续增计数模式，预分频值为1，使用内部时钟
	*T3PR    = 0x9C40;					// Timer3的周期寄存器的值根据2ms延时和预分频值确定
	*T3CNT   = 0x00;					// Timer3的计数器清零
}

/*---------------------------------------------------------------------------
Name: 			T3_INTERRUPT()
Description:	Timer 3 interrupt service, Timer 3 is used as timer for PID caculation
Input par:		none
Output par:		none
---------------------------------------------------------------------------*/	
void interrupt TimerPIntServe()
{					
	int i = 0;
	TimerPIntFlag = 1;
	switch(*PIVR)
	{
	//Timer 3 Period interrupt service
	case 0x002F:	
		*T3CNT = 0x00;                  //Clear Timer3 counter
		*EVBIFRA = *EVBIFRA & 0x0080;   //Clear T3 Period interrupt flag
		*T3CON &= 0xFFBF;				//Stop T3
		*T1CNT = 0x00;					//Clear T1 counter to stop inspect service
		
		//Serve motor
		motorServe(MOTORA_ID, motorCtrlData[MOTORA_ID]);
		motorServe(MOTORB_ID, motorCtrlData[MOTORB_ID]);
		motorServe(MOTORC_ID, motorCtrlData[MOTORC_ID]);
		motorServe(MOTORD_ID, motorCtrlData[MOTORD_ID]);
		
		//Get Current position and then begin PID calculate
		QepACap();
		QepBCap();
		QepCCap();
		QepDCap();
		PID_Calculate(MOTORA_ID);
		PID_Calculate(MOTORB_ID);
		PID_Calculate(MOTORC_ID);
		PID_Calculate(MOTORD_ID);

		for(i = 0; i < 4; i++)
		{
			motorCmd[i] = 0;
		}
		break;
	//Timer 1 Period interrupt service
	case 0x0027:
		*T1CNT = 0x00;                  //Clear Timer1 counter
		*EVAIFRA = *EVAIFRA & 0x0080;   //Clear T1 Period interrupt flag

		//Serve motor
		motorServe(MOTORA_ID, motorCtrlData[MOTORA_ID]);
		motorServe(MOTORB_ID, motorCtrlData[MOTORB_ID]);
		motorServe(MOTORC_ID, motorCtrlData[MOTORC_ID]);
		motorServe(MOTORD_ID, motorCtrlData[MOTORD_ID]);
		
		//Get Current position and then begin PID calculate
		QepACap();
		QepBCap();
		QepCCap();
		QepDCap();
		PID_Calculate(MOTORA_ID);
		PID_Calculate(MOTORB_ID);
		PID_Calculate(MOTORC_ID);
		PID_Calculate(MOTORD_ID);
		break;
	default:
		break;

	}
	
}