/*******************************************************************
*
* Architecture	Midrange PIC
*	Processor	16F873
*	Compiler	Hi-Tech PICC v9.65
*
********************************************************************
*
*	Files required:	none
*
********************************************************************
*
*	Description:	Bot with the Pololu motors
*
*	
*
********************************************************************
*
;	Port Pin Assignments
;					873/6	874/7
;	RA0/AN0			pin 2	pin 2	Ain		Analog input ch0, Sharp IR distance sensor
;	RA1/AN1			pin 3	pin 3	Ain		Analog input ch1	?nu
;	RA2/AN2/Vref-	pin 4	pin 4	Din		Sharp digital 5cm, right front
;	RA3/AN3/Vref+	pin 5	pin 5	Ain		Analog input ch3	?nu
;	RA4/T0CLI		pin 6	pin 6	Din		Sharp digital 5cm, left front
;	RA5/AN4/SS		pin 7	pin 7	Din		SPI slave select (~SS) ?nu - PIR sensor
;
;	RB0/INT			pin 21	pin 33	out		Motor 2 (left) 	BIN2
;	RB1				pin 22	pin 34	out		Motor 2 (left)	BIN1
;	RB2				pin 23	pin 35	out 	Motor 1 (right)	AIN1
;	RB3/PGM			pin 24	pin 36	out		Motor 1 (right)	AIN2
;	RB4				pin 25	pin 37	in		right wheel encoder-1OUTA, int on change
;	RB5				pin 26	pin 38	in		left  wheel encoder-2OUTA, int on change
;	RB6/PGC			pin 27	pin 39	in		left  wheel encoder-1OUTB, int on change ?nu
;	RB7/PGD			pin 28	pin 40	in		right wheel encoder-2OUTB, int on change ?nu
;
;	RC0/T1OSO/T1CLI	pin 11	pin 15	out 	STBY#
;	RC1/T1OSI/CCP2	pin 12	pin 16	out		Motor 2 (left) PWMA output, (Enable)
;	RC2/CCP1		pin 13	pin 17	out		Motor 1 (right) PWMB output, (Enable)
;	RC3/SCK/SCL		pin 14	pin 18	SSP		SPI SCK	?nu  Pyzo buzzer output
;	RC4/SDI/SDA		pin 15	pin 23	SSP		SPI SDI ?nu
;	RC5/SDO			pin 16	pin 24	in		SPI SDO ?nu
;	RC6//TX/CK		pin 17	pin 25	UART	Serial TX
;	RC7/RX/DT		pin 18	pin 26	UART	Serial RX
;
;	~MCRL			pin 1	pin 1		Vpp
;	Vdd				pin 20	pin 11&32
;	Vss				pin 8&19    12&31
;	OSC1/CLKIN		pin 9	pin 13
;	OSC2/CLKOUT		pin 10	pin 14
*
*
*
********************************************************************/
#include <htc.h>

__CONFIG ( HS & WDTDIS & PWRTEN & UNPROTECT & BORDIS & LVPDIS); // defs are in pic1687x.h

#define XTAL_FREQ 20000000
#include "Delay.h"

#define XTAL 20000000
#define _20_00CLK
#define fcyc XTAL /4

/*
;------------------------------------------------------------------------------
; Output to Dual H-bridge, Pololu TB6612FNG
; Port bits	pins (873)	Wiring
;                          ---------------
;                         |               |_1A________	
;   RC1,PWM	13       PWM -|          H-   |           )
;   RB2     23       IN1 -|        bridge |   motor 1 (	left side
;   RB3	    24       IN2 -|               |_1B________)
;                         |               |
;                         |               |_2A________
;   RC2,PWM	12       PWM -|               |           )
;   RB1     22       IN1 -|               |   motor 2 (	right side
;   RB0     21       IN2 -|               |_2B________)
;   RC0     11      STBY -|               |
;                          ---------------
;
;	Logic input                output
;  IN1   IN2   PWM   STBY   OUT1   OUT2   Mode
;   H     H    H/L	  H      L      L     short brake
;
;   L     H    H      H      L      H     CCW- rev		(lf-fwd)
;   L     H    L      H      L      L     short brake
;
;   H     L    H      H      H      L     CW- fwd
;   H     L    L      H      L      L     short brake
;
;   L     L    H      H     OFF (hi-Z)    stop
;   H/L   H/L  H/L    L     OFF (hi-Z)    Standby
;------------------------------------------------------------------------------
;
;	Right	Left	PortB
;	stop	stop	0000	0000
;	brk		brk		1111	1111
; 	brk		fwd		1110	1101
;	brk		rev		1101	1110
;	fwd		brk		1011	1011
;	rev		brk		0111	0111
;	fwd		fwd		1010	1001
;	rev		rev		0101	0110
*/
//-----------------------------------------------------------------------------
// Port descriptions
//---------------------------------------------------------------
// PORT A
#define porta_dir_mask	0xff	// AD input, bit 5 = PIR sensor (SPI SS#)
#define adcon0_mask		0x81	// Fosc/32, ch0, ADON
//#define adcon1_init_lr	0x00	// RA0,1,2,3,4 & RE1,2,3 = AD input,
								// left justify AD result (ADRESH = high 8 bits)
#define adcon1_init_lr	0x04	// RA0,1,3 = AD input, RA2,5, RE0,1,2 = Digital input
								// left justify AD result (ADRESH = high 8 bits)

#define ra_sensor_mask	0x34	// 0011 0100
#define ra_PIR_mask		0x20	// 0010 0000

#define rt_bump			2	// 0x04
#define lf_bump			4	// 0x10		
#define PIR_sensor		5	// 0x20		Low = triggered				
//---------------------------------------------------------------
// PORT B Definitions
// 3-0 = output
// 7-4 = input, 
// enable weak pull-ups, OPTION_REG, bit 7 = 0
// interrupt on change enabled, INTCON reg bit 3 = 1
// interrupts on only chage of input state, high or low going
#define portb_dir_mask	0xf0 	// 1111 0000b

// Wheel sensor definitions
#define Sensor_port PORTB

#define motorrt_in2	0	// IN2 to H-bridge
#define motorrt_in1	1	// IN1
#define motorlf_in1	2	// IN1
#define motorlf_in2	3	// IN2
#define rtwh_EncA 	0x10	// 0001 0000b
#define lfwh_EncA	0x20	// 0010 0000b
#define lfwh_EncB	0x40	// 0100 0000b
#define rtwh_EncB   0x80	// 1000 0000b
//#define rtbump_sw		6	// 0100 0000b, pin 27
//#define lfbump_sw		7	// 1000 0000b, pin 28
#define rb_sensor_mask		0xf0	// 1111 0000b, int on change

//---------------------------------------------------------------
// PORT C
//#define portc_dir_mask	0xd0	// bits 0 = ~CS, 1&2=PWM output, 3=SCK, 4=SDI, 5=SDO SPI, 6,7=UART
#define	SPI_CS			RA5
#define portc_dir_mask	0xf0	// bits 0 = STBY output, 1&2=PWM output, 3 pizo out, 4 in 5=input, 6,7=UART
#define portc_out_mask	0x01	//
//#define LED				0		// low = on
#define STBY			0		// low = motor driver standby
#define PIEZO			3 //?

// motor_state bit defines
#define	rtwhOn		0
#define	lfwhOn		1
#define	rtwhFwd		2
#define	lfwhFwd		3
#define rtwhOk		4
#define	lfwhOk		5

// Initial values and limits
#define	init_pwm_cnt	0x40	//motor start, 31% duty
#define Min_pwm_cnt		0x30	// 25% duty
#define	Max_pwm_cnt		0xFA	//250		// 98% duty
#define	init_wheel_cnt	0x0c	//0x1a	// target wheel speed (smaller value = faster)
#define Min_wheel_cnt	0x05	// ?
#define	Max_wheel_cnt	0x23	// ?
#define Kp				3		// Proportional term
#define Kd				1		// Derivative term
//----------------------------------------------------------------------------------------
// Pololu wheels have a code wheel of 16 white and 12 black segments per one revolution
// counted from start of black to start of next black (12 segments per revolution)

// Wheel diameter = 1.685 in, 5.29 in circumference [42.8mm dia, 134.4mm circ]
// reduction ratio = 100:1 gear box
// RPM, no load = 120 @ 6V

// Equations: 
//	wheel RPM = (linear speed * 60)/circum.
//	motor RPM = wheel RPM * reduction ratio
//	# wheel code counts per sec = (wheel RPM * 12)/60
//	time for 1 wheel code (msec/code) = 1000/code count
//	
// TMR0 interrupt counts the time per segment
// based on TRM0 at 2msec
//					20MHz	
// linear   wheel 	motor 		time /segs 	#counts/
// speed	RPM		RPM			(msec)		segment
// 1		13.17	1317		379			195
// 2		26.34	2634		189			98
// 3		39.51	3951		126			65
// 4		52.69	5268		94			49
// 5		65.86	6585		75			39
// 6		79.03	7902		63			33
// 7 		92.20	9220		54			28
// 8		105.37	10537		47			24
// 9		118.54	11854		42			22
// 10		113.34	11334		44			23
// 11		124.68	12467		40			21
//
// see BugBotCalc.xls for calculations
//
// Turning, one wheel stopped as Pivot			
//			
// Bot rotates	distance	# wheel		# encoder
// degrees		m			rotations	segments
//  22.5		0.036		0.269		2.2
//  45			0.072		0.538		4.3
//  90			0.145		1.076		8.6
//  120			0.193		1.434		11.5
//  180			0.289		2.152		17.2
//  270			0.434		3.227		25.8
//  360			0.579		4.303		34.4

//==============================================================================
// Register set-up values
//------------------------------------------------------------------------------
#define	option_init	0x06	// TMR0 prescaler = 1:128, RBUP on
#define	intcon_init	0xE8	//(GIE | T0IE | RBIE | PEIE)
#define cmcon_init	0x07	// comparadors off, PA = I/O
#define t1con_init	0x00	// T1CKPS1 | T1CKPS0		// _T1PRE_8

#define pie1_init	0x02 	// TMR2IE 
#define pie2_init	0x00

//---------------------------------------------------------------
// Timer init values
//------------------------------------------------------------------------------
// TMR0 timer and pre-scaler -
//
// timer counts down from set value, interrupts when count reaches zero
// time = (255 - tmr0_cnt) * prescaler * cycle time + interrupt latency
// pre-scale of 2 to 256, (2, 4, 8, 16, 32, 64, 128, 256)
// 
// prescaler = 1:2, set prescaler in option_reg to PS_2
//
// TMR0 prescale = 1:2, OPTION_REG, bits 2-0 = 000
// prescaler assignment = TMR0, OPTION_REG bit 3 = 0
// TMR0 clock source = Instr cycle, OPTION_REG, bit 5 = 0
//
#define	TMR0_cnt	96		// (256 - TMR0)*PRE = #inst cycles
							// = 320; time = 64 us
#define	Sys_tick	0x17	// TMR0 IRQ * Sys_tick = 99msec

//------------------------------------------------------------------------------
// TMR2 timer and pre-scaler -
//
#define pwm_mode_init	0x0c				// set in CCP1CON & CCP2CON
// T2CON: Timer2 control reg
// bit<6-3> Postscale 1-16, bit2 TMR2ON, bit<1-0> Prescale 1,4,16
//#define t2con_init	tmr2_post | tmr2_pre_19530	// post = 4, pre = 1
//#define t2con_init	0x3d	// post = 4, pre = 4, timer on
// #define t2con_init 0x01	// post=1, pre=4, timer stop
//#define t2con_init 	0x06	// post=1, pre=16, timer on
#define t2con_init 	0x1e	// post=4, pre=16, timer on

//#define _tmr2_post 		8					// value = 1 to 16
//#define tmr2_post		(_tmr2_post - 1) *8	//0x00	// post = 1
//#define tmr2_pre_4880 	_T2PRE_4			// prescale = 4
#define pr2_2440		0x7f		// for pre = 16
#define pr2_4880		0xff		// for pre = 4

//#define t2con_init	tmr2_post | tmr2_pre_4880	//post = 4, pre = 4
//---------------------------------------------------------------
// USART init values
#define	Sout	TXREG	// serial tx reg
#define	Sin		RCREG	// serial rec reg

// Serial port baud rates, based on 19.6608MHz clock & TXSTA<BRGH> = 1
#ifdef _19_66CLK
#define baudh_4800		255
#define baudh_9600		127
#define baudh_19_2k		63	
#define baudh_38_4k		31	
#define baudh_57_6k		20	
#endif

// Serial port baud rates, based on 20.000MHz clock & TXSTA<BRGH> = 1
#ifdef _20_00CLK
#define baudh_4800		255
#define baudh_9600		129
#define baudh_19_2k		64	
#define baudh_38_4k		31	
#define baudh_57_6k		20	// XBee baud rate
#define baudh_115_2k	10	
#endif
//---------------------------------------------------------------
// SPI init value
#define sspstat_init	0x40	// input sampled at middle, data trans of clk falling edge
#define sspcon_init		0x23	// Enable SPI, Master, clock = TMR2 output/2
#define	sspcon2_init	0x00	// NU (I2C)

//---------------------------------------------------------------
// Constants
#define	TX_BUF_LEN 40
#define RX_BUF_LEN 40



//==============================================================================
// 	Flags
//
//==============================================================================
// Wheel/motor control defines

// Define wheel encoder flags
// wh_encoder_flags
#define 	rtwh_cnt_f 		0	// new wheel encoder count
#define 	lfwh_cnt_f 		1	//
#define 	rtwh_cnt_ovf 	2	// wheel encoder count over flowed (16 bit)
#define 	lfwh_cnt_ovf 	3	//
#define 	rtwh_segcnt_f 	4	// the set code wheel segment counter has reached zero
#define  	lfwh_segcnt_f 	5	//
#define 	rtwh_invalid_f 	6	// counts invalid due to Starting up
#define 	lfwh_invalid_f 	7	//

// Define Action flags 
// static unsigned char	action_flags;
#define  	rt_bumped_f		0	// bump switch has closed. Digital Sharp IR
#define  	lf_bumped_f		1	//
#define		PIR_f			2	// 
#define		PIR_timeout_f	3	//
#define		SHARP_bump_f	4	//	Sharp distance too close

static bit  pwm_cnts_f;			// pmw counts reached zero

#define 	LEFT			1
#define		RIGHT			2
#define		FORWARD			3
#define		REVERSE			4

// Define the Time_out flags
// timeout_flags
#define 	rtwh_start_f	0		// process that requested the time-out counter services
#define 	lfwh_start_f	1		//
#define 	Sys_tick_f 		2		// 256 ticks of TMR0 = 51.2 msec
#define 	time_out_f		3		// TMR1 time-out, start timer when waiting for something
#define 	Task_tick_f		4		// 
#define 	unused5_to_f	5		//
#define 	unused6_to_f	6		//
#define 	EEPROM_busy_f	7		//	; set in wrtee and cleared in EE ISR

#define	ADch0	0		// RA0/AN0
#define	ADch1	1		// RA1/AN1
#define	ADch2	2		// RA2/AN2
#define	ADch3	3		// RA3/AN3
#define	ADch4	4		// RA5/AN4

#define Sharpe_D120 1

#ifdef Sharpe_D120
// Sharp GP2D120 IR distance sensor values for 8 bit ADC (left just, ADRESH)
#define Sharp_2in	0x70	// 2.2V
#define Sharp_3in	0x56	// 1.7V
#define Sharp_4in	0x42	// 1.3V
#define Sharp_5in	0x36	// 
#define Sharp_6in	0x2d	// 0.9V
#define Sharp_8in	0x23	// 0.7V
#define Sharp_10in	0x19	// 0.5V
#define Sharp_12in	0x14	//
#define Sharp_14in	0x11	//
#define Sharp_16in	0x0f	// 0.3V

#elif Sharp_A02
#define Sharp_2in	0x8d	// ^
#define Sharp_3in	0x8d	// ^
#define Sharp_4in	0x8d	// ^
#define Sharp_5in	0x8d	// less than minimum distance
#define Sharp_6in	0x8c	// 2.75V Minimum Distance
#define Sharp_8in	0x80	// 2.5V
#define Sharp_10in	0x73	// 2.25V
#define Sharp_12in	0x5F	// 1.9
#define Sharp_14in	0x50	// 1.6
#define Sharp_16in	0x46	// 0.3V
#define Sharp_20in	0x3c	// 1.2
#define Sharp_24in	0x34	// 1.0
#define Sharp_30in	0x29	// 0.8
#define Sharp_40in	0x21	// 0.66
#define Sharp_50in	0x19	// 0.5
#define Sharp_60in	0x17	// 0.45V
#endif

#define D_2in	0
#define D_3in	1
#define D_4in	2
#define D_5in	3
#define D_6in	4
#define D_8in	5
#define D_10in	6
#define D_12in	7
#define D_14in	8
#define D_16in	9
#define D_20in	10
#define D_24in	11
#define D_30in	12
#define D_40in	13
#define D_50in	14
#define D_60in	15

//======================================================================
// variables
static unsigned char SPI_rxdata;

// UART Rx and Tx ring buffers
static bit 				TxBufFull_f;
static bit 				TxBufEmpty_f;
static bit 				RxBufFull_f;
static bit 				RxBufEmpty_f;
static bit 				ReceivedCR_f;
static bit 				RxOverRerr_f;
static bit 				RxFrameErr_f;
static bit 				RxBuffOver_f;

static unsigned char 	TxByteCount;
static unsigned char 	RxByteCount;
static unsigned char 	BufferData;			// temp data store
static unsigned char 	*TxStartPtr;
static unsigned char 	*TxEndPtr;
static unsigned char 	*RxStartPtr;
static unsigned char 	*RxEndPtr;
static unsigned char 	RxBuffer[RX_BUF_LEN];
static unsigned char 	TxBuffer[TX_BUF_LEN];

// ADC flags
static bit  			Sharp_data_f;	//
static bit 				avoid_f;	//
static unsigned char	Sharp_voltage;
static unsigned char	Sharp_data;
static unsigned char	Sharp_Dist;
static unsigned char	Sharp_Temp;

static unsigned char	ra_temp;
static unsigned char	ra_change_bits;		// holds previous sensor inputs
static unsigned char	rb_temp;
static unsigned char	tmr2_temp;

static unsigned char	portB_image;		// port mirror, set/clear bits here then write value to port
static unsigned char	portC_image;		// to prevent any RMW issues

//static unsigned char	motor_flags;		// enable and dir of both motors
//static unsigned char	motor_state;		// the current state of the motors
static unsigned char	lf_motor_pwm_cnt;	// curent PWM cnt
static unsigned char	rt_motor_pwm_cnt;
static unsigned char	pwm_cnts;			// count of PWM (TRM2) pulses
//static unsigned char	rt_pwm_cnt;

static unsigned char	rb_change_bits;		// holds previous sensor inputs, for change bit detection
static unsigned char	wh_encoder_flags;	// holds wheel encode status
static unsigned char	action_flags;		// holds bump switch states
static char				direction;

//static unsigned char	BumpAction_state;	// hold the next Action state to enter
static unsigned char	timeout_flags;		// flag for with device/function requested the time-out timer
static unsigned char	timeout_cnt;		// count # tmr1 time-outs betfore setting time_out flag

static unsigned char	Sys_tick_counter;	// 2ms per tick per TMR0
static unsigned char	Sys_task_counter;

//static unsigned char	lfwh_sen_cntH;
static unsigned char	lfwh_sen_cntL;		// active count for one code segment, tmr0 ticks
//static unsigned char	rtwh_sen_cntH;
static unsigned char	rtwh_sen_cntL;		// 
//static unsigned char	rtwh_high_cnt;
static unsigned char	rtwh_low_cnt;		// total count for one code segment
//static unsigned char	lfwh_high_cnt;
static unsigned char	lfwh_low_cnt;		//
static unsigned char	lfwh_target_cnt;
static unsigned char	rtwh_target_cnt;	// target count for one code segment 
static unsigned char	lfwh_code_cnt;
static unsigned char	rtwh_code_cnt;		// number of code segments counted, measured movment distance
static char 			lfwh_target_err;
static char 			rtwh_target_err;
static char 			lfwh_target_prev_err;
static char 			rtwh_target_prev_err;
static unsigned char	temp_b;

static int	Num_tries;
//===============================================================
// MACROS
#define bitset(var,bitno) ((var) |= 1UL << (bitno))
#define bitclr(var,bitno) ((var) &= ~(1UL << (bitno))
#define bittest(var,bitno) (((var) & 1UL << (bitno)) >> (bitno))	// result in bit 0

#define NOP1() asm( "nop");
//======================================================================
//	Function Prototypes
char ASCII( char);
void ADselect( unsigned char);

void Go_STOP( void);
void Go_FWD ( void);
void Go_REV (void);

void Set_rt_FWD( void);
void Set_lf_FWD( void);
void Set_rt_REV( void);
void Set_lf_REV ( void);

void Set_PWM( unsigned char pwm);
void Set_wheel_speed_default( void);
void Set_wheel_speed( unsigned char speed);
void Set_lfwheel_speed( unsigned char speed);
void Set_rtwheel_speed( unsigned char speed);

void Fast_rtwheel( unsigned char n);
void Fast_lfwheel( unsigned char n);
void Go_slower( unsigned char n);
void Go_faster( unsigned char n);

void Check_Flags( void);
void Get_bumpers( void);
void Chirp( void);
void Set_move_distance( unsigned char a);
void GetCommand( void);
void Set_pwm_distance( unsigned char a);
void Go_pwm_distance( void);

void FindInitPathB( void);
void FindInitPath( void);
void FindClearPath( void	);
void Avoid_Right( void);
void Avoid_Left( void);
unsigned char Get_Sharp( void);

void Go_Back_count( unsigned char a);
void SPI_send( void);
void SPI_output( unsigned char);

void Put_UART( void);

void SendByte( unsigned char a);
void SendChar( unsigned char a);
void SendString( unsigned char* s);

void PutTxBuffer( unsigned char a, char ie);
void PutRxBuffer( unsigned char);
unsigned char GetRXbuffer(void);

unsigned char GetADC( unsigned char);

void init( void);
void PWM_init( void);
void Serial_init( void);
void SPI_init( void);
void InitTxBuffer( void);
void InitRxBuffer( void);
void ADC_init( void);
void LongDelay ( int n);

//---------------------------------------------------------------
//	
char ASCII( char a) {
	char b;
	const char convASCII[16] = {
		0x30,	// 0
		0x31,	// 1
		0x32,	// 2
		0x33,	// 3
		0x34,	// 4
		0x35,	// 5
		0x36,	// 6
		0x37,	// 7
		0x38,	// 8
		0x39,	// 9
		0x41,	// A
		0x42,	// B
		0x43,	// C
		0x44,	// D
		0x45,	// E
		0x46	// F
	};
	a = a & 0x0F;
	b = convASCII[a];
	return b;
}
//---------------------------------------------------------------
void ADselect( unsigned char a) {
	const char SelectAD[5] = {
		0x81,
		0x89,
		0x91,
		0x99,
		0xa1
	};
	ADCON0 = SelectAD[a];
	return;
}
//================================================================
//	ISRs
void interrupt isr(void){
	unsigned char a;

//----------------------------------------------------------------
// Fosc = 20MHz
// Tcyc = 200ns
// prescaler = 1:128 => 4.096msec****
// prescaler = 1:64 => 2.048msec
// prescaler = 1:32 => 1.024msec

	if(T0IE && T0IF){ // every 4.096msec
		T0IF = 0;
		TMR0 = TMR0_cnt;
		
		rtwh_sen_cntL++;								// 
		if (rtwh_sen_cntL == 0) {						// check for overflow
			bitset( wh_encoder_flags, rtwh_cnt_ovf);
		}
		lfwh_sen_cntL++;
		if (lfwh_sen_cntL == 0) {
			bitset( wh_encoder_flags, lfwh_cnt_ovf);
		}
		if (Sys_tick_counter-- == 0) {
			bitset( timeout_flags, Sys_tick_f);
		}
	}
//----------------------------------------------------------------
// about every 50msec with a wheel count = 0x0c & Tmr0 @ 4.096msec
	if (RBIE && RBIF) {
		rb_temp = Sensor_port & rb_sensor_mask;						// read PortB inputs
		RBIF = 0;													// clear interrupt flag
		if ((rb_change_bits = rb_temp ^ rb_change_bits ) == 0){		// did any change?
			goto rb_exit;											// no- exit
		}
		// Left wheel
		if (rb_change_bits & lfwh_EncA){							// did left wheel encoder change?
			if (rb_temp & lfwh_EncA){								// yes- low to high?
				if ( bittest( wh_encoder_flags ,lfwh_invalid_f) == 0) {
					if (lfwh_code_cnt-- == 0) {						// yes - count down number of segments
						bitset( wh_encoder_flags, lfwh_segcnt_f);	// set the code segment count flag if seg count finished
					}		
					lfwh_low_cnt = lfwh_sen_cntL;					// save wheel speed count
					bitset( wh_encoder_flags, lfwh_cnt_f);			// & set new count flag
					lfwh_sen_cntL = 0;								// clear counter to start again
				} else {											// current count invalid
					bitclr( wh_encoder_flags ,lfwh_invalid_f);		// start over
					lfwh_sen_cntL = 0;
				}
			}
		}
		// Right wheel
		if (rb_change_bits & rtwh_EncA){							// did the right change?
			if (rb_temp & rtwh_EncA){								// low to high?
				if ( bittest( wh_encoder_flags ,rtwh_invalid_f) == 0) {
					if (rtwh_code_cnt-- == 0){						// yes - count down number of segments
						bitset( wh_encoder_flags, rtwh_segcnt_f);	// set the code segment count flag
					}
					rtwh_low_cnt = rtwh_sen_cntL;
					bitset( wh_encoder_flags, rtwh_cnt_f);
					rtwh_sen_cntL = 0;
				} else {											// current count invalid
					bitclr( wh_encoder_flags ,rtwh_invalid_f);		// start over
					rtwh_sen_cntL = 0;
				}
			}
		}	
		rb_change_bits = rb_temp;
	}
rb_exit:
//-------------------------------------------------------------------------
// TMR2 interrupt handler
// PWM period = 409.6 usec
// Interrupt very forth PWM period 1.636msec (set with POST counter) 
//  PWM end/beginning of period????
//
// This ISR handles all motor control. 
// Requests are set in the motor_flags and are acted on in this ISR.
// 
// The actions are:
// 1) set/change motor direction
// 
// 2) ramp motor speed up or down
// update PWM duty?	ok, duty written to CPPxL, transfered to CCPxH at end of period
//
// 3) enable/disable motor?
//

	if (TMR2IE && TMR2IF) {			
		TMR2IF = 0;
		// Just Proportional feedback !this works 6feb10
		if( bittest(wh_encoder_flags, lfwh_cnt_f) == 1) {
			lfwh_target_err = lfwh_low_cnt - lfwh_target_cnt;
			lf_motor_pwm_cnt = lf_motor_pwm_cnt + (Kp * lfwh_target_err);
			bitclr(wh_encoder_flags, lfwh_cnt_f);
			if (lf_motor_pwm_cnt < (Min_pwm_cnt-4)) {		// check pwm duty and keep within limits
				lf_motor_pwm_cnt = Min_pwm_cnt;				// set to minimum pwm
			} else if (lf_motor_pwm_cnt > Max_pwm_cnt) {
				lf_motor_pwm_cnt = Max_pwm_cnt;				// set to max pwm
			}
			//lfwh_target_prev_err = lfwh_target_err;		// D-error
		}
		if( bittest(wh_encoder_flags, rtwh_cnt_f) == 1) {
			rtwh_target_err = rtwh_low_cnt - rtwh_target_cnt;
			rt_motor_pwm_cnt = rt_motor_pwm_cnt + (Kp * rtwh_target_err);
			bitclr(wh_encoder_flags, rtwh_cnt_f);
			if (rt_motor_pwm_cnt < (Min_pwm_cnt-4)) {
				rt_motor_pwm_cnt = Min_pwm_cnt;
			} else if (rt_motor_pwm_cnt > Max_pwm_cnt) {
				rt_motor_pwm_cnt = Max_pwm_cnt;
			}
			//rtwh_target_prev_err = rtwh_target_err;		// D-error
		}
		
		if( pwm_cnts-- == 0) {
			pwm_cnts_f = 1;	
		}
		CCPR2L = lf_motor_pwm_cnt;
		CCPR1L = rt_motor_pwm_cnt;
		PORTB = portB_image;
	} 
//tmr2_emd:
//----------------------------------------------------------------
//	if(SSPIE && SSPIF) {
//		SSPIF = 0;
//	}
//----------------------------------------------------------------
// UART Receive ISR
// 15.8usec
	if(RCIE && RCIF) {
		RCIF = 0;
		if (OERR == 1)
			goto ErrOERR;
		if (FERR == 1)
			goto ErrFERR;
		if (RxBufFull_f == 1)
			goto ErrRxOver;
		a = RCREG;
		if (a == 0x0d) {
			ReceivedCR_f = 1;
		} else {
			ReceivedCR_f = 0;
		}
		PutRxBuffer( a);	// move incoming byte into receive buffer
		goto RCintEnd;
ErrOERR:					// Over run error
		CREN = 0;
		CREN = 1;
		RxOverRerr_f = 1;
		goto RCintEnd;
ErrFERR:					// Framing error
		RxFrameErr_f = 1;
		a = RCREG;			// read and discard received data that has error
		goto RCintEnd;
ErrRxOver:					// receive buffer full error
		a = RCREG;
		RxBuffOver_f = 1;
		if (a == 0x0d) { 	// but if char is a CR
			ReceivedCR_f = 1;
		} else {
			ReceivedCR_f = 0;
		}
RCintEnd:
	;
	}
//----------------------------------------------------------------
// UART Transmit ISR
	if(TXIE && TXIF) {
		TXIF = 0;
		TXREG = *TxStartPtr;
		TxByteCount--;
			// test if buffer pointer needs to wrap around to beginning of buffer memory
		if (TxStartPtr > &TxBuffer + TX_BUF_LEN-2) {
			TxStartPtr = &TxBuffer;					// buffer wraps to beginning
		} else {
			TxStartPtr++;
		}
		if (TxByteCount == 0) {
			TXIE = 0;			// disable TX interrupt because all done
			TxBufEmpty_f = 1;
		}
	}
////----------------------------------------------------------------	
//	if (INTF && INTE) {
//		INTF = 0;	
//	}
////----------------------------------------------------------------	
//	if (TMR1IE && TMR1IF) {
//		TMR1IF = 0;
//	}
////----------------------------------------------------------------
//	if (EEIE && EEIF) {
//		EEIF = 0;
//	}
}	// End ISRs

//===============================================================
// Motor and wheel control primitives

void Set_PWM_default( void) {
	rt_motor_pwm_cnt = init_pwm_cnt;		// right
	lf_motor_pwm_cnt = init_pwm_cnt;		// left
}
void Set_PWM( unsigned char pwm) {
	rt_motor_pwm_cnt = pwm;					// right
	lf_motor_pwm_cnt = pwm;					// left
}

void Set_wheel_speed_default( void) {
	lfwh_target_cnt = init_wheel_cnt;
	rtwh_target_cnt = init_wheel_cnt;
}
void Set_wheel_speed( unsigned char speed) {
	lfwh_target_cnt = speed;
	rtwh_target_cnt = speed;
}
void Set_lfwheel_speed( unsigned char speed) {
	lfwh_target_cnt = speed;
}
void Set_rtwheel_speed( unsigned char speed) {
	rtwh_target_cnt = speed;
}

void Go_STOP( void) {
	bitclr( portC_image, STBY);		// disable motor driver
	PORTC = portC_image;
	portB_image = 0x0f;		// 1111 = brk
}
void Go_FWD ( void) {
	bitset(wh_encoder_flags ,lfwh_invalid_f);
	bitset(wh_encoder_flags ,rtwh_invalid_f);
	portB_image = 0x09;							// 1001
	bitset( portC_image, STBY); 	// enable motor driver
	PORTC = portC_image;
}
void Go_REV (void) {
	bitset(wh_encoder_flags ,lfwh_invalid_f);	// start over
	bitset(wh_encoder_flags ,rtwh_invalid_f);
	portB_image = 0x06;							// 0110
	bitset( portC_image, STBY); 	// enable motor driver
	PORTC = portC_image;
}

void Set_rt_FWD( void) {
	bitset(wh_encoder_flags ,rtwh_invalid_f);
	portB_image = (portB_image & 0x03) | 0x08;
}
void Set_lf_FWD( void) {
	bitset(wh_encoder_flags ,lfwh_invalid_f);
	portB_image = (portB_image & 0x0c) | 0x01;	// xx01
}
void Set_rt_REV( void) {
	bitset(wh_encoder_flags ,rtwh_invalid_f);
	portB_image = (portB_image & 0x03) | 0x04; // 01xx
}
void Set_lf_REV ( void) {
	bitset(wh_encoder_flags ,lfwh_invalid_f);
	portB_image = (portB_image & 0x0c) | 0x02;	// xx10
}
void Set_rt_STOP( void) {
	bitset(wh_encoder_flags ,rtwh_invalid_f);
	portB_image = (portB_image & 0x03) | 0x0c; // 11xx
}
void Set_lf_STOP( void) {
	bitset(wh_encoder_flags ,lfwh_invalid_f);
	portB_image = (portB_image & 0x0c) | 0x03; // xx11
}

void Go_rtwheel_Slower( unsigned char n) {
	rtwh_target_cnt = rtwh_target_cnt + n;
}
void Go_lfwheel_Slower( unsigned char n) {
	lfwh_target_cnt = lfwh_target_cnt + n;	
}
void Go_rtwheel_Faster( unsigned char n) {
	rtwh_target_cnt = rtwh_target_cnt - n;
}
void Go_lfwheel_Faster( unsigned char n) {
	lfwh_target_cnt = lfwh_target_cnt - n;
}
void Go_slower( unsigned char n) {
	rtwh_target_cnt = rtwh_target_cnt + n;
	lfwh_target_cnt = lfwh_target_cnt + n;
}
void Go_faster( unsigned char n) {
	rtwh_target_cnt = rtwh_target_cnt - n;
	lfwh_target_cnt = lfwh_target_cnt - n;
}
//---------------------------------------------------------------
//
void Set_move_distance( unsigned char a) {
	lfwh_code_cnt = a;
	rtwh_code_cnt = a;
	bitclr( wh_encoder_flags, lfwh_segcnt_f);
	bitclr( wh_encoder_flags, rtwh_segcnt_f);
}
void Go_move_distance( void) {
	bitset( portC_image, STBY); 	// enable motor driver
	PORTC = portC_image;
	while ((bittest( wh_encoder_flags, lfwh_segcnt_f) 
	      | bittest( wh_encoder_flags, rtwh_segcnt_f)) == 0);
}
void Set_pwm_distance( unsigned char a) {
	pwm_cnts = a;
	pwm_cnts_f = 0;
}
void Go_pwm_distance( void) {
	bitset( portC_image, STBY);		// enable motor driver
	PORTC = portC_image;
	while (pwm_cnts_f == 0);
}
//===============================================================
//	MAIN
//---------------------------------------------------------------
void main(void){
	
	unsigned char a;
	unsigned char s[8] = "abcd";
	Num_tries = 0;
	
	init();		// all ports and used periphials

	LongDelay( 20);	// Allow time for XBee to connect
		
	bitclr(timeout_flags, Sys_tick_f);
	Sys_tick_counter = Sys_tick;				// for 99msec
	bitset( portC_image, STBY);		// take motor driver out of STBY
	bitset( portC_image, PIEZO);		// pizo off
	PORTC = portC_image;
	Set_PWM_default();				// initial PWM speed
	Set_wheel_speed_default();
	direction = LEFT;

	action_flags = 0;
	TMR2IE = 1;
	RCIE = 1;
	INTCON = intcon_init;	// turn on interrupts
	
	DelayMs (120);
	//Go_FWD();
	SendChar('S');
	a = 0x55;
	SendByte (a);
	
	bitset( action_flags, SHARP_bump_f);
	NOP();
	NOP1();
	
	SendString(&s);
	s[0] = 'e';
	s[1] = 'f';
	s[2] = 'g';
	s[3] = 0x00;
	SendString(&s);
	
	FindInitPathB();

	while(1) {		// Main Loop
		Get_bumpers();
		Check_Flags();
		Sharp_Dist = Get_Sharp();
		if (Sharp_Dist < D_4in) {
			bitset( action_flags, SHARP_bump_f);
			FindClearPath();
			Go_FWD();
		}
		if (bittest(timeout_flags, Sys_tick_f) == 1) {	// 99ms tasks
			Sys_tick_counter = Sys_tick;
			bitclr(timeout_flags,Sys_tick_f);
			Sys_task_counter++;
			Num_tries = 0;
		//	if (Sys_task_counter > 19) {				// 2 second tasks
			if (Sys_task_counter > 40) {				// 4 second tasks
				Sys_task_counter = 0;
				bitclr(action_flags, PIR_timeout_f);	// 

			}
		//	Put_UART();		// send data for diagonisics
		}
	}	
}

//---------------------------------------------------------------
// Functions

void Check_Flags( void) {
	if( bittest(action_flags, rt_bumped_f) == 1) {
		FindClearPath();
		Go_FWD();
	}
	if( bittest(action_flags, lf_bumped_f) == 1) {
		FindClearPath();
		Go_FWD();
	}
	if( bittest(wh_encoder_flags, lfwh_cnt_ovf) == 1) {
		bitclr(wh_encoder_flags, lfwh_cnt_ovf);
	}	
	if( bittest(wh_encoder_flags, rtwh_cnt_ovf) == 1) {
		bitclr(wh_encoder_flags, rtwh_cnt_ovf);
	}
	if (RxBufEmpty_f == 0) {	// incoming data in buffer
		GetCommand();	
	}
	if( bittest(action_flags, PIR_timeout_f) == 0) {	// 
		if( bittest(action_flags, PIR_f) == 1) {
			Go_STOP();
			Chirp();
	//		FindClearPath();
			bitset(action_flags, PIR_timeout_f);
			Chirp();
			Go_FWD();
		}
	}	
}
//---------------------------------------------------------------
void Chirp( void) {
	bitclr( portC_image, PIEZO);		// piezo on
	PORTC = portC_image;
	DelayMs(140);
	bitset( portC_image, PIEZO);		// piezo off
	PORTC = portC_image;
	DelayMs(50);
	bitclr( portC_image, PIEZO);		// piezo on
	PORTC = portC_image;
	DelayMs(100);
	bitset( portC_image, PIEZO);		// piezo off
	PORTC = portC_image;
}
//---------------------------------------------------------------
// need to de-bounce? 
void Get_bumpers( void) {
	ra_temp = PORTA & ra_sensor_mask;
	if( bittest( ra_temp, rt_bump) == 0) {		
		bitset(action_flags, rt_bumped_f);	// rt_bumped
	} else {
		bitclr(action_flags, rt_bumped_f);
	}
	if ( bittest( ra_temp, lf_bump) == 0) {		
		bitset(action_flags, lf_bumped_f);	// lf bumped
	} else {
		bitclr(action_flags, lf_bumped_f);
	}
	if ( bittest( ra_temp, PIR_sensor) == 0) {		
		bitset(action_flags, PIR_f);		// PIR detected person ?
	} else {
		bitclr(action_flags, PIR_f);
	}	
}

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

void FindInitPathB( void) {
	unsigned char i, j = 0;

	Sharp_Dist = Get_Sharp();
	SendByte(Sharp_Dist);	
	for( i=0; i<12; i++) {
		//Set_move_distance(1);
		Set_pwm_distance(28);
				
		direction = RIGHT;
		Set_rt_REV();
		Set_lf_FWD();
		//Go_move_distance();
		Go_pwm_distance();
		
		Go_STOP();
		DelayMs (50);
		Sharp_Temp = Get_Sharp();
		SendByte( Sharp_Temp);
		DelayMs (50);
		if( Sharp_Temp > Sharp_Dist) {
			Sharp_Dist = Sharp_Temp;
			j = i;
		}
	}
	SendChar('M');
	SendByte(Sharp_Dist);
	SendChar('s');
//	Set_move_distance(12-j);
	do {
		//Set_move_distance(1);
		Set_pwm_distance(28);
		
		direction = LEFT;
		Set_rt_FWD();
		Set_lf_REV();
		//Go_move_distance();
		Go_pwm_distance();
		
		Go_STOP();
		DelayMs (50);
		Sharp_Temp = Get_Sharp();
		SendByte( Sharp_Temp);
	} while ( Sharp_Temp != Sharp_Dist);
	SendChar('F');
	Go_FWD();
}

//---------------------------------------------------------------
void FindInitPath( void) {
	unsigned char i, j = 0;

	Sharp_Dist = Get_Sharp();
	SendByte(Sharp_Dist);	
	for( i=0; i<10; i++) {
		Set_move_distance(1);
		direction = RIGHT;
		Set_rt_REV();
		Set_lf_FWD();
		Go_move_distance();
		Go_STOP();
		DelayMs (250);
		Sharp_Temp = Get_Sharp();
		SendByte( Sharp_Temp);
		DelayMs (250);
		if( Sharp_Temp > Sharp_Dist) {
			Sharp_Dist = Sharp_Temp;
			j = i;
		}
	}
	SendByte(Sharp_Dist);
//	Set_move_distance(12-j);
	do {
		Set_move_distance(1);
		direction = LEFT;
		Set_rt_FWD();
		Set_lf_REV();
		Go_move_distance();
		Go_STOP();
		DelayMs (120);
		
	} while ( Get_Sharp() != Sharp_Dist);
	Go_FWD();
}
//---------------------------------------------------------------
// turn until the path in front is clear
void FindClearPath( void) {

//	int	Num_tries = 0;
//	unsigned char s[8] = "abcd";
	
	Go_STOP();
	//turn_again:
	do {
		DelayMs (20);
		//Set_move_distance(1);
		Set_pwm_distance(20 + Num_tries);
		if( bittest(action_flags, rt_bumped_f) 
				&& bittest(action_flags, lf_bumped_f)) {
			if( Num_tries < 5) {
				direction = REVERSE;
				PutTxBuffer('a', 0);
				Set_lf_REV();
				Set_rt_REV();
				Set_pwm_distance(30 + Num_tries);
				Go_pwm_distance();
			} else if ( Num_tries < 8) {
				direction = RIGHT;
				PutTxBuffer('g', 0);
				Set_lf_FWD();
				Set_pwm_distance(30 + Num_tries);
				Go_pwm_distance();
			} else {
				direction = LEFT;
				PutTxBuffer('h', 0);
				Set_rt_FWD();
				Set_pwm_distance(30 + Num_tries);
				Go_pwm_distance();
			}
		} else if( bittest(action_flags, rt_bumped_f)) {
			PutTxBuffer('b', 0);
			direction = LEFT;
			Set_lf_REV();
			//Go_lfwheel_Slower(4);
			Go_pwm_distance();
			//Go_lfwheel_Faster(4);
		} else if( bittest(action_flags, lf_bumped_f)) {
			PutTxBuffer('c', 0);
			direction = RIGHT;
			Set_rt_REV();
			//Go_rtwheel_Slower(4);
			Go_pwm_distance();
			//Go_rtwheel_Faster(4);
		} else {					// only Sharp IR
			//Set_move_distance(2 + Num_tries);
			
			if( Num_tries < 5) {
				PutTxBuffer('d', 0);
				Set_pwm_distance(18 + Num_tries);
				if ( direction == LEFT) {
					Set_lf_REV();
					Set_rt_FWD();
				} else {
					Set_rt_REV();
					Set_lf_FWD();
				}
				Go_pwm_distance();
			} else if( Num_tries < 8){
				PutTxBuffer('e', 0);
				Set_move_distance(1);	// back up
				Set_lf_REV();
				Set_rt_REV();
				Go_move_distance();
			} else {
				PutTxBuffer('f', 0);
				Set_move_distance(7);	// turn around
				if ( direction == LEFT) {
					Set_lf_REV();
					Set_rt_FWD();
				} else {
					Set_rt_REV();
					Set_lf_FWD();
				}
				Go_move_distance();
			}
		}
		//Go_move_distance();
		//Go_pwm_distance();
		Go_STOP();
		Get_bumpers();
		Num_tries++;
//		PutTxBuffer('n', 0);
//		SendString(&s);
		SendByte(Num_tries);
		if (Num_tries > 50)
			break;
	} while ( Get_Sharp() < D_8in);
	bitclr( action_flags, SHARP_bump_f);
	Go_STOP();	
	DelayMs (20);
}
//---------------------------------------------------------------
unsigned char Get_Sharp( void) {
	unsigned char c, d;
	
	Sharp_voltage = GetADC( ADch0);
	if (Sharp_voltage > Sharp_3in) {	// higher voltage  is closer
		c = D_3in;						// too close
	} else if (Sharp_voltage > Sharp_4in) {
		c = D_4in;
	} else if (Sharp_voltage > Sharp_5in) {
		c = D_5in;
	} else if (Sharp_voltage > Sharp_6in) {
		c = D_6in;						// ok
	} else if (Sharp_voltage > Sharp_8in) {
		c = D_8in;
	} else if (Sharp_voltage > Sharp_10in) {
		c = D_10in;
	} else if (Sharp_voltage > Sharp_12in) {
		c = D_12in;						// far
	} else if (Sharp_voltage > Sharp_14in) {
		c = D_14in;
	} else {
		c = D_16in;					// too far to detect
	}	
	return c;
}

//---------------------------------------------------------------
// Serial motor interface control
// Commands:
//  s	= stop
//	f	= forward
//	b	= reverse
//	mv	= Set_move_distance, number of encode ticks
//	w	= Wait for distance moved
//	ev	= Set wheel speed, encode counts
//	p	= set default PWM count
//	t	= right wheel fwd
//	y	= right wheel rev
//	u	= left wheel fwd
//	i	= left wheel rev
//
void GetCommand( void) {
	unsigned char a;

	a = GetRXbuffer();
	PutTxBuffer( a, 1);		// ehco char
	switch (a) {
		case 's':			// stop
			Go_STOP();	
			break;
		case 'f':			// go forward
			Go_FWD();
			break;
		case 'b':			// go backward
			Go_REV();
			break;
		case 'm':
			while( RxBufEmpty_f == 1);				// wait for the next value
				Set_move_distance(GetRXbuffer());
			break;
		case 'w':
			Go_move_distance();
			break;
		case 'e':
			while( RxBufEmpty_f == 1);				// wait for the next value
			Set_wheel_speed(GetRXbuffer());
			break;
		case 'p':
			Set_PWM_default();
			break;
		case 'd':			// read last distance sensor measurement
			PutTxBuffer(ASCII(Sharp_voltage >> 4), 0);
			PutTxBuffer(ASCII(Sharp_voltage), 0);
			PutTxBuffer(0x0d, 1);
			break;
		case 'r':			// turn right
			Go_rtwheel_Slower(3);
			break;
		case 'l':			// turn left
			Go_lfwheel_Slower(3);
			break;
		case 'a':			// slow down
			Go_slower(3);
			break;
		case 'z':			// speed up
			Go_faster(3);
			break;
		default:
			break;
	}
}

//---------------------------------------------------------------
// Send byte to UART
// Output diagnostic data to UART
void Put_UART( void) {
//	return;
//		PutTxBuffer(ASCII(ra_temp >> 4), 0);
//		PutTxBuffer(ASCII(ra_temp), 0);
//		PutTxBuffer(0x20, 0);
//		PutTxBuffer(ASCII(action_flags >> 4), 0);
//		PutTxBuffer(ASCII(action_flags), 0);
//		PutTxBuffer(0x20, 0);
//		PutTxBuffer(ASCII(wh_encoder_flags >> 4), 0);
//		PutTxBuffer(ASCII(wh_encoder_flags), 0);
//		PutTxBuffer(0x20, 0);
//		PutTxBuffer(ASCII(Sharp_Dist >> 4), 0);
//		PutTxBuffer(ASCII(Sharp_Dist), 0);

		PutTxBuffer(ASCII(lfwh_low_cnt >> 4), 0);
		PutTxBuffer(ASCII(lfwh_low_cnt), 0);
		PutTxBuffer(ASCII(lf_motor_pwm_cnt >> 4), 0);
		PutTxBuffer(ASCII(lf_motor_pwm_cnt), 0);
		PutTxBuffer(ASCII(rtwh_low_cnt >> 4), 0);
		PutTxBuffer(ASCII(rtwh_low_cnt), 0);
		PutTxBuffer(ASCII(rt_motor_pwm_cnt >> 4), 0);
		PutTxBuffer(ASCII(rt_motor_pwm_cnt), 0);
		PutTxBuffer(0x0d, 1);
}
//---------------------------------------------------------------
void SendByte( unsigned char a) {
	PutTxBuffer(ASCII(a >> 4), 0);
	PutTxBuffer(ASCII(a), 0);
	PutTxBuffer(0x0d, 1);
}
void SendChar( unsigned char a) {
	PutTxBuffer(a, 0);
	PutTxBuffer(0x0d, 1);
}
void SendString( unsigned char* s) {
	while (*s >0) {
		PutTxBuffer (*s, 0);
		s++;
	}
	PutTxBuffer(0x0d, 1);
}
//---------------------------------------------------------------
// PutTxBuffer
// 	Move a byte into the next empty location of the ring buffer
//  Enables the UART TX Interrupt to start sending if ie = 1
//
void PutTxBuffer( unsigned char a, char ie) {
	if (TxBufFull_f == 1) {
		goto ErrTxBufFull;	// no saving of data because buffer full
	}
	TXIE = 0;		// disable while writing to the ring buffer
	*TxEndPtr = a;	// first empty location
	TxByteCount++;
	// test if buffer pointer needs to wrap around to beginning of buffer memory
	if (TxEndPtr > &TxBuffer + TX_BUF_LEN-2) {
		TxEndPtr = &TxBuffer;					// buffer wraps to beginning
	} else {
		TxEndPtr++;
	}
	// test if buffer is full
	if (TxEndPtr == TxStartPtr) {
		TxBufFull_f = 1;
	}
	TxBufEmpty_f = 0;
ErrTxBufFull:
	if ( ie == 1) {
		TXIE = 1;	// enable
	}
	return;		
}
//---------------------------------------------------------------
// Entered from UART Rx ISR
//
void PutRxBuffer(unsigned char a) {
	if (RxBufFull_f == 1) {
		goto ErrRxBufFull;	// no saving of data because buffer full
	}
	*RxEndPtr = a;
	RxByteCount++;
	// test if buffer pointer needs to wrap around to beginning of buffer memory
	if (RxEndPtr > &RxBuffer + RX_BUF_LEN-2) {
		RxEndPtr = &RxBuffer;
	} else {
		RxEndPtr++;
	}
	// test if buffer is full
	if (RxEndPtr == RxStartPtr) {
		RxBufFull_f = 1;
	}
	RxBufEmpty_f = 0;
ErrRxBufFull:
	return;		
}
//---------------------------------------------------------------
// retreive a byte from the UART receiver ring buffer
//
unsigned char GetRXbuffer(void) {
	unsigned char a;
	
	if (RxBufEmpty_f == 1) {
		goto ErrRxBufEmpty;
	}
	RCIE = 0;		// prevent ISR from changing buffer
	a = *RxStartPtr;
	RxByteCount--;
	// test if buffer pointer needs to wrap around to beginning of buffer memory
	if (RxStartPtr > &RxBuffer + RX_BUF_LEN-2) {
		RxStartPtr = &RxBuffer;
	} else {
		RxStartPtr++;
	}
	// test if buffer is now empty
	if (RxEndPtr == RxStartPtr) {
		RxBufEmpty_f = 1;
	}
	
	RCIE = 1;		// enable receive interrupt
	return a;
ErrRxBufEmpty:
	RCIE = 1;		// enable receive interrupt
	return 0;
}

//---------------------------------------------------------------
// Get data from AD
// Fosc/32 = 1.536 MHz, Tad = 6.5104 usec
// conversion = 12 Tad = 78.125 usec  
// 2Tad = 13.02 usec
unsigned char GetADC( unsigned char c) {
	ADselect( c);
	DelayUs ( 10);
	GODONE = 1;
AD_wait:
	if ( GODONE == 1)
		goto AD_wait;
	//return (ADRESH<<8 & ADRESL);
	//Sharp_data = ADRESH;
	return ADRESH;
}

//---------------------------------------------------------------
//// in from UART, out to SPI
//void SPI_send(void) {
//	unsigned char a;
//	
//	SPI_CS = 0;
//	a = GetRXbuffer();
//	TXREG = a;
//	SPI_output(a);
//	a = GetRXbuffer();
//	TXREG = a;
//	SPI_output(a);
//	a = GetRXbuffer();
//	TXREG = a;
//	SPI_CS = 1;
//	SPI_output(a);
//	ReceivedCR_f = 0;
//}
////---------------------------------------------------------------
////
//void SPI_output(unsigned char a) {
//	SSPBUF = a;
//	while (!BF){}
//	SPI_rxdata = SSPBUF;
//}

//---------------------------------------------------------------
// Init Functions

void init(void){
	PORTA 	= 0;
	PORTB	= 0;
	PORTC	= 0;
	portB_image = 0;
	bitset( portC_image, PIEZO);		// pizo off
	PORTC = portC_image;
	portC_image = 0;
	TRISA	= porta_dir_mask;		// Set PORTA as input
	TRISB	= portb_dir_mask;		// Set PORTB bits 0-3 in output mode
	TRISC	= portc_dir_mask;

	//OPTION	= PS_64;	// divide by 64 prescaler
	OPTION	= option_init;		
	TMR0	= TMR0_cnt; 		// Timer 0 count
	T1CON 	= t1con_init;
//	T0IE	= 1;		// timer 0 is interrupt enabled
	
	lf_motor_pwm_cnt = 0;
	rt_motor_pwm_cnt = 0;
	PWM_init();
	Serial_init();
//	SPI_init();
	ADC_init();
	
//	PEIE = 1;
//	GIE	= 1;		// turn on interrupts
}
//=============================================================================
// PWM module set-up
//-----------------------------------------------------------------------------
// period = (PR2 +1) * 4 * Tosc *(TMR2 prescale)
// freq = 1/period
// Tclock 	= 20.0MHz	10MHz
// Tosc		= 50nsec	100ns
// PR2 		= 255		255
// TMR2 prescale = 4		4
// freq 	= 4.88kHz	2.88k, also sets SPI bit rate
// peroid 	= 204.9 usec	409.8u
//-----------------------------------------------------------------------------
void PWM_init(void) {
	T2CON = t2con_init;
	TMR2 = 0;
//	PR2 = pr2_4880;		// 4.88kHz
	PR2 = pr2_2440;
	CCP1CON = pwm_mode_init;	// 0000 1100b	set to PWM mode
	CCP2CON = pwm_mode_init;
	CCPR1L = 0;			// set 0% duty, motors off
	CCPR2L = 0;
	TMR2ON = 1;			// timer2 on
//	PIE1 = pie1_init;
}
//---------------------------------------------------------------
//
void Serial_init(void) {
	SPBRG = baudh_57_6k;
	TXSTA = 0x24;		// 8bit, TXEN, SYNC = async, BRGH=high speed
	RCSTA = 0x90;		// SPEN enabled, 8bit, CREN=1
	
	InitTxBuffer();
	InitRxBuffer();
//	RCIE = 1;			// Rc interrupt enabled
}
//---------------------------------------------------------------
//
void SPI_init(void) {
	SPI_CS = 1;
	SSPSTAT = sspstat_init;
	SSPIE = 1;
	SSPCON = sspcon_init;
}
//---------------------------------------------------------------
//
void InitTxBuffer(void) {
	TxStartPtr = &TxBuffer;
	TxEndPtr = TxStartPtr;
	TxBufFull_f = 0;
	TxBufEmpty_f = 1;
	TxByteCount = 0;
}
//---------------------------------------------------------------
//
void InitRxBuffer(void) {
	RxStartPtr = &RxBuffer;
	RxEndPtr = RxStartPtr;
	RxBufFull_f = 0;
	RxBufEmpty_f = 1;
	RxByteCount = 0;
}
//---------------------------------------------------------------
// 8 bits in ADRESH
void ADC_init(void) {
	ADCON0 = adcon0_mask;
	ADCON1 = adcon1_init_lr;
}
void LongDelay ( int n) {
	int i;
	for( i=0; i<n;i++) {
		DelayMs(250);
	}
}
// ------ eof ----------------------------