/***************************************************************************
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 10 to 50.
Keep rc=r/10!!!
The array "speed1" and "speed2" are the control result, in 0.1s and 1s.  
The length of "speed1" is 1200, and "speed2" is 120.
The "pw" and "nw" are the parameters of PWM.
The test will last 2 min.
*****************************************************************************/
#pragma CODE_SECTION(vect,"vect")
extern void cpu_init() ;
void dis_buff(unsigned int count);
void Start(void);
void DAout(void);
void delay(unsigned int num);
#include "math.h"
#include "stdio.h"
#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 */
int led_buff[16]={0x3f,0x06,0x5b,0x4f,0x66,0x6d,0x7d,0x07,0x7f,0x6f,0x77,0x7c,0x39,0x5e,0x79,0x71};

#define 	Len1 1200
#define 	Len2 180
#define         Len3 2


int dis1=0,dis2=0,dis3=0,dis4=0,count=0,dis11=0,dis22=0;
double rc=1;      /*adjust the rc, then change the motor's speed*/

int P=0,P0=0,add=0,P1,P2,Rx1;
unsigned int  *pmem=0;
unsigned int  flagc,flagt;
int TIMER1,TIMER2,TIMER3,pw,nw;
int count1,count2,count3,speed2[Len2],i,j,k,n;
int sp[];
int *ppw =(int *)0x1002; 
int *npw =(int *)0x1004;
int  *ds1 =(int *)0x1005;
int  *ds2 =(int *)0x1006;
int  *ds3 =(int *)0x1007;
int  *ds4 =(int *)0x1008;

double deta,duty,Rx,x;

double e0,e1,e2,y0,y1,y2;
double Kp = 5;     
double Ti = 1.0;   
double Td = 0.2;
double T = 1;
double a0,a1,a2;                

void cpu_init()                  //     PLLNDIV=0  PLLMUL=F時      CLKOUT 為(1/4)*CLK
{                                //     PLLNDIV=0  PLLMUL=0－14時  CLKOUT 為(1/2)*CLK
	asm(" NOP");                 //     PLLNDIV=1  PLLMUL=F時      CLKOUT 為1*CLK
	asm(" STM #0, CLKMD");       //     PLLNDIV=1  PLLMUL=0－14時  CLKOUT 為(PLLMUL+1)*CLK
	                             //           PLLMUL           PLLNDIV
	asm(" STM #0, CLKMD");       //    0xf007=1111 0000 0000 0111
	asm(" STM #0xf007, CLKMD");  //    確定CLOCKOUT=1/(10M*1);
	asm(" rpt  #0ffffh");
	asm(" nop");
	
	PMST=0x3FA0;
	SWWSR=0xffff;
	SWCR=0x0000;
	IMR=0;
	IFR=IFR;
	asm(" stm 1000h,ar1");
	asm(" stm 1006h,ar6");
	
} 

interrupt void int1()   
{
	flagc=flagc^1;
	count1++;       
	count2++;
	count3++;
	
} 

void set_int()    
{ 
	asm(" ssbx intm"); 			//                    TSS TDDR                        
	TCR0=0x0b1b;       			//0x0b1b=0000 1011 0001 1011
     			                //0x4e1f=19999;  
	PRD0=0x9c3;                 //定時T=0.0025S
	IMR=IMR|0x000a;  
	IFR=IFR;           			//                         TSS  TDDR
	TCR0=0x0b29;       			//tcr0=0x0b29=0000 1011 0010 1001開定時器并且
	                   			//定時器定時為TIM0=CLOCKOUT*(PRD0+1)*(TDDR+1)
	asm(" rsbx intm"); 			//                =(1/10M)*(09999+1)*(9+1) 
}                      			//                =0.01S

void dis_buff(unsigned int Rx)
{   
   
//    Rx=Rx*60;
    dis1 = Rx%1000%100%10;
    dis2 = Rx%1000%100/10;
    dis3 = Rx%1000/100;
    dis4 = Rx/1000;   
      
    *ds1=led_buff[dis1];
    asm(" stm 1005h,ar1"); 
    asm (" nop ");
    asm (" nop ");  
    asm(" portw *ar1,8001h");
    asm (" nop ");
    asm (" nop ");
    delay(20);
    
    *ds2=led_buff[dis2];
    asm(" stm 1006h,ar2");
        asm (" nop ");
    asm (" nop ");
    asm(" portw *ar2,8002h");
    asm (" nop ");
    asm (" nop ");
    delay(20);
    
    *ds3=led_buff[dis3];
    asm(" stm 1007h,ar3");
        asm (" nop ");
    asm (" nop ");
    asm(" portw *ar3,8003h");
    asm (" nop ");
    asm (" nop "); 
    delay(20);
    *ds4=led_buff[dis4];
    asm(" stm 1008h,ar4");
        asm (" nop ");
    asm (" nop ");
    asm(" portw *ar4,8004h");
    asm (" nop ");
    asm (" nop "); 
    delay(20);
}


interrupt void tint0()
{  

    TIMER1++;
	TIMER2++;
	TIMER3++;
	if(TIMER1==40)    			/* 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)*39.0);
		deta = (a0*e0 + a1*e1 + a2*e2)*duty;
		
		pw = pw + 10000*deta;
		nw = nw - 10000*deta;

		if (pw >= 10000) pw = 10000*0.9;
		if (nw >= 10000) nw = 10000*0.9;
		if (pw <= 0)     pw = 10000*0.1;
		if (nw <= 0)     nw = 10000*0.1;
		
		*ppw = pw;
		*npw = nw;
       
         
		TIMER1 = 0;
        count1 = 0;
	 }
		
	if (TIMER2==400)   			/*  to test the speed of motor in 1s */
	{
	   	
		flagt = flagt^1;
		if (j<Len2) 
		 x=speed2[j]= count2;   /* store  the speed of motor */
		 
		 add=add+count2;
		 if(P2==0)
		 Rx=Rx1;
		 if(x==0)
		 {P1=0;
		  P2=0;}
	     else 
		 {P1=1;
		  P2=1;}
		 P0=add*P1/j;
		 P=x-P0; 
		if(j<10)
		  Rx=P0 * 60;
		if(10<=j<Len2)
		  {Rx1=Rx+P;
		  Rx=(Rx+P)*P2;}
		 TIMER2 = count2 =0;     
		j++;
	}
	
    	if (j>=Len2)
	 {*ppw = 0; 
	  *npw = 10000; 
	  j=j;
	  dis_buff(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 = 5000;
	nw = 5000;
	*ppw = pw;
	*npw = nw;
	flagc = flagt = 0;
	count1 = count2 = count3=0;
	TIMER1 = TIMER2=TIMER3 =0;
	for (j=0; j<Len2; j++)
	{speed2[j]=0;}

	i=j=k=0;

	Start();

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

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

void delay(unsigned int num)
{
    int i,j;
    for (i=0;i<=50;i++)
     {
      for (j=0;j<=num;j++);
     }
}
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(" rete");       /* int0 */
   asm(" nop");
   asm(" nop");
   asm(" nop");
   asm(" b _int1");       /* 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");
}