#include <xc.h>
#define _XTAL_FREQ 8000000 	// 8MHz

#define TRUE 1
#define FALSE 0
#define TURN_L 1
#define TURN_R 0
#define UINT8	unsigned char
#define UINT16 unsigned int
#define UINT32 unsigned long

__CONFIG(
	FOSC_INTOSCIO &	// Internal osc.
	WDTE_OFF &		// Disable watch dog timer
	PWRTE_ON &		// Enable power on timer
	BOREN_ON &		// Enable brown out
	MCLRE_OFF &		// Disable ~MCLR
	CP_OFF &			// Disable Code protection
	IESO_OFF &		// Disable IE switch over
	FCMEN_OFF			// Disable Fail safe clock
	);

UINT8	gPot = 0;
UINT8	gPos = 0;

#define PULSE		GP2
#define SENS		GP3
#define OUTB		GP4
#define OUTA		GP5

#define HH()	{OUTA=1;OUTB=1;}
#define LH()	{OUTA=0;OUTB=1;}
#define LL()	{OUTA=0;OUTB=0;}
#define HL()	{OUTA=1;OUTB=0;}


/** -----------------------------------------------------------------
Prototype
------------------------------------------------------------------ */
void rotate(int dir);
void motor();

/** -----------------------------------------------------------------
 void main(void)
------------------------------------------------------------------ */
void main(void)
{
	OPTION_REG = 0b10000010;	// No PU
	WPU        = 0b00000000;	// No PU
	OSCCON     = 0b01110000;	// 8MHz
	TRISIO     = 0b00001101;	// GP5/GP4/GP3/GP2/GP1/GP0
	ANSEL      = 0b01010001;	// Enable AN3 and Tad(Fosc/16)=2.0us
	CMCON0     = 0x7;			// Disable comparator
	ADCON0     = 0b00000001;	// = Left justified/VRef VDD/Select AN0
	CCP1CON    = 0b00000000;	// CCP1 reset

	OUTA = 0;
	OUTB = 0;

	// Main loop
	int ii;

	__delay_ms(3000);

	// serch start pos
	while( 0 == SENS ) {
		rotate( TURN_R );
	}

	while( 0 != SENS ) {
		rotate( TURN_L );
	}
	
	gPos == 0;
	
	// 200 step right rotate
	for( ii = 0; ii < 200; ii++ ) {
		rotate( TURN_R );
		GP1 = GP3;
	}	
	
	// 200 step left and go to ready pos
	for( ii = 0; ii < 200; ii++ ) {
		rotate( TURN_L );
		GP1 = GP3;
	}
	
	// ready to start
	for( ii = 0; ii < 10; ii++ ) {
		GP1 = !GP1;
		__delay_ms(200);
	}	

	while(1) {
		GP1 = GP3;

		ADCON0     = 0b00000011;
		__delay_us(100);
		while(GO_DONE);	// watit for adcon Tad*11=22us
		gPot = ADRESH;

		if( gPot > gPos ) {
			rotate( TURN_R );
			gPos++;
		}			
		else if( gPot < gPos ) {
			rotate( TURN_L );
			gPos--;
		}
	}
}

void rotate(int dir)
{
	static signed char phase = 0;
	
	if( dir ) {
		phase++;
		phase %= 4;
	}	
	else
	{
		phase--;
		if( phase < 0 ) phase = 3;
	}	

	switch( phase ) {
		case 0: HH(); break;
		case 1: LH(); break;
		case 2: LL(); break;
		case 3: HL(); break;
		default: break;
	}	
	__delay_ms(5);
}