/****************************************************************************
 Title	:   C  file for the compass functions (compass.c)
 Author:    Gorang Gandhi (gorang@ufl.edu)
 Date:	    July/24/2006
 Software:  AVR-GCC 
 Target:    Atmega 128
 Comments:  To be used with the CMPS03 compass
*****************************************************************************/

#include <avr/io.h>
#include "i2c.h"
#include "compass.h"

volatile unsigned char bearing;

//Compass Directions

#define North		192			// These constants vary with time/location, even after a recalibration
#define South		72
#define East		255
#define West		135

signed int bearing_8(uint8_t num)
// Returns average bearing of the compass.  Num is the number of samples taken.
{
	uint8_t k;
	uint16_t byte = 0;

	for(k = 0; k < num; k++)
	{
		I2C_START_TX(0xC0);
  	 	i2c_transmit(1);
   		I2C_START_RX(0xC0);

		byte = i2c_receive(I2C_QUIT) + byte; 
			                     
   		i2c_stop();
	}
	
	return byte/num;
}

uint16_t compass_rev(void)
// Returns the revision number- Rev 9.
{
	uint16_t data = 0;

	I2C_START_TX(0xC0);
  	i2c_transmit(0);
   	I2C_START_RX(0xC0);

	data = i2c_receive(I2C_QUIT); 
			                     
   	i2c_stop();

	return data;
}

void compass_calib(void)
// Used to calibrate the compass.  Writes 255 to register 15 at N,S,E,W
{
	I2C_START_TX(0xC0);
  	i2c_transmit(15);
   	//I2C_START_RX(0xC0);

	i2c_transmit(255);
			                     
   	i2c_stop();
}

void Turn_West(void)
// Turns to the West.  Only turns if not with in 100 of West.
// Alot of noise in compass readings
{
	bearing = bearing_8(10); 	//read compass

	if(bearing > North)	// If facing N to E
	{
		Turn_Left();
		Update_Motors();

		while(bearing > West)	// While not facing West	
		{
			bearing=bearing_8(10); 	//read compass
		}
	
		Stop_Motors();
	}

	else if(bearing < South)  // If facing S to E
	{
		Turn_Right();
		Update_Motors();

		while(bearing < West)	// While not facing West	
		{
			bearing=bearing_8(10); 	//read compass
		}
	
		Stop_Motors();
	}
}

void Turn_West_Perfect(void)
// Turns to the West.  Only turns if not with in 20 of West.
// Alot of noise in compass readings
{
	bearing = bearing_8(10); 	//read compass

	if(bearing > (West + 10))	// If facing NW to E
	{
		Turn_Left();
		Update_Motors();

		while(bearing > West)	// While not facing West	
		{
			bearing=bearing_8(10); 	//read compass
		}
	
		Stop_Motors();
	}

	else if(bearing < (West - 10))	// If facing SW to E
	{
		Turn_Right();
		Update_Motors();

		while(bearing < West)	// While not facing West	
		{
			bearing=bearing_8(10); 	//read compass
		}
	
		Stop_Motors();
	}
}

void Turn_North(void)
// Turns to the North.  Only turns if not with in 100 of North.
// Alot of noise in compass readings
{
	bearing = bearing_8(10); 	//read compass

/*
Never > East (255)
	if(bearing > East)	// If facing E to NE
	{
		Turn_Left();
		Update_Motors();

		while(bearing > North)	// While not facing North	
		{
			bearing=bearing_8(10); 	//read compass
		}

		Stop_Motors();
	}
*/
	if(bearing < West)		// If facing E to W
	{
		Turn_Right();
		Update_Motors();

		while(bearing < North)	// While not North	
		{
			bearing=bearing_8(10); 	//read compass
		}
	
		Stop_Motors();
	}
}

void Turn_North_Perfect(void)
// Turns to the North. Only turns if not with in 20 of North.  
// Alot of noise in compass readings
{
	bearing = bearing_8(10); 	//read compass

	if(bearing > (North + 10))	// If facing E to NE
	{
		Turn_Left();
		Update_Motors();

		while(bearing > North)	// While not facing North	
		{
			bearing=bearing_8(10); 	//read compass
		}

		Stop_Motors();
	}

	else if(bearing < (North - 10))		// If facing E to NW
	{
		Turn_Right();
		Update_Motors();

		while(bearing < North)	// While not North	
		{
			bearing=bearing_8(10); 	//read compass
		}
	
		Stop_Motors();
	}
}

void Turn_South(void)
// Turns to the South.  Only turns if not with in 100 of South.
// Alot of noise in compass readings
{
	bearing = bearing_8(10); 	//read compass

	if(bearing > West)	// If facing W to E
	{
		Turn_Left();
		Update_Motors();

		while(bearing > South)	// While not facing North	
		{
			bearing=bearing_8(10); 	//read compass
		}

		Stop_Motors();
	}

/*
	Very slim Case 0 to South - 50
	else if(bearing < (South - 50))		// If facing SE to E
	{
		Turn_Right();
		Update_Motors();

		while(bearing < South)	// While not North	
		{
			bearing=bearing_8(10); 	//read compass
		}
	
		Stop_Motors();
	}
*/
}

void Turn_South_Perfect(void)
// Turns to the South.  Only turns if not with in 20 of South.
// Alot of noise in compass readings
{
	bearing = bearing_8(10); 	//read compass

	if(bearing > (South + 10))	// If facing SW to E
	{
		Turn_Left();
		Update_Motors();

		while(bearing > South)	// While not facing North	
		{
			bearing=bearing_8(10); 	//read compass
		}

		Stop_Motors();
	}

	else if(bearing < (South - 10))		// If facing SE to E
	{
		Turn_Right();
		Update_Motors();

		while(bearing < South)	// While not North	
		{
			bearing=bearing_8(10); 	//read compass
		}
	
		Stop_Motors();
	}

}

void Turn_East(void)
// Turns to the East.  Only turns if not with in 100 of East.
// Alot of noise in compass readings
{
	bearing = 0;
	bearing = bearing_8(1); 	//read compass.  Reading it once fixes the problem.

	if((bearing > (South - 10)) && (bearing <= West))	// If facing SE to W
	{
		Turn_Left();
		Update_Motors();

		while(bearing > 10)	// While not facing East	
		{
			bearing=bearing_8(10); 	//read compass
		}
	
		Stop_Motors();
	}


	if((bearing < (North + 10)) && (bearing > West))  // If facing N to W
	{
		Turn_Right();
		Update_Motors();

		while(bearing < (East - 10))	// While not facing East	
		{
			bearing=bearing_8(10); 	//read compass
		}
	
		Stop_Motors();
	}
}

void Turn_East_Perfect(void)
// Turns to the East.  Only turns if not with in 20 of East.
// Alot of noise in compass readings
{	
	bearing = 0;
	bearing = bearing_8(10); 	//read compass

	if((bearing > 10)  && (bearing <= West))	// If facing SE to W
	{
		Turn_Left();
		Update_Motors();

		while(bearing > 10)	// While not facing West	
		{
			bearing=bearing_8(10); 	//read compass
		}
	
		Stop_Motors();
	}

	else if((bearing < (East - 10)) && (bearing > West))  // If facing NE to W
	{
		Turn_Right();
		Update_Motors();

		while(bearing < (East - 10))	// While not facing East	
		{
			bearing=bearing_8(10); 	//read compass
		}
	
		Stop_Motors();
	}
}