dcmcontrol.c

5402DSP程序设计语言

#pragma CODE_SECTION(vect,"vect")

void Start(void);
void DAout(void);

#define IMR   *(pmem+0x0000)  
#define IFR   *(pmem+0x0001) 
#define PMST  *(pmem+0x001D)  
#define SWCR  *(pmem+0x002B)  
#define SWWSR *(pmem+0x0028)  

/* timer 0 */
#define TIM0  *(pmem+0x0024)  /* timer0 register */
#define PRD0  *(pmem+0x0025)  /* timer0 period register */
#define TCR0  *(pmem+0x0026)  /* timer0 control register */

#define Len1 1200
#define Len2 120

int r = 80;       /* The speed is target we want to control, in 1s */         
int rc = 8;       /* The actual speed target, per control, per 0.1s */

unsigned int  *pmem=0;
unsigned int  flagc,flagt;
int TIMER1,TIMER2,pw,nw;
int count1,count2,speed1[Len1],speed2[Len2],i,j,n;
int *ppw =(int *)0x1002; 
int *npw =(int *)0x1004; 
double deta,duty;

double e0,e1,e2,y0,y1,y2,x;

double Kp = 8.5;     
double Ti = 1.0;   
double Td = 0.2;   
double T = 0.1;

double a0,a1,a2;

void cpu_init()
{
	asm(" NOP");
	asm(" STM #0, CLKMD");   
	asm(" STM #0, CLKMD");   
	asm(" STM #0xf007, CLKMD"); 
	asm(" rpt  #0ffffh");
	asm(" nop");
	
	PMST=0x3FA0;
	SWWSR=0x7fff;
	SWCR=0x0000;
	IMR=0;
	IFR=IFR;
	asm(" stm 1000h,ar1");
	asm(" stm 1006h,ar6");
} 

interrupt void int1()   
{
	flagc=flagc^1;
	count1++;       
	count2++;
	asm(" rpt  #03fffh");
	asm(" nop");

} 

void set_int()    
{
	asm(" ssbx intm");
	TCR0=0x0b1b;
	PRD0=0x4e1f;       
	IMR=IMR|0x0009;  
	IFR=IFR;
	TCR0=0x0b29;
	asm(" rsbx intm");  
}

interrupt void tint0()
{
	TIMER1++;
	TIMER2++;

	if(TIMER1==5)    /* 0.1s, the PWM is changed in 0.1s */
	{	
		y2 = y1;
		y1 = y0;
		y0 = count1;
		e0 = rc - y0;
		e1 = rc - y1;
		e2 = rc - y2;

		duty = 0.9/(2*(a0+a2)*14.0);
		
		deta = (a0*e0 - a1*e1 + a2*e2)*duty;
		
		pw = pw - 20000*deta;
		nw = nw + 20000*deta;
		if (pw>=10000) pw=10000;
		if (nw>=10000) nw=10000;
		if (pw<=0)     pw =0;
		if (nw<=0)     nw=0;
		
		*ppw = pw;
		*npw = nw;

		TIMER1 = 0;
    	speed1[i] = count1;  
		i++;
		count1 = 0;
	 }
		
	if (TIMER2==50)   /*  to test the speed of motor in 1s */
	{
		flagt = flagt^1;
		x=speed2[j] = count2;   /* store  the speed of motor */
		TIMER2 = count2 =0;     
		j++;
	}

	if (j==Len2) {*ppw = 100; *npw = 0;}
} 

void main(void)
{
	cpu_init();
	
	a0 = Kp*(1.0 + T/Ti + Td/T);
	a1 = Kp*(1.0 + 2.0*Td/T);
	a2 = Kp*Td/T;

	e0=e1=e2=y0=y1=y2=0;
	pw = 10000;
	nw = 10000;
	*ppw = pw;
	*npw = nw;
	flagc = flagt = 0;
	count1 = count2 = 0;
	TIMER1 = TIMER2 =0;
	for (i=0; i<Len1; i++)
	{speed1[i] = 0;}
	for (j=0; j<Len2; j++)
	{speed2[j]=0;}
	i=j=0;
	
	Start();

	for (n=0; n<1024; n++)
	{
		DAout();
	}
	
	set_int();

	for(;;)
	{
		if(flagt==1) {asm(" ssbx xf");}
		else         {asm(" rsbx xf");}

		DAout();
	}
}

void vect()
{
   asm(" .ref _c_int00");/*pseudoinstruction*/
   asm(" .ref _int1");

   asm(" b _c_int00");/* reset */  
   asm(" nop");
   asm(" nop");
   asm(" rete");        
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" b _int1");       /* int0 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");      /* int1 */
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* int2 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" b _tint0");      /* tint0 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* brint0 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* bxint0 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");      /* dmac0 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* tint1 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* int3 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* hpint */
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* brint1 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* bxint1 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");       /* dmac4 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" rete");      /* dmac5 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" nop");
}