#include <reg52.h>

#define PWMPort		P1

sbit GetDataFlag 	= P2^1;
sbit LocationFlag	= P2^0;

static unsigned int PWM[8] = {0xF450, 0xF450, 0xF450, 0xF450, 0xF450, 0xF450, 0xF450, 0xF450};
static unsigned int Servo0Local1;
static unsigned int Servo0Local2;
static unsigned int Servo0Flag;
static unsigned int Servo0SpeedMinus;
static unsigned int Servo0SpeedPlus;
static unsigned char PWMCMD, PWMChIndex, PWMMASK, Servo0Speed;
static unsigned char Location;

code unsigned char PWMDisable[8] = {0xFE, 0xFD, 0xFB, 0xF7, 0xEF, 0xDF, 0xBF, 0x7F};
code unsigned char PWMEnable[8] = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};

#define CBYTE ((unsigned char volatile code *)0)
#define DBYTE ((unsigned char volatile idata *) 0)

/*------------------------------------------------
Timer 0 Interrupt Service Routine.
------------------------------------------------*/
void timer0int(void) interrupt 1
{
	PWMCMD = PWMCMD & PWMDisable[PWMChIndex];
	PWMPort = PWMCMD;
	TR0 = 0;
}

/*------------------------------------------------
Timer 1 Interrupt Service Routine.
------------------------------------------------*/
void timer1_ISR (void) interrupt 3
{
	TR1 = 0;
	TH1 = (65536-4982)>>8;
	TL1 = (65536-4982);
	TR1 = 1;
	PWMChIndex++;
	if (PWMChIndex == 0x08)
	{
		PWMChIndex = 0x00;
		PWM[0] += Servo0Flag;
	}
	PWMCMD = ( PWMCMD | PWMEnable[PWMChIndex] ) & PWMMASK;
	TH0 = (unsigned char)(PWM[PWMChIndex] >> 8);
	TL0 = (unsigned char)(PWM[PWMChIndex]);
	PWMPort = PWMCMD;
	TR0 = 1;
}


/*------------------------------------------------
MAIN C function
------------------------------------------------*/
void main (void)
{
/*--------------------------------------
8通道循序控制SSC伺服機控制器
解析度:
1) 0.5ms ~ 2.475ms / 分成 252步 (命令值 0x01 ~ 0xFC ) 
2) 以 2 byte 輸入, 支援 0.2ms ~ 2.45ms 每1us 調整

程式說明:
Timer0 用來產生 High duty （mode1)
Timer1 用來產生 2.5ms 來輪流控制 8個 channel (mode1)
Timer2 用來當 UART 的鮑率產生器 
P1 當輸出控制 8 通道信號
*/

//---------------------------

//---------------------------

	PWMMASK = 0xFF;
	PWMChIndex = 0x00;
	Servo0Flag = 0x0020;
	Servo0Local1 = 0xF000;
	Servo0Local2 = 0xF63C;
	Servo0SpeedPlus = 0x0020;
	Servo0SpeedMinus = 0xFFE0;

//Set the Timer1 Run control bit.
//--------------------------------------
	TMOD = 0x99;  /* Set Mode (8-bit timer with reload) */
	ET0 = 1;                      /* Enable Timer 0 Interrupts */
	TR0 = 1;                      /* Start Timer 0 Running */
	ET1 = 1;                      /* Enable Timer 1 Interrupts */
	TR1 = 1;                      /* Start Timer 1 Running */
	EA = 1;                       /* Global Interrupt Enable */

/*--------------------------------------
Do Nothing.  Actually,
--------------------------------------*/
	while (1)
	{
		if (Servo0Local2 >= 0xFC18)
		{
			Servo0Local2 = 0xFC18;
		}
		if (Servo0Local1 >= 0xFC18)
		{
			Servo0Local1 = 0xFC18;
		}
		if (Servo0Local2 <= 0xEC78)
		{
			Servo0Local2 = 0xEC78;
		}
		if (Servo0Local1 <= 0xEC78)
		{
			Servo0Local1 = 0xEC78;
		}
		if (Servo0SpeedPlus >= 0x00F0)
		{
			Servo0SpeedPlus = 0x00F0;	
		}
		if (Servo0SpeedMinus <= 0xFF10)
		{
			Servo0SpeedMinus = 0xFF10;
		}


		if (PWMChIndex != 0x00)
		{
			if (Servo0Flag <= 0x0100)
			{
				if (Servo0Local2 <= PWM[0])
				{
					Servo0Flag = Servo0SpeedMinus;
				}
			}
			else
			{
				if (Servo0Local1 >= PWM[0])
				{
					Servo0Flag = Servo0SpeedPlus;
				}
			}
		}
		if (GetDataFlag)								//P2^1
		{
			if (LocationFlag)							//P2^0
			{
				Location = P0;
			}
			else
			{
				switch (Location)
				{
					case 0x31:
						PWMMASK = P0;
						break;
					case 0x32:
						DBYTE[0x0013] = P0;				//Servo0Local1(High)
						break;
					case 0x33:
						DBYTE[0x0014] = P0;				//Servo0Local1(Low)
						break;
					case 0x34:
						DBYTE[0x0015] = P0;				//Servo0Local2(High)
						break;
					case 0x35:
						DBYTE[0x0016] = P0;				//Servo0Local2(Low)
						break;
				    case 0x36:
						DBYTE[0x0019] = P0;				//Proximity Light CRTL(High)
						break;
					case 0x37:
						DBYTE[0x001A] = P0;				//Proximity Light CRTL(Low)
						break;
					case 0x38:
						DBYTE[0x001B] = P0;				//Light Sensor CRTL(High)
						break;
				    case 0x39:
						DBYTE[0x001C] = P0;				//Light Sensor CRTL(Low)
						break;
					case 0x3A:
						DBYTE[0x0011] = P0;				//Servo0SpeedPlus(High)
						break;
					case 0x3B:
						DBYTE[0x0012] = P0;				//Servo0SpeedPlus(Low)
						break;
					case 0x3C:
						DBYTE[0x000E] = P0;				//Servo0SpeedMinus(High)
						break;
					case 0x3D:
						DBYTE[0x000F] = P0;				//Servo0SpeedMinus(Low)
						break;
				}
				Location = 0;
			}
		}
	}
}