#include <ez8.h>

void forward(int ontime1, int offtime1);
void reverse(int ontime1, int offtime1);
void left(int ontime1, int offtime1);
void rotateL(int ontime1, int offtime1);
void right(int ontime1, int offtime1);
void rotateR(int ontime1, int offtime1);

int a, b, d;

void main()
{
	int input;
	a=0;

//	PAADDR=0x01;
	PADD=0xff;			//Port A all input
//	PBADDR=0x01;
	PBDD=0x00;			//Port B all output
//	PCADDR=0x01;
	PCDD=0xff;			//Port C all input

	input=PAIN;
	input=input&0x70;

/*	PB truth table:			hex value:
	0000 = stop						0x00
	0010 = left						0x02
	1111 = reverse					0x0f
	1110 = rotateL					0x0e
	1000 = right					0x08
	1011 = rotateR					0x0b
	1010 = forward					0x0a

	Sensor truth table:		hex value:
	000 = reverse				  	0x00
	001 = rotateR					0x01
	010 = forward					0x02
	011 = right						0x03
	100 = rotateL					0x04
	101 = 							0x05
	110 = left						0x06
	111 = forward					0x07
*/
	while(1)
	{
		while(input==0x00)
		{
			for(b=0;b<10;b++){
			reverse(400,800);}
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x10)
		{
			rotateR(700,500);
			a=1;
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x20)
		{
			forward(250,950);
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x30)
		{
			rotateR(600,600);
			a=1;
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x40)
		{
			rotateL(700,500);
			a=0;
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x50 && a==1)
		{
			rotateR(700,500);
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x50 && a==0)
		{
			rotateL(700,500);
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x60)
		{
			rotateL(600,600);
			a=0;
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x70)
		{
			forward(250,950);
			input=PAIN;
			input=input&0x70;
		}
			input=PAIN;
			input=input&0x70;
	}
}		

void reverse(int ontime1, int offtime1)
{
	PBOUT=0x0f;			// reverse
	for(d=0;d<ontime1;d++){}
	PBOUT=0;				//	stop
	for(d=0;d<offtime1;d++){}
}

void left(int ontime1, int offtime1)
{
	PBOUT=0x02;			// rotateL
	for(d=0;d<ontime1;d++){}
	PBOUT=0;				//	stop
	for(d=0;d<offtime1;d++){}
}

void rotateL(int ontime1, int offtime1)
{
	PBOUT=0x0e;			// rotateL
	for(d=0;d<ontime1;d++){}
	PBOUT=0;				//	stop
	for(d=0;d<offtime1;d++){}
}

void right(int ontime1, int offtime1)
{
	PBOUT=0x08;			// rotateR
	for(d=0;d<ontime1;d++){}
	PBOUT=0;				// stop
	for(d=0;d<offtime1;d++){}
}

void rotateR(int ontime1, int offtime1)
{
	PBOUT=0x0b;			// rotateR
	for(d=0;d<ontime1;d++){}
	PBOUT=0;				// stop
	for(d=0;d<offtime1;d++){}
}

void forward(int ontime1, int offtime1)
{
	PBOUT=0x0a;			// forward
	for(d=0;d<ontime1;d++){}
	PBOUT=0;				//	stop
	for(d=0;d<offtime1;d++){}
}