/*
** Program main.C
**
*/

#include <16F876.H>
#include <CTYPE.H>

#fuses   HS,NOPROTECT,NOWDT
#use delay (clock = 20000000)

#define COMPASS			1
#define SONAR			0

#define MESSAGE_SIZE		8
byte messageByte [MESSAGE_SIZE];

//compass pins
#define COMPASS_SDO    		PIN_C4
#define COMPASS_RESET 		PIN_B1
#define COMPASS_PC		PIN_B2
#define COMPASS_EOC		PIN_B0
#define COMPASS_SCLK   		PIN_B4

//compass variable
long compass_data = 0;
byte compass_done = 0;

//sonar pins
#define SONAR_INIT 		PIN_B3
#define SONAR_ECHO		PIN_C1
#define SONAR_BINH		PIN_B5

//sonar variable
long time = 0;
long distance = 0;
byte sonar_done = 0;

/************* compass *****************/
void reset_compass(){

	//set the signal lines
	output_high(COMPASS_RESET);
	output_high(COMPASS_PC);
	output_high(COMPASS_SCLK);

	//reset the compass
	output_low(COMPASS_RESET);

	//minimum 10 msec delay needed
	delay_ms(10);

	output_high(COMPASS_RESET);

	//minimum 500 msec delay before the first read
	delay_ms(500);

}


void read_compass(){

	//give a pulse to compass
	output_low(COMPASS_PC);

	//minimum 10 msec delay needed
	delay_ms(10);

	output_high(COMPASS_PC);

}


#int_ext
void compass_ready() {
   	short int j;
	int i;

	//output_high(COMPASS_RESET);

	//delay at least 10 msec before reading
	delay_ms(10);

	//PC and SS are connected together!
	output_low(COMPASS_PC);

	//create the first 8 cycles
	for ( i = 0; i < 8; i++) {
		output_low (COMPASS_SCLK);
		delay_us(15);
		output_high (COMPASS_SCLK);
		delay_us(15);
		j = input(COMPASS_SDO);
		compass_data = (compass_data<<1) | j;

	}

	//this delay is application dependent
	delay_ms(5);

	//create the second 8 cycles
	for ( i = 0; i < 8; i++) {
		output_low (COMPASS_SCLK);
		delay_us(15);
		output_high (COMPASS_SCLK);
		delay_us(15);

		j = input(COMPASS_SDO);
		compass_data = (compass_data<<1) | j;
	}

	//set SS high again (SS and PC are connected)
	output_high(COMPASS_PC);
	compass_done = 1;

}


#use rs232(baud=19200, xmit=PIN_C6, rcv=PIN_C7)

/************* sonar *******************/
void read_sonar() {

	output_high(SONAR_INIT);

	// start TIMER1
	set_timer1(0);

	delay_us(900);

	//add the blanking signal to here
	output_high(SONAR_BINH);

	// wait for ECHO to go high and get the time
	while(!input(SONAR_ECHO));

	time = get_timer1();

	delay_ms(100);

	output_low(SONAR_INIT);
	output_low(SONAR_BINH);

	delay_ms(100);

	sonar_done = 1;
}


void transmit_data(long data, byte source){
	int j;
	byte byte5, byte6;


	messageByte[4] = source;
	messageByte[5] = data >> 8;
	messageByte[6] = data;

//	printf("source %x : %x%x\n\r", source, messageByte[5], messageByte[6]);

	for (j = 0; j < MESSAGE_SIZE; ++j){

		putchar(messageByte[j]);
	}


	if (messageByte[1] < 7)
		++messageByte[1] ;

	else
		messageByte[1] = 1;


}

void main(){

	int i;

	messageByte[0] = 50;   	//To/From
	messageByte[1] = 1;	//packet ID
	messageByte[2] = 5;	//message length
	messageByte[3] = 2;	//start of message indicator
	messageByte[7] = 3;	//end of message indicator

	//compass setup functions
	ext_int_edge( l_to_h );
	enable_interrupts(global);
	enable_interrupts(ext_int);
	reset_compass();

	//sonar setup functions
	setup_timer_1( T1_INTERNAL|T1_DIV_BY_8);

	while(TRUE){

		read_compass();

		if (compass_done){
			transmit_data(compass_data, COMPASS);
			compass_done = 0;
		}

		//delay for RF
		//delay_ms(125);

		read_sonar();
		if (sonar_done){
			// calculate distance = (time * 0.0008)*345
			distance = (time * 0.276);
			transmit_data(distance, SONAR);
			sonar_done = 0;
		}

		//delay for RF
		delay_ms(250);
	}

}