/*
	sv2hb07
	April 19, 2001 Bob Blick
	Licensed under terms of the GNU General Public License, www.gnu.org
	No warranties expressed or implied.
	Significant change: current limit pins moved off Schmitt trigger portC to portB

Memory Usage Map:

Program ROM   $0000 - $00C3  $00C4 (   196) words
Program ROM   $056F - $07FF  $0291 (   657) words
Program ROM   $2007 - $2007  $0001 (     1) words
                             $0356 (   854) words total Program ROM 

Bank 0 RAM    $0021 - $0040  $0020 (    32) bytes
Bank 0 RAM    $0070 - $0077  $0008 (     8) bytes
                             $0028 (    40) bytes total Bank 0 RAM  

Bank 1 RAM    $00A0 - $00CF  $0030 (    48) bytes total Bank 1 RAM  
Bank 0 Bits   $0100 - $0106  $0007 (     7) bits  total Bank 0 Bits 

 */

/*	PIN ASSIGNMENTS PIC14 28 pin
	7	6	5	4	3	2	1	0

			AN4/SS	 T0IN	AN3/VR	 AN2	AN1	AN0
			7-RA5	 6-RA4	5-RA3	 4-RA2	3-RA1	2-RA0
								
						
	ICD	ICD						INT
	28-RB7	27-RB6	26-RB5	25-RB4	24-RB3	23-RB2	22-RB1	21-RB0
	CL2B	CL2A	CL1B	CL1A	HB2B	HB2A	HB1B	HB1A
	
	RXD	TXD	SDO	SDI/SDA	SCK/SCL	CCP1	CCP2	T1IN
	18-RC7	17-RC6	16-RC5	15-RC4	14-RC3	13-RC2	12-RC1	11-RC0
	RS232I	RS232O				SRVO2IN	SRVO1IN	
						STEER	THROTTLE	
 */
#define	XTAL		196608000	//crystal frequency
#define	CCPSCALE	18		//divisor scales to pwm
#define CCPCENTER	(7373 / CCPSCALE)	//this should be 7373 for 1.5 millisec at 19.66 MHZ
#define MINPWM		10		//add some deadband
#define CLDELAY		2		//delay before monitoring transistor saturation
#define MINGOOD		4000		//4915 is 1 millisecond
#define MAXGOOD		11000		//limits for acceptable servo pulse widths
#define BIGTICKLOAD	10		//about 1/150 second each

#include <pic.h>
__CONFIG(FOSC1 | UNPROTECT | BODEN | BKBUG | CPD);
#define byte unsigned char
//H-bridge output pins
#define HB2B 0b00001000
#define HB2A 0b00000100
#define HB1B 0b00000010
#define HB1A 0b00000001
//current limit sense pins
#define CL2B RB7
#define CL2A RB6
#define CL1B RB5
#define CL1A RB4
#define	RSTXBUFSIZE	0x1F

// ROM CONSTANT TABLES
const char Hex_tbl[] = {	48,49,50,51,52,53,54,55,56,57,65,66,67,68,69,70	};//hexadecimal lookup

//variables
static volatile unsigned int	CCP1 @0x15;
static volatile unsigned int	CCP2 @0x1B;
static volatile byte	motorloop;		//isr uses this
static volatile byte	pwm1;	//set motor 1 value. limit this to 127!
static volatile byte	pwm2;	//set motor 2 value
static volatile union {
	byte 		unchar[2];
	unsigned int	uint;
	} ccp1rise;		//ccp1 snapshot at input 1 pulse rise
static volatile union {
	byte 		unchar[2];
	unsigned int	uint;
	} ccp2rise;		//ccp2 snapshot at input 2 pulse rise
static volatile unsigned int	ccp1pulse;	//input 1 positive pulse width
static volatile unsigned int	ccp2pulse;	//input 2 positive pulse width
static volatile bit	ccp1flag;		//set when ccp1pulse is new
static volatile bit	ccp2flag;		//set when ccp2pulse is new
static volatile bit	pwm1dir;		//set is negative
static volatile bit	pwm2dir;
byte badbits;	//when seriously out-of-range pulse values come in, shift a 1 into this
static bank1 signed char	ccp1history[8];
static bank1 signed char	ccp2history[8];
byte	history1pos;
byte	history2pos;
signed char	ccp1scaled;
signed char	ccp2scaled;
static bit	newccp1;
static bit	newccp2;
static bit	errorflag;
static volatile byte	bigtick;	//decremented every 128 T0 interrupts, about 150 per second

// rs232
static volatile bank1 byte		rstxbuf[(RSTXBUFSIZE + 1)];
static volatile byte		rstxbufstart;
static volatile byte		rstxbufend;

//function prototypes
void SetupStuff(void);	//setup onboard peripherals
void SerialHex(byte);
void SerialPuts(const char * s);
byte Add232TxBuf(byte);	//adds to tx que, returns 0 if buffer full
signed char ScaleCCP(unsigned int);	//feed ccp value, get scaled-to-pwm signed char returned	
bit ChkCCP(unsigned int pulseval);	//returns 1 if pulse value acceptable
signed char Average(signed char servoval, byte *ppos, signed char *history); //averages last 8 values
void Mixer(void);		//mixes throttle and steering to make left and right motor PWM
byte SumBits(byte);		//counts the number of bits in a byte
signed char Average1(signed char servoval);
signed char Average2(signed char servoval);

void main() {
	unsigned int mainuint;
	byte i;
	SetupStuff();
	while(1) {
		if(ccp1flag) {		//steering value
			ccp1flag = 0;
			mainuint = ccp1pulse;	// copy off capture value
			newccp1 = 1;
			badbits <<= 1;
			if (ChkCCP(mainuint)) {	// test for out of scale values
				ccp1scaled = ScaleCCP(mainuint);
				ccp1scaled = Average1(ccp1scaled);
				//ccp1scaled = Average(ccp1scaled, &history1pos, ccp1history);
			} else {
				badbits |= 1;	//mark as bad
			}
		}
		if(ccp2flag) {		//throttle value
			ccp2flag = 0;
			mainuint = ccp2pulse;	// copy off capture value
			newccp2 = 1;
			badbits <<= 1;
			if (ChkCCP(mainuint)) {	// test for out of scale values
				ccp2scaled = ScaleCCP(mainuint);
				ccp2scaled = Average2(ccp2scaled);
				//ccp2scaled = Average(ccp2scaled, &history2pos, ccp2history);
			} else {
				badbits |= 1;	//mark as bad
			}
		}
		
		errorflag = 0;

		if (!bigtick) {		//if bigtick runs out and no signal, generate an error
			bigtick = BIGTICKLOAD;
			badbits = 255;
			for(i = 0; i < 8; i++) {
				ccp1history[i] = 0;
				ccp2history[i] = 0;
			}
		}

		if (SumBits(badbits) > 2)
			errorflag = 1;
		
		if (errorflag == 1) {
			pwm1 = 0;
			pwm2 = 0;
		} else {

			if (newccp1 && newccp2) {
				newccp1 = 0;
				newccp2 = 0;
				Mixer();
				bigtick = BIGTICKLOAD;
			}
		}
	} //end while(1)
} //end main

void interrupt isr() {
	if(T0IF) {
		T0IF = 0;
// START SOFTWARE PWM MOTOR DRIVE
// 2 Motor PWM driver. byte values pwm1 and pwm2 set motor speed
// pwm1dir, pwm2dir flags for direction
// First, turn on motors with appropriate polarity
		if (motorloop == 0)
		{
			if(bigtick)
				bigtick--;	//used for timeout in main, not for motor PWM
			if (pwm1) {
				if (pwm1dir)	//negative
					PORTB |= HB1B;
				else
					PORTB |= HB1A;
			}
			if (pwm2) {
				if (pwm2dir)	//negative
					PORTB |= HB2B;
				else
					PORTB |= HB2A;
			}
		}
// shut off motors when time greater than PWM value
		if(motorloop >= pwm1)
			PORTB &= ~(HB1A | HB1B);
		if(motorloop >= pwm2)
			PORTB &= ~(HB2A | HB2B);
// current limiting
		if(motorloop > CLDELAY) {
			if (CL1B)
				PORTB &= ~HB1B;
			if (CL1A)
				PORTB &= ~HB1A;
			if (CL2B)
				PORTB &= ~HB2B;
			if (CL2A)
				PORTB &= ~HB2A;
		}
		motorloop++;
		motorloop &= 0x7F;	//rollover from 127 to 0
// Done with the motor PWM
// USART transmit buffer
		if(TRMT && (rstxbufstart != rstxbufend)) {	//if uart transmit is free
			TXREG = rstxbuf[rstxbufstart];		//send it
			rstxbufstart++;				//increment pointer
			rstxbufstart &= RSTXBUFSIZE;		//rollover
		}
// End USART buffer
	}
// End T0 interrupt
// Begin capture interrupts for servo in pulses
// In order to measure positive pulse width, capture trigger polarity must
// be set to positive before the rise and negative after the rise
// First servo input 1, the steering channel
	if(CCP1IF) {
		if(CCP1CON & 1) {	//if trigger set to rising edge
			ccp1rise.unchar[0] = CCPR1L;
			ccp1rise.unchar[1] = CCPR1H;
			CCP1CON = 0b00000100;	//set ccp1 to falling edge trigger
		} else {
			ccp1pulse = CCP1 - ccp1rise.uint;	//diff eq pos pulse width
			ccp1flag = 1;
			CCP1CON = 0b00000101;	//set ccp1 to rising edge trigger
		}
		CCP1IF = 0;	//normally I'd do this earlier but writing CCP1CON can cause false CCP1IF
	}
// Now servo input 2, the throttle channel
	if(CCP2IF) {
		if(CCP2CON & 1) {	//if trigger set to rising edge
			ccp2rise.unchar[0] = CCPR2L;
			ccp2rise.unchar[1] = CCPR2H;
			CCP2CON = 0b00000100;	//set ccp2 to falling edge trigger
		} else {
			ccp2pulse = CCP2 - ccp2rise.uint;	//diff eq pos pulse width
			ccp2flag = 1;
			CCP2CON = 0b00000101;	//set ccp2 to rising edge trigger
		}
		CCP2IF = 0;	//see above
	}
// End capture interrupts
}	// End all interrupts

void SetupStuff() {
//a lot of setup stuff. Ports first.
	PORTA = 0b00000000;	//
	PORTB = 0b00000000;	//
	PORTC = 0b00000000;	//

	TRISC = 0b11000110;	//RC6 input even though UART transmit.
				//
				//
	TRISB = 0b11110000;	//no longer all outputs, now current sense upper 4
	TRISA = 0b11000000;	//currently unused
				//
//init A/D converter
	ADCON1 = 0b00000110;	//no analog inputs 
	ADCON0 = 0b00000000;	//A/D off
//set up capture channel 1
	CCP1CON = 0b00000101;	//capture every rising edge
//set up capture channel 2, beam return envelope
	CCP2CON = 0b00000101;	//capture every rising edge
//start Timer 1 for the capture
	T1CON = 0b00000001;	//no pre, no osc, internal clock
//serial port stuff next
// baud rate = xtal/(64(SPBRG+1)) for BRGH = 0 or xtal/(16/(SPBRG+1)) for BRGH = 1
	SPBRG = 0x1F;		//0x1F = 31 = 9600 baud with BRGH = 0 with 19.66 MHz xtal
	RCSTA = 0x90;
	TXSTA = 0xA2;		//brgh = 0
//get timer0 going
	TMR0 = 0;
	CLRWDT();
	OPTION = 0b11011100;	//prescaler = /16 assigned to WDT .018 * 16 = .288 sec typ
				//WDT cold 16 * .009 * sec, hot 16 * .028 sec
				//bit 6 hi: RB0 INT is rising edge triggered
	TMR0 = 0;
//get timer2 going
	PR2 = 0xFF;		//this is done at reset anyway
	T2CON = 0b01111110;	//set for max postscale max prescale
	TMR2IF = 0;
//misc registers
	badbits = 255;
//enable interrupts
	PIE1 = 0b00000100;		//CCP1 interrupt enabled
	PIE2 = 0b00000001;		//CCP2 interrupt enabled
	INTCON = 0b11100000;		//global, peripheral, timer interrupts enabled
}	//end of "SetupStuff()"

void SerialHex(byte hexnum) {
	while(!Add232TxBuf(Hex_tbl[(hexnum >> 4)]));
	while(!Add232TxBuf(Hex_tbl[(hexnum & 0x0F)]));
}

void SerialPuts(const char * s) {
	while(*s)
	{
		if(Add232TxBuf(*s))
			s++;
	}
}

byte Add232TxBuf(byte b) {
	byte newbuf;
	newbuf = rstxbufend + 1;
	newbuf &= RSTXBUFSIZE;
	if(newbuf == rstxbufstart)
		return 0;		//buffer is full
	rstxbuf[rstxbufend] = b;
	rstxbufend = newbuf;
	return 1;
}

signed char ScaleCCP(unsigned int pulseval) {
	signed int fooval;
	signed char retval;
	fooval = pulseval / CCPSCALE;
	fooval = fooval - CCPCENTER;
	if(fooval > 127)
		fooval = 127;
	if(fooval < -127)
		fooval = -127;
	retval = fooval;
	return (retval);
}

bit ChkCCP(unsigned int pulseval) {
	if (pulseval > MAXGOOD)
		return (0);
	if (pulseval < MINGOOD)
		return (0);
	return (1);
}

signed char Average(signed char servoval, byte *ppos, signed char *history) {
	signed int average;
	byte i;
	*ppos++;
	*ppos &= 7;
	history[*ppos] = servoval;
	average = 0;
	for(i = 0; i < 8; i++) {
		average += history[i];
	}
	average = average / 8;
	return ((signed char)average);
}

signed char Average1(signed char servoval) {
	signed int average;
	byte i;
	history1pos++;
	history1pos &= 7;
	ccp1history[history1pos] = servoval;
	average = 0;
	for(i = 0; i < 8; i++) {
		average += ccp1history[i];
	}
	average = average / 8;
	return ((signed char)average);
}

signed char Average2(signed char servoval) {
	signed int average;
	byte i;
	history2pos++;
	history2pos &= 7;
	ccp2history[history2pos] = servoval;
	average = 0;
	for(i = 0; i < 8; i++) {
		average += ccp2history[i];
	}
	average = average / 8;
	return ((signed char)average);
}

void Mixer(void) {	// servo pulses get shorter with more forward throttle or more left
	signed int left;
	signed int right;
	byte temp;
	left = -ccp2scaled + ccp1scaled;
	right = -ccp2scaled - ccp1scaled;
	if (left < 0) {
		left = -left;
		pwm1dir = 1;
	} else {
		pwm1dir = 0;
	}
	if (left > 127)
		left = 127;
	temp = (byte) left;
	if (temp < MINPWM)	//give a deadband
		temp = 0;
	pwm1 = temp;
	if (right < 0) {
		right = -right;
		pwm2dir = 1;
	} else {
		pwm2dir = 0;
	}
	if (right > 127)
		right = 127;
	temp = (byte) right;
	if (temp < MINPWM)
		temp = 0;
	pwm2 = temp;
}

byte SumBits(byte inval) {		//count set bits in a byte
	byte numbits;
	for(numbits = 0; inval != 0; inval >>= 1)
		if (inval & 1)
			numbits++;
	return numbits;
}