
/*
    Robot Controller Source
	(C) 2006 Jason Hunt nulluser@gmail.com

	Version 0.01

	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.
*/

#include <int16cxx.h>

#pragma chip PIC16f871
#pragma config |= 0x3FF2 			// For 20 mhz ceramic resinator


#define NUM_RECV_BYTES	11			// Bytes in control packet
#define NUM_SEND_BYTES	20			// Bytes in telemetery packet

#define NUM_SERVOS	6	   		    // Number of servo outputs

#define NUM_ANALOG 8

/* Pin Assignments */

#pragma bit led_0 @ PORTB.6
#pragma bit led_1 @ PORTB.7

//#pragma bit encoder_0 @ PORTB.0 // digital in 0
//#pragma bit encoder_1 @ PORTA.4
//#pragma bit encoder_2 @ PORTE.0
//#pragma bit encoder_3 @ PORTE.1

#pragma bit encoder_0 @ PORTD.0 // digital in 0
#pragma bit encoder_1 @ PORTD.1
#pragma bit encoder_2 @ PORTD.2
#pragma bit encoder_3 @ PORTD.3


//#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

/* Pin assignments for PWM */
#pragma bit pwm_out_0 @ PORTB.1
#pragma bit pwm_out_1 @ PORTB.2
#pragma bit pwm_out_2 @ PORTB.5
#pragma bit pwm_out_3 @ PORTB.4

/* Data buffers for serial */
unsigned char recv_buffer[NUM_RECV_BYTES];
unsigned char send_buffer[NUM_SEND_BYTES];

unsigned char servo_pos[NUM_SERVOS];		// Servo Positions
unsigned char pwm_value[4];					// PWM Dutysw

unsigned char analog_low[8];				// Analog low bytes
unsigned char analog_high[8];				// Analog high bytes


unsigned char cur_analog;					// Current analog capture

unsigned char servo_mode;			// Current state of the servo interrupt system
unsigned char cur_servo;			// Current servo 
unsigned char pwm_cycle;			// CUrrent PWM cutoff value

unsigned char cur_recv_byte;		// Index of recv buffer
unsigned char cur_send_byte;		// Index of send buffer
unsigned char read_timeout;			// Timeout counter
bit reading_data;					// True if reading

unsigned char wait_analog;			// True if waiting to start converstion

unsigned char update_timeout;

unsigned char running_counter;
unsigned char recv_counter;
bit last_valid;


bit last_encoder_0;
bit last_encoder_1;
bit last_encoder_2;
bit last_encoder_3;



unsigned char last_dir;

unsigned char encoder_0_count;
unsigned char encoder_1_count;
unsigned char encoder_2_count;
unsigned char encoder_3_count;

unsigned char encoder_0_speed;


unsigned char encoder_low[4];
unsigned char encoder_high[4];


unsigned char run_count;

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

#pragma interruptSaveCheck  n
#pragma origin 4





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

    if (RCIF)
    {
        RCIF = 0;
		reading_data = 1;
		read_timeout = 0;

		// Eat data and timeout if sync is off
		if (!last_valid)
		{
			recv_buffer[cur_recv_byte++] = RCREG;  
			cur_recv_byte = 0;
		}


		// Send current telemetry data for the first char recevied
        // This will utilize full duplex for better transfer
		if (cur_recv_byte == 0 && last_valid)
		{	   
            // Package current telemetry data for transmittion
			// Start byte
		    send_buffer[0] = 0xff;

			// Low bytes of analog data
			send_buffer[1] = analog_low[0];
			send_buffer[2] = analog_low[1];
			send_buffer[3] = analog_low[2];
			send_buffer[4] = analog_low[3];
			send_buffer[5] = analog_low[4];
			send_buffer[6] = analog_low[5];
			send_buffer[7] = analog_low[6];
			send_buffer[8] = analog_low[7];

			// Shift MSB of analog data to fit into two bytes
			send_buffer[9] = analog_high[0] | (analog_high[1]<<2) ;
			send_buffer[9] += analog_high[2]<<4;

			unsigned int tmp = analog_high[3] * 64;
			send_buffer[9] += tmp;


			send_buffer[10] = analog_high[4] | (analog_high[5]<<2) ;
			send_buffer[10] += analog_high[6]<<4;

			tmp = analog_high[7] * 64;
			send_buffer[10] += tmp;

//			send_buffer[7] = analog_high[3] | (analog_high[4]<<2);

			// Other telemetry data
//			send_buffer[8] =run_count;// encoder_0_speed;

			send_buffer[11] = encoder_low[0];
			send_buffer[12] = encoder_high[0];

			send_buffer[13] = encoder_low[1];
			send_buffer[14] = encoder_high[1];

			send_buffer[15] = encoder_low[2];
			send_buffer[16] = encoder_high[2];

			send_buffer[17] = encoder_low[3];
			send_buffer[18] = encoder_high[3];

			// Stop byte
			send_buffer[19] = 0xff;

			TXIE = 1;					// Enable transmit to send packet 
		} 

		// Last char in packet
        if (cur_recv_byte >= NUM_RECV_BYTES)
        {
			cur_recv_byte = 0;


            if (recv_buffer[0] == 0xff && recv_buffer[NUM_RECV_BYTES-1] == 0xff)
            {
				// first byte is For digital controls.

				unsigned char dir = recv_buffer[1] & 0x03;

				if (dir != last_dir && recv_buffer[2] != 0 && recv_buffer[3] != 0)
				{
	led_0 = !led_0;
					last_dir = dir;

					encoder_high[0] = 0;
					encoder_low[0] = 0;

					encoder_high[1] = 0;
					encoder_low[1] = 0;

					encoder_high[2] = 0;
					encoder_low[2] = 0;

					encoder_high[3] = 0;
					encoder_low[3] = 0;

				}

				if (recv_buffer[1] & 0x01)
				{
					pwm_value[0] = recv_buffer[2];
					pwm_value[1] = 0;										
				}
				else
				{
					pwm_value[0] = 0;
					pwm_value[1] = recv_buffer[2];
				}

				if (recv_buffer[1] & 0x02)
				{
					pwm_value[2] = recv_buffer[3];
					pwm_value[3] = 0;
				}
				else
				{
					pwm_value[2] = 0;
					pwm_value[3] = recv_buffer[3];
				}


				// Copy servo data
				servo_pos[0] = recv_buffer[4];
				servo_pos[1] = recv_buffer[5];
				servo_pos[2] = recv_buffer[6];
				servo_pos[3] = recv_buffer[7];
				servo_pos[4] = recv_buffer[8];
				servo_pos[5] = recv_buffer[9];


				recv_counter++;
	
				if (recv_counter >= 10)
				{
					recv_counter = 0;
					//led_1 = !led_1;
				}


				last_valid = 1;
             } 
			else 
				last_valid = 0;

			reading_data = 0;

			update_timeout = 0; // Reset update timeout


		}
        else
    		recv_buffer[cur_recv_byte++] = RCREG;  // Normal char for packet
    } 

    // Ready for TX byte
    if (TXIF && TXIE)
    {
		// End of packet?
		if(cur_send_byte >= NUM_SEND_BYTES)
        {
			cur_send_byte = 0;					// Reset to first byte for next send
            TXIE = 0;							// Last byte has been sent, disable tx
		}
		else
			TXREG = send_buffer[cur_send_byte++]; // Send next byte
    }


    // check for timer zero interrupt
	if (T0IF)
	{

		if (servo_mode == 0) // setupup for base pos wait
		{
			servo_mode = 1; 					   // next trigger we will setup for base delay
			OPTION = 4;   					   // reset back into 1:32 prescaled mode

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

	   		PORTC |= s; // turn this one on

		} else
		if (servo_mode == 1) 					// setup for position hold
		{
    		servo_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 (servo_mode == 2)					// setupfor next servo
		{
			servo_mode = 0;						// reset back to base mode

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

	  	    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_SERVOS)
			{
				cur_servo = 0;	// reset back the first servo

				OPTION = 7;

				TMR0 = 40;		// set the value for the longer delay
			}
		}

	    T0IF = 0; // clear timer zero interrupt
	} 

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

		running_counter++;





		if (running_counter >= 20)
		{
run_count++;
			encoder_0_speed = encoder_0_count;
			encoder_0_count = 0;
	led_1 = !led_1;
			running_counter = 0;
		//	led_0 = !led_0;
		}

        if (reading_data)
        {
			read_timeout++;

			// The read has timed out
			if (read_timeout >= 4)
			{
				reading_data = 0;				// reset reading
				cur_recv_byte = 0;				// reset to current byte
				last_valid = 1;
	       }
		}


		// Update time out
		update_timeout++;

		// Too many timeouts have passed, kill motors
		if (update_timeout >= 30)
		{
			update_timeout = 30;
			pwm_value[0] = 0;
			pwm_value[1] = 0;
			pwm_value[2] = 0;
			pwm_value[3] = 0;
		}


		// Check for recevive error
    	if (OERR)
    	{
    		CREN = 0;  // reset recieve logic
    		CREN = 1;
    	}



		TMR1IF = 0; // clear flag
	} 

	if (TMR2IF)	
	{
//		led_0 = encoder_0;

		if (encoder_0 != last_encoder_0)
		{
			encoder_low[0]++;

			if (encoder_low[0] == 0)
				encoder_high[0]++;


			last_encoder_0 = encoder_0;
			encoder_0_count++;
		}

		if (encoder_1 != last_encoder_1)
		{
			encoder_low[1]++;

			if (encoder_low[1] == 0)
				encoder_high[1]++;

			last_encoder_1 = encoder_1;
			encoder_1_count++;
		}


		if (encoder_2 != last_encoder_2)
		{
			encoder_low[2]++;

			if (encoder_low[2] == 0)
				encoder_high[2]++;


			last_encoder_2 = encoder_2;
			encoder_2_count++;
		}

		if (encoder_3 != last_encoder_3)
		{
			encoder_low[3]++;

			if (encoder_low[3] == 0)
				encoder_high[3]++;

			last_encoder_3 = encoder_3;
			encoder_3_count++;
		}

		pwm_cycle++;

		// Check for cycle starts.
		// Do not set the pin if the value is zero
		if (pwm_cycle == 0 && pwm_value[0]) pwm_out_0 = 1; 
		if (pwm_cycle == 0 && pwm_value[1]) pwm_out_1 = 1;
		if (pwm_cycle == 0 && pwm_value[2]) pwm_out_2 = 1; 
		if (pwm_cycle == 0 && pwm_value[3]) pwm_out_3 = 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;
		if (pwm_cycle > pwm_value[2]) pwm_out_2 = 0;
		if (pwm_cycle > pwm_value[3]) pwm_out_3 = 0;

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

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

	//	TMR2 = 0x20;

		// See if we are waiting to start the A->D
		// Also, make sure it's not already running
		if (wait_analog && !GO)
		{
			// Reduce wait count
			wait_analog--;

			// Done waiting, start the converter
			if (wait_analog == 0) 
                GO = 1;
		}

		TMR2IF = 0; // clear flag
	}

	// The analog to digital converter is done
    if (ADIF)
    {  
		// Save current values
		analog_low[cur_analog] = ADRESL;
		analog_high[cur_analog] = ADRESH & 0x03;

		// Setup for next analog channel
     	cur_analog++;

		// Wrap arund back to first
    	if (cur_analog >= NUM_ANALOG) cur_analog = 0;

		// Create mask for new channel
    	unsigned char mask = cur_analog << 3;

    	ADCON0 &= 0b.1100.0111;
    	ADCON0 |= mask; // mask off channel bits, and set new channel

		// Setup to wait to fire the go signal
		wait_analog = 200;

		ADIF = 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 */




/*******************
** 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

	TRISD = 0b.0000.1111;	// D6, D7 radio in, all others digital out

	TRISE = 0b.0000.0111;	// E0-E2 digital in


//		ADCON0 = 0b.1000.0001 | mask;

	// clear core values before interrupts are turned on

    running_counter = 0;
    recv_counter = 0;


	servo_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;
	pwm_value[2] = 0;
	pwm_value[3] = 0;



	// 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;

   update_timeout = 0;

   reading_data = 0;
   read_timeout = 0;
   cur_recv_byte = 0; // Waiting for first byte
   cur_send_byte = 0; // Waiting for first byte
	last_valid = 1;

   RCIF = 0;	// Clear receive interrupt
   RCIE = 1; 	// Enable receive interrupts

   TXIF = 0;	// Clear receive interrupt
   TXIE = 1; 	// Enable receive interrupts


	// Analog to Digital Setup
    cur_analog = 0;

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

	ADCON1 = 0b.1000.0000;	// RE0, 1, 2 : digital RA0,1,2,3,5 : analog


	ADCS1 = 1;				// Fosc/32 clocl
	ADCS0 = 0;
	ADCON0 &= 0b.1100.0111;
	ADON = 1;					// turn on converter

    ADIF =0;
    ADIE =1;

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

encoder_0_count = 0;
encoder_0_speed = 0;


	encoder_low[0] = 0;
	encoder_high[0] = 0;
	encoder_low[1] = 0;
	encoder_high[1] = 0;
	encoder_low[2] = 0;
	encoder_high[2] = 0;
	encoder_low[3] = 0;
	encoder_high[3] = 0;

last_dir = 0;
run_count = 0;
}
/* end of startup */

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



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



/* deal with digital out command */
/*void digital_out_command ( void )
{
	unsigned char n = get_hex_nibble(); // get output number

	if (n > 5) return;	// range check

	unsigned shift = 1 << n; // create bit mask

	unsigned char v = get_hex_nibble(); // get value (on or off)

	if( v == 0)
	{
		shift = 255 - shift; // and mask
		PORTD &=  shift;
	}

	if (v == 1)
		PORTD |= shift;   // or mask
}*/
/* end of digital_out_command */

/* return the valur of a digital input */
/*bit get_digital_value(unsigned char n)
{
	if (n == 0) return(digital_in_0); else
	if (n == 1) return(digital_in_1); else
	if (n == 2) return(digital_in_2); else
	if (n == 3) return(digital_in_3); else
	if (n == 4) return(digital_in_4);

	return(0);
}*/
/* end of get_digital_value */


/***********************************************
**	End of serial command interface functions **
***********************************************/

/* Read data from sonar sensor */
// The parmeter is the input /output pair number
// for example passing '2' will use input 2 and output 2
//
/*void get_sonar_command( void )
{
	unsigned char n = get_hex_nibble(); 

	unsigned char mask = 1 << n;

    PORTD |= mask;   // turn output on

	delay(0x8);

	PORTD &= ~mask;   // turn output off

	TMR1H = 0;

	while(!get_digital_value(n) && TMR1H < 0x80);

	TMR1H = 0;
	TMR1L = 0;

	// Read the pulse width
	while(get_digital_value(n) && TMR1H < 0x80);

	unsigned char l = TMR1L;			// Save low order, it will change during transmittion

	send_hex(TMR1H);
	send_hex(l);
}*/
/* End of read sonar */





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

	setup_serial();

	while (1);	// infinte loop
}
/* end of main */
