example_posspeed.c

iceteck公司提供的F2806的开发板的实例程序

// TI File $Revision: /main/4 $
// Checkin $Date: April 18, 2005   10:04:09 $
//###########################################################################
//
// FILE:	Example_posspeed.c
//
// TITLE:	Pos/speed measurement using EQEP peripheral
//
// DESCRIPTION:
//
// This file contains source for the posspeed module
//
//###########################################################################
// Original Author: SD
//
// $TI Release: DSP280x V1.30 $
// $Release Date: February 10, 2006 $
//###########################################################################

#include "DSP280x_Device.h"     // DSP280x Headerfile Include File
#include "DSP280x_Examples.h"   // DSP280x Examples Include File
#include "Example_posspeed.h"   // Example specific Include file

void  POSSPEED_Init(void)
{

		EQep1Regs.QUPRD=1000000;			// Unit Timer for 100Hz
		EQep1Regs.QDECCTL.bit.QSRC=00;		// QEP clock Inputs
		
		EQep1Regs.QEPCTL.bit.FREE_SOFT=2;
		EQep1Regs.QEPCTL.bit.PCRM=00;		// PCRM=00 mode
		EQep1Regs.QEPCTL.bit.UTE=1; 		// Unit Timeout Enable 
		EQep1Regs.QEPCTL.bit.QCLM=1; 		// Latch on unit time out
		EQep1Regs.QPOSMAX=0xffffffff;
		EQep1Regs.QEPCTL.bit.QPEN=1; 		// QEP enable
		
		EQep1Regs.QCAPCTL.bit.UPPS=5;   	// 1/32 for unit position
		EQep1Regs.QCAPCTL.bit.CCPS=7;		// 1/128 for CAP clock
		EQep1Regs.QCAPCTL.bit.CEN=1; 		// QEP Capture Enable
		

        EALLOW;                       		// Enable EALLOW
        GpioCtrlRegs.GPAMUX2.bit.GPIO20 = 1;// Assuming GPIO20 is EQEP1A
        GpioCtrlRegs.GPAMUX2.bit.GPIO21 = 1;// Assuming GPIO21 is EQEP1B
        GpioCtrlRegs.GPAMUX2.bit.GPIO23 = 1;// Assuming GPIO23 is EQEP1I
        EDIS;                         		// Disable EALLOW

}

void POSSPEED_Calc(POSSPEED *p)
{
     long tmp;
     unsigned int pos16bval,temp1;
   	 _iq Tmp1,newp,oldp;
   
     p->DirectionQep = EQep1Regs.QEPSTS.bit.QDF;

	 pos16bval=(unsigned int)EQep1Regs.QPOSCNT;
     p->theta_raw = pos16bval+ p->cal_angle;
     tmp = (long)((long)p->theta_raw*(long)p->mech_scaler);  	// Q0*Q26 = Q26 
     tmp &= 0x03FFF000;
     p->theta_mech = (int)(tmp>>11);         		// Q26 -> Q15 
     p->theta_mech &= 0x7FFF;
	     
     p->theta_elec = p->pole_pairs*p->theta_mech;  // Q0*Q15 = Q15 
     p->theta_elec &= 0x7FFF;

// Check an index occurrence
     if (EQep1Regs.QFLG.bit.IEL == 1)                   
     {  
    	p->index_sync_flag = 0x00F0;
    	EQep1Regs.QCLR.bit.IEL=1;					// Clear interrupt flag
     }
      

// Check unit Time out-event for speed calculation:
// Unit Timer is configured for 100Hz in INIT function
//**** High Speed Calcultation using QEP Position counter ****//
	if(EQep1Regs.QFLG.bit.UTO==1)
	{ 
		/** Differentiator	**/
	 	pos16bval=(unsigned int)EQep1Regs.QPOSLAT;	// Latched POSCNT value		
     	tmp = (long)((long)pos16bval*(long)p->mech_scaler);  	// Q0*Q26 = Q26 
     	tmp &= 0x03FFF000;
     	tmp = (int)(tmp>>11);         				// Q26 -> Q15 
     	tmp &= 0x7FFF;
		newp=_IQ15toIQ(tmp);
		oldp=p->oldpos;

   		if (p->DirectionQep==0)      				// POSCNT is counting down
   		{
    		if (newp>oldp)
      			Tmp1 = - (_IQ(1) - newp + oldp);
    		else
      		Tmp1 = newp -oldp;
   		}
   		else if (p->DirectionQep==1)      			// POSCNT is counting up
   		{
    		if (newp<oldp)
      		Tmp1 = _IQ(1) + newp - oldp;
    		else 
      		Tmp1 = newp - oldp;
   		}

	   	if (Tmp1>_IQ(1))
	     	p->Speed_fr = _IQ(1);
	   	else if (Tmp1<_IQ(-1))
	     	p->Speed_fr = _IQ(-1);      
	   	else
	     	p->Speed_fr = Tmp1;

		// Update the electrical angle
    	p->oldpos = newp;
     
		// Change motor speed from pu value to rpm value (Q15 -> Q0)
		// Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q
   		p->SpeedRpm_fr = _IQmpy(p->BaseRpm,p->Speed_fr); 
		//=======================================
		
		EQep1Regs.QCLR.bit.UTO=1;					// Clear interrupt flag
	}	

//**** Low-speed computation using QEP capture counter ****//	
	if(EQep1Regs.QEPSTS.bit.UPEVNT==1)
	{
		if(EQep1Regs.QEPSTS.bit.COEF==0) // No Capture overflow
			temp1=(unsigned long)EQep1Regs.QCPRDLAT;
		else							// Capture overflow, saturate the result
			temp1=0xFFFF;
	
		p->Speed_pr = _IQdiv(p->SpeedScaler,temp1); 
		Tmp1=p->Speed_pr;
	
		if (Tmp1>_IQ(1))
	 		p->Speed_pr = _IQ(1);   
		else
	 		p->Speed_pr = Tmp1;
	
		if (p->DirectionQep==0) 
			p->SpeedRpm_pr = -_IQmpy(p->BaseRpm,p->Speed_pr); 	// Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q
		else
			p->SpeedRpm_pr = _IQmpy(p->BaseRpm,p->Speed_pr); 	// Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q

	
		EQep1Regs.QEPSTS.all=0x88;					// Clear Unit position event flag	
													// Clear overflow error flag
	}


}