#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 d;

void main()
{
	int input;

	PBADDR=0x01;
	PBCTL=0x00;
	PAADDR=0x01;
	PACTL=0xff;
	PCADDR=0x01;
	PCCTL=0xff;

	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					0x02
	010 = forward					0x04
	011 = right						0x06
	100 = rotateL					0x08
	101 = reverse					0x0a
	110 = left						0x0c
	111 = reverse					0x0e
*/
	while(1)
	{
		while(input==0x00)
		{	
			reverse(100,1100);
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x10)
		{
			rotateR(800,400);
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x20)
		{
			forward(450,750);
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x30)
		{
			right(1100,100);
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x40)
		{
			rotateL(800,400);
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x50)
		{
			reverse(100,1100);
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x60)
		{
			left(1100,100);
			input=PAIN;
			input=input&0x70;
		}
		while(input==0x70)
		{
			reverse(100,1100);
			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++){}
}