/*	
	Sonar bot source.
	(C) 2005 Jason Hunt

	Based on:

	ORPP Motion Controller Source
	(C) 2005 Jason Hunt nulluser@gmail.com

	16f871.h has been modified as follows:


	// original
	// #pragma chip PIC16F871, core 14, code 2048, ram 32 : 0xBF /0 /3

	#pragma chip PIC16F871, core 14, code 0x1F00, ram 32 : 0x1FF
	#pragma cdata[0] = 0x0180 + 0x0A // CLRF PCLATH, prepare for code page change
	#pragma cdata[1] = 0 // NOP
	#pragma cdata[2] = 0 // NOP
	#pragma resetVector 3 // start address for user program

	This reserves space for the serial bootloader.

*/

#define start_string	"ORPP-1. (C) 2005 Jason Hunt"


#include <int16cxx.h>

#pragma chip PIC16f871
#pragma config |= 0x3FF2 // for ceramic resinator

/* Pin Assignments */

#pragma bit led0 @ PORTB.6
#pragma bit led1 @ PORTB.7

#pragma bit digital_in_0 @ PORTB.0
#pragma bit digital_in_1 @ PORTA.4
#pragma bit digital_in_2 @ PORTE.0
#pragma bit digital_in_3 @ PORTE.1
#pragma bit digital_in_4 @ PORTE.2

#pragma bit radio_in_0 @ PORTD.6
#pragma bit radio_in_1 @ PORTD.7

#pragma bit open_collector_0 @ PORTB.4
#pragma bit open_collector_1 @ PORTB.5

#pragma bit pwm_out_0 @ PORTB.1
#pragma bit pwm_out_1 @ PORTB.2


#define num_servo	6						// number of servos connected to the board


unsigned char servo_pos[num_servo];			// positions of the 8 servos

unsigned char pwm_value_0;
unsigned char pwm_value_1;



/*******************
** Interrupt Code **
*******************/

#pragma interruptSaveCheck  n
#pragma origin 4

unsigned char int_mode;
unsigned char cur_servo;
unsigned char pwm_cycle;

unsigned char counter_h;
unsigned char counter_l;



/* the ISR to update the servos */
interrupt ISR(void)
{
    int_save_registers

    // check for timer zero interrupt
	if (T0IF)
	{
		if (int_mode == 0) // setupup for base pos wait
		{
			int_mode = 1; 					   // next trigger we will setup for base delay
			OPTION = 4;   					   // reset back into 1:16 prescaled mode

			OPTION &= 0xf8;						// clear prescaler bits
			OPTION |= 0x04;						// set to 1:32 prescaler

			TMR0 = 0xa0;   // set base time delay		
			
			unsigned char s = 1 << cur_servo; // create mask

	   		PORTC |= s; // turn this one on

		} else
		if (int_mode == 1) 					// setup for position hold
		{
			int_mode = 2;					// next mode will turn off and check for long delay
			TMR0 = servo_pos[cur_servo];	// reset the timer to the pos
	
		} else
		if (int_mode == 2)					// setupfor next servo
		{
			int_mode = 0;						// reset back to base mode

			unsigned char s = 1 << cur_servo;  // create mask

			s = 255 - s;

	  	    PORTC &= s; // turn the current servo off
	
			cur_servo++; // next servo
		
			// Setup longer delay to fill the 20 ms window, 
			// we need to burn 11ms on average
			// 5Mhz clock, 0.0512 MS per increment at 1:256 prescaler
			// 11ms / 0.0512 ms = 215 cycles to burn, start time at 40

			if (cur_servo == num_servo)
			{
				cur_servo = 0;	// reset back the first servo

				OPTION &= 0xf8;		// clear prescaler bits
				OPTION |= 0x07;		// set to 1:256 prescaler

				TMR0 = 40;		// set the value for the longer delay
			}
		}
	    T0IF = 0; // clear timer zero interrupt

	} else
    // check for timer one interrupt
/*	if (TMR1IF)	
	{
//		TMR1L = 0x00;			// reset timer
//		TMR1H = 0x80;

		TMR1IF = 0; // clear flag
	} else*/
	if (TMR2IF)	
	{
		if (pwm_cycle > 0x40)
		{
			counter_l++;
			if (!counter_l) counter_h++;
		}

		pwm_cycle++;

		if (pwm_cycle == 0 && pwm_value_0) pwm_out_0 = 1; // cycle start, make sure pwm is nonzero
		if (pwm_cycle == 0 && pwm_value_1) pwm_out_1 = 1;

		// turn outputs off after their duty has passed
		if (pwm_cycle > pwm_value_0) pwm_out_0 = 0;
		if (pwm_cycle > pwm_value_1) pwm_out_1 = 0;

		TMR2 = 0xe0; // reset timer  this is about 500 hz

		TMR2IF = 0; // clear flag
	}

    int_restore_registers
}
/* end of interrupt service routine */

/**************************
** End of interrupt Code **
**************************/


/***********************
**	Serial functions **
***********************/

/* Set the serial port to  9600 8N1 */
void setup_serial(void)
{
//	SPBRG = 129;   // divisor for 9600 baud
	SPBRG = 64;    // divisor for 19200 baud

    TXSTA = 0b.0010.0100;  // transmitt enable, high speed
	RCSTA = 0b.1001.0000; // serial enable

	char ch = RCREG;  // flush buffer
	ch = RCREG;
	ch = RCREG;
}
/* end of setup serial */


/* sends a byte to the serial port */
void send_serial(unsigned char byte)
{
	while (!TRMT);	// wait until buffer is empty	

	TXREG  = byte;					// send the byte
}
/* end of send byte */


/* sends a byte to the serial port */
unsigned char get_serial(void)
{
	while (!RCIF);

	return(RCREG);
}
/* end of send byte */


/* sends 4 bits in hex format */
void send_nibble( unsigned char n )
{
    if (n > 9)
        send_serial(n - 10 + 'A');
    else
        send_serial(n + '0');
}
/* end of send_nibble */


/* sends a byte as hex */
void send_hex(unsigned char d)
{
	send_nibble(d >> 4);
	send_nibble(d & 0xf);
}
/* end of send_hex */


/* sends a byte as hex */
void linefeed( void )
{
	send_serial(0x0d);
	send_serial(0x0a);
}
/* end of send_hex */


/* gets a hex digit from the serial port */
unsigned char get_hex_nibble( void )
{
	unsigned char c = get_serial(); // force upper case

	if (c > '9')
	{
		c &= 0xdf;
		return(c - 'A' + 10);			
	}
	else
		return(c - '0');
}
/* end of get_hex_nib */


/* gets a byte in hex from the serial port */
unsigned char get_hex(void)
{
	unsigned char val;

	val = get_hex_nibble() << 4;

	return(val + get_hex_nibble());
}
/* end of get_hex */



/*****************************
**	End of serial functions **
*****************************/


/*******************
** Initialization **
*******************/

/* system startup */
void startup( void )
{
	PORTA = 0;			// clear ports
	PORTB = 0;
	PORTC = 0;
	PORTD = 0;
	PORTE = 0;

	TRISA = 0b.1111.1111;	// all inputs
	TRISB = 0b.0000.0001;	// bit 0 is digital in 2, bit 3 is led1
	TRISC = 0b.1000.0000;	// bit 7 is rx
	TRISD = 0b.1100.0000;	// D6, D7 radio in, all others digital out
	TRISE = 0b.0000.0111;	// E0-E2 digital in

	// setup a/d
	ADCON1 = 0b.1000.0010;	// RE0, 1, 2 : digital RA0,1,2,3,5 : analog

	ADCS1 = 1;				// Fosc/32 clocl
	ADCS0 = 0;

	ADON = 1;					// turn on converter

//		ADCON0 = 0b.1000.0001 | mask;

	// clear core values before interrupts are turned on

	int_mode = 0;			// finite state machine for servo control
	cur_servo = 0;			// current servo number
	pwm_cycle = 0;			// state of pwm system

	pwm_value_0 = 0;
	pwm_value_1 = 0;

    GIE = 1;				// interrupts allowed 
	PEIE = 1;				// enable periphial interrupts

	// setup timer 0
    OPTION = 4;				// prescaler divide by 16
	T0CS = 0;				// internel clock
    TMR0 = 0;				// clear the timer
    T0IE = 1;				// enable interrupt for timer 0


	// setup timer 1
	TMR1L = 0;			// reset timer
	TMR1H = 0;

	TMR1ON = 0; 			// shut off timer to modify values

	T1CKPS1 = 1;			// setup prescaler
	T1CKPS0 = 0;
	T1OSCEN = 0;			// shut off oscillator

	TMR1CS = 0;				// internal clock
//	TMR1IE = 1;				// enable interrupt

	TMR1ON = 1; 			// turn timer on
	TMR1IF = 0;				// clear interrupt flag

	TMR2 = 0xe0;
	TMR2ON = 1;				// enable timer 2
	TMR2IE = 1;

	servo_pos[0] = 0x80;	// setup inital servo positions
	servo_pos[1] = 0x80;	
	servo_pos[2] = 0x80;
	servo_pos[3] = 0x80;	
	servo_pos[4] = 0x80;	
	servo_pos[5] = 0x80;
}
/* end of startup */

/**************************
** End of Initialization **
**************************/



/***********************
**	Utility functions **
***********************/


/* general use delay function */
void delay(unsigned char time)
{
	TMR1H = 255 - time;
	while(TMR1H);
}
/* end of delay */



/******************************
**	End of Utility functions **
******************************/



/****************************************
**	Serial command interface functions **
****************************************/



unsigned int get_sonar( void )
{
//	unsigned char n = get_hex_nibble(); 

    PORTD |= 0x01;   // or mask

	delay(0xa0);

	PORTD &= 0xfe;   // or mask

	TMR1H = 0;
	TMR1L = 0;
	while(!digital_in_0&& TMR1H < 0x80);

	TMR1H = 0;
	TMR1L = 0;

	while(digital_in_0&& TMR1H < 0x80);

	unsigned char h = TMR1H;

    return(h);
}


void ddelay(unsigned int d)
{
	unsigned int i, k, j;

	for (i = 0; i < d ; i++);

}

int turncheck()
{
		servo_pos[1] = 0x40;

		unsigned int countl = 0;
		unsigned int counth = 0;

		while(counth < 0x20) { countl++; if (!countl) counth++; ddelay(4); }

		unsigned int side_1 = get_sonar();


		servo_pos[1] = 0xb0;

		countl = 0;
		counth = 0;

		while(counth < 0x20) {countl++; if (!countl) counth++; ddelay(4); }


		unsigned int side_2 = get_sonar();

		servo_pos[1] = 0x80;

		while(counth < 0x40) {countl++; if (!countl) counth++; ddelay(4); }


		return(side_1 - side_2);

}

void stop(void)
{
	open_collector_0 = 1;	// reverse motor dir, stops faster
	pwm_value_1 = 0;


	counter_h = 0;
	while(counter_h < 0x80);
}


#define sonar_stop 0x0b

/* main function */
void main ( void )
{
	startup();

	delay(0xff); // wait for serial to stabilize

	setup_serial();

	linefeed();	// make sure start message is on the same line 

	// infinte loop

	//int mode = 0;
	unsigned int countl = 0;
	unsigned int counth = 0;


	while (1)
	{
		// moving forward checking sonar, wheels straight

		pwm_value_1 = 0x68;		// speed
		open_collector_0 = 0;	// moving forward
		servo_pos[0] = 0x90;   // steering straight

		servo_pos[1] = 0x80;

		while(get_sonar() > sonar_stop); // forward

		// stop
		stop();


		int t = turncheck();

		if (t > 0)
			servo_pos[0] = 0xb0;
		else
			servo_pos[0] = 0x50;


		// reverse
		pwm_value_1 = 0xa0;
		open_collector_0 = 1;


		// delay for reverse
		counter_h = 0;
		while(counter_h < 0x60);

	
		stop();


		if (t < 0)
		{
			servo_pos[0] = 0xb0;
			servo_pos[1] = 0xb0;
		}
		else
		{
			servo_pos[0] = 0x50;
			servo_pos[1] = 0x40;
		}


		counter_h = 0;
		while(counter_h < 0x20);

		//	forward and turn

		pwm_value_1 = 0x90;
		open_collector_0 = 0;


		counter_h = 0;

		while (counter_h < 0x60 && get_sonar() > sonar_stop);  // turn

 	}

}
/* end of main */