/* 
Typewriter Repairmen 2010 NURC control receiver program 

Controls ROV motors, switches, servos using RS-485 link with a 
hex ASCII command message and a reply message with telemetry.

Revision history

2010-03-19 DF  Split from ps2-tx.c, wrote ADC, PWM, motor, init code
2010-03-20 DF  Wrote switch and servo code 
2010-03-25 DF  Wrote limit switch code 
2010-04-19 DF  Enabling reply message for analog data
2010-04-20 DF  Adding timeouts to serial receive code

Things to do:

3. Add better comm. error handling
4. Devise a way to test error handling

PC board errors:

1. Servo 3 is on RC2, which is supposed to be Ph. 
   Move servos 3,4 to RC3,4 and move Ph to RC2. 
2. Analog inputs AN4,5 are really on pins RA5, RE0. 
   Use ANA5, ANA6 (RA4, RA5) as limit switches, not analog inputs.
3. The LM2663 LV pin 6 needs to be grounded. Don't believe the datasheet.
4. The LM2594HV is not nearly powerful enough to run a servo. Use LM2586HV.

*/

//------------ INCLUDES ------------------------------- //

// load macros and functions for 18 family
#include <p18cxxx.h>
// load macros and functions specific to 4523
#include <p18f4523.h>
#include <stdio.h>
#include <delays.h>
#include <string.h>
#include <stdlib.h>
#include <pconfig.h>		// to make ADC config work right
#include <usart.h>
#include <adc.h> 

// ------------ Sizes of things --------------- //

#define NMOTORS 10		// max number of motor control channels
#define NSERVOS 4		// max number of servo control channels
#define NSWITCHES 10	// max number of switch channels
#define NVOLTS 4		// max number of analog channels sent

#define NSAMPLES 8		// ADC number of reads = pretend 15 bit ADC
//------------ CONFIG ------------------------------- //

// Configure the chip in code instead of through MPLAB's Configuration Bits menu.

#pragma config OSC = EC		// external clock
#pragma config BOREN = OFF	// brownout reset disabled 
#pragma config WDT = OFF	// watch dog timer OFF 
#pragma config LVP = OFF	// low voltage programming disabled 

//------------ Prototypes ------------------------------- //

void Initial(void);
int read_adc(unsigned char chan); 
char atoxdigit(char chr);
int atox(char *str, char n);
int parse_command_msg(char *msg, unsigned char *mots, char *sws, unsigned char *sers);
void build_reply_msg(char *msg);
void send_msg(char *msg);
void loadRSC(char board, char addr, char data);
void load_all_motors(void);
void load_all_switches(void);
void stop_all_motors(void);
void turn_off_all_switches(void);
void run_all_servos(void);

//------------ Global Variables ------------------------------- //

// these are received from the surface
unsigned char motors[NMOTORS];		// motor command values
char switches[NSWITCHES];			// switch values: 1 = on (active), 0 = off
unsigned char servos[NSERVOS];		// servo command values
int volts[NVOLTS];					// ADC results

// The ROV messages get put here
char command_msg[60];	// command message, newline, 0
char reply_msg[60];		// reply message, newline, 0
char buf[22];			// temporary string storage

/* ----------------- init code -------------------- */

#define BAUDRATE   8	// 8=115.2k, 17=57.6k, 26=38.4k, 52=19.2k

// Initialize the onboard peripherals
void Initial() {

	// Initialize the port pins to quiescent state
	LATA = 0;
	LATB = 0x1f;			// LD1\ to LD5\ high
	LATC = 0;				// servos, TxE low
	LATD = 0;				// data low
	LATE = 0;

	// enable the tristate outputs as needed (0=out, 1=in)
	TRISA = 0b11111111;		// A0-5 analog ins
	TRISB = 0b11000000;		// B0-4 RSC LD\ outs, B6-7 ICSP 
	TRISC = 0b10000000;		// C0-1,3-4 servo outs, C2 Ph out, C5 TxE out, C6 TxD out  
	TRISD = 0b00000000;		// D0-D7 speed data bus out
	TRISE = 0b00000000;		// E2 is watchdog out: all driven, none used

	OpenUSART(USART_TX_INT_OFF & USART_RX_INT_OFF & USART_ASYNCH_MODE & \
		USART_EIGHT_BIT & USART_CONT_RX & USART_BRGH_HIGH, BAUDRATE); 

// The stuff below should use ADC_V5 which ought to do the refs properly. But no.

	// configure A/D converter 
	OpenADC( ADC_FOSC_16 & ADC_RIGHT_JUST & ADC_6_TAD, // for 16 MHz osc
			ADC_CH0 & ADC_INT_OFF & ADC_VREFPLUS_VDD & ADC_VREFMINUS_VSS,
			ADC_4ANA); 				// AN0-3 are analog, others digital

   ADCON1 = 0b00001011; 	// The config above fails for some reason - brute-force it!


	// Configure TMR2 and ECCP1 to make 62 kHz Ph output on RC2
	TMR2 = 0;		// reset the timer phase to 0
	PR2 = 64;		// period 130 PWM clocks per phase (actual period = TP2+1)
	T2CON = 4;		// turn on timer 2, 1:1 prescale and postscale
	CCPR1L = 32;	// for 50% PWM duty cycle from TP2 (not perfect, but close)
	CCP1CON = 0b00001100;	// active hi PWMon P1A (pin RC2) only
}

// ---------------------- ADC read --------------------------- //

int read_adc(unsigned char chan) { 
	int result;

	SetChanADC(chan<<3);	// select channel and shift into proper bit field!
	Delay10TCYx(50);		// Delay for 50TCY 
	ConvertADC();			// Start conversion 
	while( BusyADC() );		// Wait for completion 
	result = ReadADC();		// Read result 
	return result;
} 

// read all ADCs NSAMPLES times and store in volts[]
void read_all_adcs(void) {
	unsigned char i, j;
	int sum;

	for (i=0;i<NVOLTS;i++) {
		sum = 0;
		for (j=0;j<NSAMPLES;j++) {
			sum += read_adc(i);
		}
		volts[i] = sum;
	}
}
	
// ----------------- Read message from USART ---------------- //

// get a message from serial port
// returns 0 if OK, 1 if timeout error
// The timer waits a fraction of a second for a valid message. 
// it throws away chars until M seen to sync up
int receive_msg(char *msg, char first_char) {
	int timer;

	// Wait for the start character, discarding all others
	do {
		timer = 10000;				// 500 millisecond timeout
		while (!DataRdyUSART()) {
			Delay10TCYx(20);		// wait 50 microseconds
			if (!--timer) 
				return 1;
		}
	}
		while ((*msg = getcUSART()) != first_char);
	msg++;	// save start char

	// wait for the rest of message with shorter timeout
	do {
		timer = 100;				// 5 millisecond timeout
		while (!DataRdyUSART()) {
			Delay10TCYx(20);		// wait 50 microseconds
			if (!--timer) 
				return 1;
		}
	}
		while ((*msg++ = getcUSART()) != '\0');	// read the rest of message thru NUL
	return 0;
}

// -------------------- Parsing routines ---------------------- //

// convert a character from ASCII to hex
// returns -1 if invalid hex character
char atoxdigit(char chr) {
	if (('a' <= chr) && (chr <= 'f')) return (chr - 'a' + 10);
	if (('A' <= chr) && (chr <= 'F')) return (chr - 'A' + 10);
	if (('0' <= chr) && (chr <= '9')) return (chr - '0');
	return -1;
}

// get an n digit positive hex number into an int from string
// returns -1 if invalid hex characters
int atox(char *str, char n) {
	char digit, i;
	int val;
	val = 0;
	for (i = 0; i < n; i++) {
		if ((digit = atoxdigit(*str++)) == -1) return -4;
		val = (val << 4) + digit;
	}
	return val;
}

// -------------------- Command parser ------------------------ //

// Parse the commands in msg into the arrays mots, sws, sers
// returns 1 if error, 0 if all OK
int parse_command_msg(char *msg, unsigned char *mots, char *sws, unsigned char *sers) {
	unsigned char i, checksum = 0;
	int val;

	if (*msg++ != 'M') return 1;

	for (i=0;i<NMOTORS;i++) {
		if ((val = atox(msg, 2)) == -1) return 1;
		msg += 2;
		checksum += mots[i] = (unsigned char) val;
	}

	if (*msg++ != 'S') return 1;
	for (i=0;i<NSWITCHES;i++) {
		sws[i] = *msg++;	// ASCII 'A' is 'active' switch code
	}

	if (*msg++ != 'P') return 1;
	for (i=0;i<NSERVOS;i++) {
		if ((val = atox(msg, 2)) == -1) return 1;
		msg += 2;
		checksum += sers[i] = (unsigned char) val;
	}

	if (*msg++ != 'C') return 1;
	if ((val = atox(msg, 2)) == -1) return 1;
	msg += 2;
//	if (checksum != (unsigned char) val) return 1;
	if (*msg++ != '\n') return 1; 
	if (*msg++ != '\0') return 1; 
	return 0;
}

// -------------------- Build a reply ------------------------ //

// build a reply string from ROV
void build_reply_msg(char *msg) {
	unsigned char i;

	*msg++ = 'V';		// for volts
	for (i=0;i<NVOLTS;i++) {
		msg += sprintf(msg, "%04X", volts[i]);
	}

	*msg++ = 'L';
	*msg++ = PORTAbits.RA4 + '0';	// Limit switches:
	*msg++ = PORTAbits.RA5 + '0';	// '1' is in limit, '0' is OK to move

	// no checksum - not needed for safety??!?!?
	*msg++ = '\n'; 
	*msg = '\0';
}

// send string to controller
void send_msg(char *msg) {
	LATCbits.LATC5 = 1;			// turn on the RS-485 driver
	putsUSART(msg);				// send the message to surface
	while(BusyUSART());			// send last chars
	LATCbits.LATC5 = 0;			// turn off the RS-485 driver when done
}

// ---------- Run the motors, switches and servos ------------ //

// port B bit mask to generate LD\ pulse for each board
char board_mask[] = {0b11111110, 0b11111101, 0b11111011, 0b11110111, 0b11101111};

// load an RSCA port with data (addr = chan + mode<<7)
void loadRSC(char board, char addr, char data) {

	LATD = addr;				// write addr
	LATB = board_mask[board];	// on falling edge of Ld
	LATD = data;				// then data 
	LATB = 0xff;				// on rising edge of Ld
}

// load all motors with speeds
void load_all_motors(void) {
	char i;

	for (i=0;i<5;i++) {		// all boards
		loadRSC(i, 0, motors[i<<1]);		// motor A
		loadRSC(i, 1, motors[(i<<1)+1]);	// motor B
	}
}

// load all switches
void load_all_switches(void) {
	char i;

	for (i=0;i<5;i++) {		// all boards
		loadRSC(i, 0x80, switches[i<<1]);		// switch A
		loadRSC(i, 0x81, switches[(i<<1)+1]);	// switch B
	}
}

// stop all motors by setting speeds to 'off'
void stop_all_motors(void) {
	char i;

	for (i=0;i<5;i++) {		// all boards
		loadRSC(i, 0, 0x80);	// motor A
		loadRSC(i, 1, 0x80);	// motor B
	}
}

// turn off all switches
void turn_off_all_switches(void) {
	char i;

	for (i=0;i<5;i++) {		// all boards
		loadRSC(i, 0x80, 0);	// switch A
		loadRSC(i, 0x81, 0);	// switch B
	}
}

// make a pulse for each of four servo outputs
void run_all_servos(void) {
	unsigned char i, del;
	short time;		// for fancy delay math
#define SERVO_CONST 38

	for (i=0;i<NSERVOS;i++) {
		// calculate PWM time delay from servo command
		time = ((servos[i] - 128) * SERVO_CONST) / 128;		// some 16 bit math
		del = time + SERVO_CONST;		// back to char for delay
		if (del == 0) del = 1;			// so it doesn't go crazy
		switch (i) {
			case 0: 
				LATCbits.LATC0 = 1;	// start the pulse
				break;
			case 1: 
				LATCbits.LATC1 = 1;
				break;
			case 2: 
				LATCbits.LATC4 = 1;	// PH is really on C2!
				break;
			case 3: 
				LATCbits.LATC3 = 1;
				break;
		}
		Delay100TCYx(24);		// Delay for 25 us * arg = 0.6 msec
		Delay100TCYx(del); 
		switch (i) {
			case 0: 
				LATCbits.LATC0 = 0;	// end the pulse
				break;
			case 1: 
				LATCbits.LATC1 = 0;
				break;
			case 2: 
				LATCbits.LATC4 = 0;	// PH is really on C2!
				break;
			case 3: 
				LATCbits.LATC3 = 0;
				break;
		}
	}
}

/*-----------------------------------------------*/
/*----------------   M A I N   ------------------*/
/*-----------------------------------------------*/

/*
Main operating loop:

Receive ROV command message  
Translate telemetry data to surface
Send ROV commands to servo port and RSC boards
Build ROV status message with telemetry data
*/

void main (void) {		
	char i, st, chan = 0;
	Initial(); 	
	stop_all_motors();				// initialize to zero speed

	while(1) {
		read_all_adcs();			// get the volts info
		build_reply_msg(reply_msg);	// code it up
		st = receive_msg(command_msg, 'M');	// get command messsage from controller
		if (st) {
			stop_all_motors();		 // timeout - shut down
			turn_off_all_switches();
		}
		else {		// got a message - is it good?
			send_msg(reply_msg);		// send analog stuff back
			st = parse_command_msg(command_msg, motors, switches, servos);
			if (!st) {			// only use message if valid
				load_all_motors();		// make motors go as specified
	//			load_all_switches();	// and switches
				run_all_servos();		// and servos
			}
		}
	}	
}