/***************************************************************************
This programme is to control DC motor in a certain speed using PWM.
The target speed is "r", it is the speed in 1s.
The sample rate is 0.1s, so the actual speed target is "rc"=r/10.
The "r" and "rc" are integer, and the range of "r" is from 50 to 100.
Keep rc=r/10!!!
The array "speed1" and "speed2" are the control result, in 0.1s and 1s.  
The length of "speed1" is 2400, and "speed2" is 240.
The "pw" and "nw" are the parameters of PWM.
The test will last 4 min.
*****************************************************************************/
extern void InitC5402(void);
extern void Start(void);
extern void DAout(void);
interrupt void tint0();
interrupt void int1();

#define Len1 2400
#define Len2 240

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

/* Keep r = rc*10, Keep rc a int!!! */
/* The range of speed control is in 50 to 100 per 1s,
    r= 50 to 100, rc= 5 to 10
*/

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

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

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

double a0,a1,a2;

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

} 



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)*12.0);
		
		deta = (a0*e0 - a1*e1 + a2*e2)*duty;
		
		nw = nw + 30000*deta;
		*ppw = pw;
		*npw = nw;

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

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

void main(void)
{

   InitC5402(); /* initialize C5402 DSP */
	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 = 15000;
	nw = 15000;
	*ppw = pw;
	*npw = nw;

	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;
	for(;;)
	{
		DAout();
	}
}

/***********************************************************************
** End of File –– AIC10Main.c
***********************************************************************/