/*
    Servo Bank controller

    This file gets position data from the main controller
    and keeps the servos updated

	Interrupt Version

	2004 Jason Hunt
*/

#include <int16cxx.h>

#pragma config |= 0x3FF0

//reverse servoes for the left side
#define left_side 

/* assign names to port pins */
bit led @ PORTB.5;
bit data @ PORTB.1;
bit clock @ PORTB.2;
bit ready @ PORTB.3;


// Base time offsets
#ifdef left_side
const unsigned servo_base[] =  {80, 190, 75, 180, 88, 190};
#else
const unsigned servo_base[] =  {200, 100, 194, 98, 145, 90};
#endif

// or with this to turn servo[x] on
const unsigned servo_on_mask[] =  {0x01, 0x02, 0x04, 0x08, 0x40, 0x80};


// and with this to turn servo[x] off
const unsigned servo_off_mask[] = {0xff - 0x01, 0xff - 0x02, 0xff - 0x04, 
								   0xff - 0x08, 0xff - 0x40, 0xff - 0x80 };

unsigned servo_pos[6];		   // positions of servos

unsigned cur_servo;				// the servo that the int is working on
unsigned int_mode;				// the state of the intrrupt handler


/* the ISR to update the servos */
#pragma interruptSaveCheck  n
#pragma origin 4
interrupt ISR(void)
{
    int_save_registers

	if (int_mode == 0) // setupup for base pos wait
	{

		int_mode = 1; 					   // next trigger we will setup for base delay
		OPTION = 2;   					   // reset back into 1:4 prescaled mode
		TMR0 = servo_base[cur_servo];      // set base time delay		
   		PORTA |= servo_on_mask[cur_servo]; // turn this one on

	} else
	if (int_mode == 1) 
	{

		OPTION = 1;						// set to prescale for pos timer
		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)
	{
		int_mode = 0;						// reset back to base mode
  	    PORTA &= servo_off_mask[cur_servo]; // turn the current servo off

		cur_servo++; // next servo

		// check for the longer delay to fill the 20 ms window
		if (cur_servo == 6)
		{
			cur_servo = 0;	// reset back the first servo
			OPTION = 5;		// ser the prescale for a longer delay time
			TMR0 = 30;		// set the value for the longer delay
		}
	}

    T0IF = 0; // clear int flag

    int_restore_registers
}
/* end of ISR */


/* reset all servoes to middle positions */
void home_servos( void )
{
	// home the servos
    servo_pos[0] = 128;  servo_pos[1] = 128;    servo_pos[2] = 128; 
    servo_pos[3] = 128;  servo_pos[4] = 128;    servo_pos[5] = 128; 
}
/* end of home_servos */


/* gets a clocked byte from the data and clock lines */
/* data is send LSB first */
char get_byte(void)
{
	char count = 8;     // the number of bits
	char ch = 0;       // the temporary char 

    while (count)
    {
        ch = ch >> 1;    // shift the byte right

        while (!clock);
		ready = 0;

        if (data) ch |= 0x80; // data is ored with the MSB

        while (clock);
		ready = 1;

        count--;
    }

	return(ch);
}
/* end of get byte */



/* main function */
void main(void)
{
	PORTA = 0;  // clear the ports
	PORTB = 0;

	TRISB = 0xD7;  // bit 6, 7 input   all others, output
	TRISA = 0x00;
	CMCON = 0x07;  // switch comparators off

	cur_servo = 0;
	int_mode = 0;

	home_servos(); // reset to base positions

    TMR0 = 0;  /* 45 * 2 = 90 periods */
    T0IE = 1;   /* enable TMR0 interrupt */
    GIE = 1;    /* interrupts allowed */
    OPTION = 2; /* prescaler divide by 2 */

	ready = 1;


	while (1)
	{
		// see of the master is sending a command
    	if (clock) 
    	{
			char command = get_byte();

			if (command == 0x00) // start byte
			{
				char i,t;
				for (i = 0; i < 6; i++)
				{	
					t = get_byte();

#ifdef left_side
					servo_pos[i] = t;
#else
					servo_pos[i] = 255 -  t;
#endif
				}
			}
	    }
    } 

}
/* end of main */