/**
 * Analog radio control servo driver on the pic16f628a.
 * Controls 7 channels.  Demands a 4MHz clock.
 *
 * Timer 0 runs in a continuous loop causing interrupts at roughly 30hz.
 * Every interrupt kicks it into a time-delay loop, pausing anywhere
 * between one millisecond to two milliseconds.
 *
 * The position is controlled over RS232 by sending three binary bytes.
 * 0xaa:  Just to let it know legit data is coming.
 *	  The old version didn't have this, and you could send it
 *	  into lala-land by sending one extra byte.
 *
 * Addr:  A value from 0-255.  Values 0-7 will control a motor local to 
 *        the chip, values >=8 will cause it to subtract 8 from the 
 *        address and forward to the next chip.
 *
 * Pos:   A position from 0-255.
 *
 * It *WOULD* have been eight channels except the MCLR pin can't 
 * be configured as an output, booo.
 */

// We needed to use 2400 baud instead of 9600 to prevent overruns when
// a computer sent lots of commands.
#define	BAUD	2400L
#define BAUD_HI	0

#define INSTANTIATE_DELAY
#define INSTANTIATE_SERIAL

#define __16f628a
#include "pic16f628a.h"
#include "tsmtypes.h"
#include "tsmdelay.h"
#include "tsmserial.h"

#undef CONFIG_WORD
// We want to make sure we have _INTOSC_OSC_NOCLKOUT so we get pins
// RA6 and RA7 as output.  Surely two extra channels is worth it.

#define CONFIG_WORD	_INTOSC_OSC_NOCLKOUT&_WDT_OFF&_LVP_OFF&_DATA_CP_OFF&_PWRTE_ON&_BOREN_OFF&_MCLRE_ON

// Set the __CONFIG word:
// I usually set it to _EXTCLK_OSC&_WDT_OFF&_LVP_OFF&_DATA_CP_OFF&_PWRTE_ON
Uint16 __at  0x2007  __CONFIG = CONFIG_WORD;

// Saves precalculated values for the delay loop.
volatile Uint8 delay[8];
// Used by the interrupt handler to decide which pin to pulse next.
static Uint8 which;

static void Intr(void) __interrupt 0
{
	static volatile Uint8 bit;

	// Always check what interrupt you got.  Makes it easier when
	// you have to start dealing with multiple ones.
	if(T0IF)
	{
		TMR0=96;	// Increase interrupt speed a bit by
				// artificially bumping the timer0 count.
				// This gets it up to about 30hz or so.
				// We can't bump it much higher or we'll
				// start missing interrupts.

		T0IF = 0;	// Clear the Timer 0 interrupt.

		which++;
		which &= 0x07;

		// We have to put it into 'bit' before we put it into PORTA.
		// Otherwise it'll try to "optimize" this into
		//	PORTA = 1 ; PORTA <<= which
		// ...which will cause unwanted pulses on other lines.
		bit = 1 << which;

		PORTA=bit;
		// The minimum delay for each CALL_SMALL_US_U8 is 14 
		// microseconds at 4Mhz, so subtract that from 1000L.
			bit=delay[which];
			DELAY_BIG_US(1000L - (14L * 4L));
			CALL_SMALL_US_U8(bit);
			CALL_SMALL_US_U8(bit);
			CALL_SMALL_US_U8(bit);
			CALL_SMALL_US_U8(bit);
		PORTA=0;
	}
//	GIE=1;		// Globally enable interrupts.
			/** We don't need to do this ourselves since
			 *  the compiler ALWAYS ADDS THIS FOR US
			 *  in interrupt functions!
			 *  If you try and DISable interrupts in an
			 *  interrupt function it WON'T WORK since
			 *  the compiler ALWAYS turns them back ON!
			 */
}

enum
{
	TMR0_4,	//0
	TMR0_6,	//1
	TMR0_8,	//2
	TMR0_16,//3
	TMR0_32,//4
	TMR0_64,//5
	TMR0_128,//6
	TMR0_256,//7
};

void main(void)
{
	Uint8 pick, d;
	TRISA = 0x00;	// All Port A latch outputs are enabled.

	TRISB = 0x0f;	// B0-B3 inputs, B4-B7 outputs.

#ifdef  __16f628a	// Only compile this section for PIC16f628a
	CMCON = 0x07;	/** Disable comparators.  NEEDED FOR NORMAL PORTA
			 *  BEHAVIOR ON PIC16f628a!
			 */
#endif

//	T0CS = 0;	// Clear to enable timer mode.
//	PSA = 0;	// Clear to assign prescaler to Timer 0.

//	PS2 = 0;	// Set up prescaler to 1:16.  
//	PS1 = 1;
//	PS0 = 1;

	// Combine all the above mess into one operation.
	OPTION_REG = TMR0_16;

	INTCON = 0;	// Clear interrupt flag bits.
	GIE = 1;	// Enable all interrupts.

	T0IE = 1;	// Set Timer 0 to 0.  
	TMR0 = 0;	// Enable peripheral interrupts.

	ASYNC_INIT();
	CREN=1;		// Enable reception.

	// With 0 being one end stop and 255 being the other, start
	// the servo near the far end.

	// You'd think a loop here would save us code space, wouldn't you?
	// But making it a loop makes this BIGGER!
	SAVE_SMALL_US_U8(delay[0], 200L+14L);
	SAVE_SMALL_US_U8(delay[1], 200L+14L);
	SAVE_SMALL_US_U8(delay[2], 200L+14L);
	SAVE_SMALL_US_U8(delay[3], 200L+14L);
	SAVE_SMALL_US_U8(delay[4], 200L+14L);
	SAVE_SMALL_US_U8(delay[5], 200L+14L);
	SAVE_SMALL_US_U8(delay[6], 200L+14L);
	SAVE_SMALL_US_U8(delay[7], 200L+14L);

	// Loop forever.  
	while(1)
	{
		PORTB=0x00;	// LED off while expecting address

		// Byte 1:  0xaa preamble
		while(!RCIF);	pick=RCREG;
		// If we didn't get the preamble, ignore and start over
		if(pick != 0xaa)	continue;

		GIE=0;	// We might miss a pulse or two.  so what.
		// Byte 2:  Address.
		while(!RCIF);	pick=RCREG;

		PORTB=0xf0;	// Light a LED when expecting position data

		// Byte 3:  position data
		while(!RCIF);
		d=RCREG;

		if(pick < 8)
		{
			// Change location of a local servo
			SAVE_SMALL_US_U8(delay[pick], (d)+14L);
		}
		else	// Retransmit to chain on more servo controllers.
		{
			pick -= 8;
			SEND(0xaa);
			SEND(pick);
			SEND(d);
		}

		GIE=1;
	}
}