#include <c8051F020.h>
#include <stdio.h>
#include <math.h>

int bf,x,i,tl01,th01,tl00,th00,tl11,th11,tl10,th10,j,flag;
double pw1,a0,a1,b0,b1,c0,c1,d0,d1,y0,y1,z0,z1,angle, k, pw0;
#define SYSCLK 22118400 // SYSCLK frequency in Hz
#define BAUDRATE 9600 // Baud rate of UART in bps

void config()
{
     unsigned int i;
     WDTCN = 0xDE;
     WDTCN = 0xAD;
     OSCXCN=0x67;
     for (i=0;i<255;i++);
	 for (i=0;i<255;i++);
            OSCICN=0x88;
	    XBR0 = 0x04;
	    XBR1 = 0x1E;
        XBR2 = 0x40;
		P0MDOUT |= 0x03;
        CKCON = 0x10; //timer 0 & 2 use system clock divided by 12, timer 1 uses system clock
}

void timer0reloadsetup()
{
    // angle = SBUF0; //when postive is turn to left, when negative is turn to right
     k = (angle / 6);
     pw0 = (k / 10000);
     c0 = d0 = 0;
     c0 = (65535 - (((0.0013 + pw0) * 22110000)/12)); 
     d0 = (65535 - (((0.01925-(0.0013+pw0)) * 22110000)/12));
     y0 = z0 = 0;
     y0 = z0 = c0;
     tl01 = (int) y0 & 0x00FF;
     th01 = (int) z0 & 0xFF00;
     th01 = th01 >> 8;
     th01 = th01 & 0x00FF;
     a0 = b0 = 0;
     a0 = b0 = d0;
     tl00 = (int) a0 & 0x00FF;
     th00 = (int) b0 & 0xFF00;
     th00 = th00 >> 8;
     th00 = th00 & 0x00FF; 
}

void t0_ini()
{
     EA=0;
     TF0=0;
     TR0=0;
     IT0=1;
     IE0=0;
     TMOD=0x99;
     TH0=th01;    
     TL0=tl01;
     ET0=1;
     EX0=0;
     EA=1;
}

void UART0_Init ()
{
   SCON0=0x50;                     // SCON0: mode 1, 8-bit UART, enable RX
   TMOD=0x20;                     // TMOD: timer 1, mode 2, 8-bit reload
   TH1=-(SYSCLK/BAUDRATE/16);     // set Timer1 reload value for baudrate
   TR1=1;                         // start Timer1
   PCON |=0x80;                      // SMOD00 = 1
   ES0=1;
   //IP=0x10;
   TI0=1;
}

/*void t3_ini()
{
     EA=0;
     TMR3CN=0x02;  //timer3 use system clock
     TMR3H=0xA9;   //1ms
     TMR3L=0xA1;
     TMR3RLH=0xA9; //auto reload value 1ms
     TMR3RLL=0xA1;
     EIE2=0x01;
     EA=1;
}*/

void main()
{ 
     while (1)
	 {
	 EA=1;
     WDTCN = 0xde;                       // disable watchdog timer
     WDTCN = 0xad;
	 config();
	 UART0_Init();
	 if (x==1)
	 {
	 timer0reloadsetup();
     t0_ini();
	 TR0=1;
	 flag=1;
     P2=1;
	 while (i==10)
	 {
	 i++;
	 }
	 }
	 }
}

void UART() interrupt 4
{
    if (RI0)
    {
   		RI0=0;
		TI0=0;
        angle=0;
        angle=SBUF0;
		x=1;
   	}
}

void int0() interrupt 1
{
     P2=!P2;
	 {
	  if (flag==0)
	 {
	     TH0=th01;
    	 TL0=tl01;    
		 flag=1;
	 }
	 else
	 {
	     TH0=th00;
    	 TL0=tl00;    
		 flag=0;
	 }
     }
}
/*void int3() interrupt 14
{    
     TMR3CN=0x06;
     if (j==100)
     {
	  TR0=0;
    //TMR3CN=0x02;
      j=0;
	 }
	 else 
	 {
      j=j++;
 	 }
}*/