#include <16f88.h>

#fuses HS,NOWDT,NOPROTECT,NOLVP,NOBROWNOUT

#use delay(internal=8000000)

//see datasheet for register definitions
#byte portb=6
#byte trisb=0x86

#bit led=portb.3
#bit left=portb.1
#bit right=portb.0

//General Variables
long delayms=1000;

long High = 5;
long Low = 6;

long SpinHigh = 5;
long SpinLow = 6;



long FastHigh = 11;
long FastLow = 3;

long cycleCount=0;

//Function Prototypes   
void blinkLeds(void);
void init(void);

void Forward(long inches);
void SpinRight(float degrees);
void SpinLeft(long degrees);

//Main Routine
void main()
{
    init();

	blinkLeds();

	Forward(5);
	SpinRight(90);
	Delay_ms(100);
    
	Forward(5);
	SpinRight(90);
	Delay_ms(100);
    
	Forward(5);
	SpinRight(90);
	Delay_ms(100);

	Forward(5);
	SpinRight(90);
	Delay_ms(100);

	Forward(3);
	SpinLeft(360);
	Delay_ms(10);


blinkLeds();
}


//Support Functions
void blinkLeds()
{
	int loops = 0;
   	for(loops=10;loops>=1;loops--)
	{
	  led=0;
      delay_ms(100);

      led=1;
      delay_ms(100);
	}
}


void blinkLedsOther()
{
    output_low(led); //ccs macro
	delay_ms(delayms);

    output_high(led);
	delay_ms(delayms);
}


void init()
{
	
	int loops;
	portb = 0b00000000;
	trisb = 0b00000000;
	//set_tris_b(0);  //ccs macro for tris register
	
	//10 quick blinks to check for repeated reset
	for(loops=10;loops>=1;loops--)
	{
	  led=0;
      delay_ms(100);

      led=1;
      delay_ms(100);
	}
}


void SpinRight(float degrees)
{
	float x = 0;
	float factor = .81;
    float amount = 0;
	
	amount = degrees * factor;

	for(x=0;x<amount;x=x+1)
	{
	  right=1;
//	  left=1;
	  delay_ms(SpinHigh);

	  right=0;
	  left=0;
	  delay_ms(SpinLow);
	}

}

void SpinLeft(long degrees)

{
	float x = 0;
	float factor = .7;
    float amount = 0;
	
	amount = degrees * factor;

	for(x=0;x<amount;x=x+1)
	{
//	  right=1;
	  left=1;
	  delay_ms(SpinHigh);

	  right=0;
	  left=0;
	  delay_ms(SpinLow);
	}

}

void Forward(long inches)
{
	float x = 0;
	float factor = 11; //translates inches to loops
	float amountF = 0;
	
	amountF = inches * factor;

	for(x=0;x<amountF;x=x+1)
	{
	  right=1;
	  left=1;
	  delay_ms(High);

	 
	  left=0; 
	  right=0;
	  delay_ms(Low);

	  //modify x with feedback her?ett
	}
}