_BODEN_OFF   
_PWRTE_ON
_WDT_OFF
_LVP_OFF
_MCLRE_OFF
_HS_OSC

// vars used only for TB_Servo1 firmware
unsigned char servo_toggle;	// to toggle servo position

//=============================================================================
//  SERVO MOVE
//=============================================================================
void servo_move(unsigned char servo, unsigned int period)
{
   	//-------------------------------------------------
	// used in TB_Servo1 tutorial firmware.
	// this function tells a servo (0-7) to move to position
	// set by servo pulse length (a period of 1000uS-2000uS
	// for most standard servos)
	//
	// The interupt moves servos automatically, all this function
	// does is put the period values in the MSB and LSB for the
	// 16bit servo pulse-hi period. The interrupt requires that
	// the 16bit period be in 0.2uS resolution, so the orig
	// period in uS must be multiplied by 5.
	// Also, the value must be inverted to (0-period) as
	// it is directly used to generate a period using TMR1.
   	//-------------------------------------------------
	// auto shut down that servo if period==0
	if(!period)
	{
		sv8_pulselength_msb[servo] = 0;		// servo is now OFF
	}
	else  // else move the servo
	{
		// force safe limits for servo pulse lengths!
		if(period > 2200) period = 2200;
		if(period < 800) period = 800;

		// convert period from uS to 0.2uS resolution (period*5)
		// use special *5 math because it compiles smaller
		period = ((period << 2) + period);

		// convert period to (0-period) and put in MSB and LSB
		period = (0 - period);
		sv8_pulselength_msb[servo] = (period / 256); 	// get MSB
		sv8_pulselength_lsb[servo] = (period & 0x00FF); // get LSB
	}
}
//-----------------------------------------------------------------------------

   	//-------------------------------------------------
	// for TB_Servo1 firmware only; Move servo0 to full left,
	// and turn the TMR1 int on (enables servos).

	servo_move(0,1000);		// move servo0 to 1000uS (=1mS, is full left)
	servo_toggle = 0;		//

	// and turn servo interrupt on
	INTCON = 0b11000000;		// global ints on, TMR1 (PEIE) on

   	//-------------------------------------------------

	// if BUT2 was quick pressed, move servo and play a sound!
	if(t0_count > 4)	// 4 gives small BUT2 debounce
	{
		// this is for TB_Servo1 firmware, each time the
		// button BUT2 is pressed is toggles the servo position
		// and plays a sound.
		if(!servo_toggle.F0)
		{
			servo_move(0,2000);	// move servo0 to 2000uS (full right)
			play_btc_lib_sound(1);	// and play sound1 twice
			play_btc_lib_sound(1);
		}
		else
		{
			servo_move(0,1000);	// move servo0 to 1000uS (full left)
			play_btc_lib_sound(0);	// and play sound0 twice
			play_btc_lib_sound(0);
		}
		servo_toggle++;
	}
	t0_count = 0;	// reset BUT2 debounce count