/* ROVotron transmitter main program 

(C) 2010 David Forbes
PS2 interface code (C) 2008 Curious Inventor, LLC
Creative Commons Attribution-Noncommercial 3.0
http://creativecommons.org/licenses/by-nc/3.0/ 

Revision history

2009-11-28 DF  Changed to 18F4520
2009-12-29 DF  Changed to 18F4523, adding flexible data structures
2010-01-02 DF  Commented the buffer code, adding PS2 to ROV conversion code
2010-03-03 DF  Adding main loop comments, writing status code
2010-03-09 DF  Adding LCD display code, raw PS2 display
2010-03-15 DF  Fixed XLCD header file to perform correctly, got PS2 display
2010-03-16 DF  Corrected bit ordering of buttons in ps2_tx.h
               Got the speed display and scaling math to work
2010-03-17 DF  Got the USART to send motor data without spurious nulls,
               eliminated all pointer warnings
2010-03-18 DF  Getting the string code to work. Made command string static
               Improved build_command_msg, fixed pragmas & baudrate
               Verified operation at 115kbaud: 29msec loop period
2010-03-19 DF  Wrote command parser, improving code to test it
               Parser plus sender plus 2 displays = 74 msec loop period
               Split from ps2_tx.c
2010-03-20 DF  Making XLCD work again using compiled libraries - see notes!
2010-03-21 DF  Split out config code to config.c, changed gain to % value
2010-03-22 DF  Redid configuration arrays to match new config code
               Moved config arrays to config.c, compiled, works
2010-03-23 DF  Adding telemetry display code, configuration from EEPROM
2010-03-24 DF  Removed Select and Start buttons from buttons[]
2010-03-25 DF  Added box buttons to buttons[], limit switch code beginning
2010-03-28 DF  Added display of telem/command screens, rate servo commands
2010-04-17 DF  Added deadband for joysticks
2010-04-19 DF  Enabled telemetry message receipt

Known bugs:
1. Need to set motors and servos to zero when entering configuration mode

Things to do:

Test switches, telemetry

Uses a PlayStation 2 Dual Shock 2 controller to run an ROV

Connects to ROV using RS-485 link with a hex ASCII command message
and a reply message with telemetry.

Displays useful information on an LCD display. 

The ROV interface is configurable using the PS2 controller. 

The software allows allocation of two control axes to each motor, to
allow 2-D control in "arcade style" for motor pairs. 

The code is table-driven. 
It collects control information from the PS2 controller
and converts it to usable form, one byte per analog position and 
one byte per button. This information is then mapped to
ROV motors and switches according to the configuration arrays. 

The configuration arrays are filled in by the configuration program. 
*/

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

// load macros and functions for 18 family
#include <p18cxxx.h>
// load macros and functions specific to 4523
#include <p18f4523.h>
#include "rtxa.h"
#include <stdio.h>
#include <delays.h>
#include <string.h>
#include <stdlib.h>
#include <spi.h>
#include <usart.h>
#include <xlcd.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 DEADBAND 16		// the sloppiness of a Sony PlayStation 2 joystick

//------------ PIC Configuration bits ----------------------- //

// 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 


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

// Command strings to PS2 controller. First byte is length of command.

// general purpose poll and command for vibration motors, 6 byte payload 
rom char const command_poll_6[] = {9, 0x01,0x42,0x00,0x00,0x00,0x00,0x00,0x00,0x00};		
// go in 'escape mode' or 'config mode'
rom char const command_enter_config[] = {5, 0x01,0x43,0x00,0x01,0x00};		
// turn dual shock mode, or 'analog' mode on
rom char const command_analog_on[] = {9, 0x01,0x44,0x00,0x01,0x03,0x00,0x00,0x00,0x00};		
// setup motor mapping
rom char const command_motor_map[] = {9, 0x01,0x4d,0x00,0x00,0x01,0xff,0xff,0xff,0xff};		
// request pressure sensing data
rom char const command_pressure_on[] = {9, 0x01,0x4f,0x00,0xff,0xff,0x03,0x00,0x00,0x00};		
// get a bit map that shows which buttons are being returned in 0x42 calls.
rom char const command_read_button_map[] = {9, 0x01,0x41,0x00,0x00,0x00,0x00,0x00,0x00,0x00};		
// exit escape mode 2nd method by lynxmotion: "CONFIG_MODE_EXIT_DS2_NATIVE" ... using only the first 2 bytes with 0's doesn't work on China knockoffs
rom char const command_exit_config[] = {9, 0x01,0x43,0x00,0x00,0x5A,0x5A,0x5A,0x5A,0x5A};		
// general purpose poll and command for vibration motors, 18 byte payload 
rom char const command_poll_18[] = {21, 0x01,0x42,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};		

#pragma udata udata1		// split up storage to fit each on a page

// the PS2 controller fills in these when read
unsigned char response[24];			// PS2 controller raw data
uint8 PS2_byte1, PS2_byte2;     	// single bit button data

// these are derived from the controller response
unsigned char buttons[NBUTTONS];	// button control values: 1 = pressed, 0 = not
unsigned char analog[NANALOG];		// analog control values

// these are sent to the ROV
unsigned char motors[NMOTORS];		// motor command values 0=full reverse, FF=full fwd
unsigned char switches[NSWITCHES];	// switch values: 'A' = active, '0' = off
unsigned char servos[NSERVOS];		// servo command values 0=full CCW, FF=full CW

#pragma udata udata2		// split up storage to fit each on a page

// 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

// data from ROV
int volts[4];			// telemetry voltages
short long smooth_volts[4];	// telemetry voltages averaged
char limits[2];			// limit switches: 0 OK, 1=in limit
int telems[4];			// telemetry in its units

char display_mode;		// display telemetry or commands - START toggles it
char was_start;			// history of START button for above

#include "config.c"		// load the configuration code now

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

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

// Initialize the onboard peripherals
void Initial() {
	ADCON0 = 0b00110000;	
	TRISB = 0b11011111;		// bit 5 Tx enable out, others in
	TRISC = 0b10010001;		// makes TX an output  
	// Ports D and E are configured by XLCD code

	// Fosc = 16 MHz, so the SPI data rate is 250kHz.
	OpenSPI(SPI_FOSC_64, MODE_11, SMPMID);  //MODE_11 = (CKP, CKE) = (1,0) 
	OpenUSART(USART_TX_INT_OFF & USART_RX_INT_OFF & USART_ASYNCH_MODE & \
		USART_EIGHT_BIT & USART_CONT_RX & USART_BRGH_HIGH, BAUDRATE); 

// !!! The XLCD library is screwy. It needs the following changes to the source:
// openxlcd.c needs CURSOR_SHIFT_LEFT changed to CURSOR_SHIFT_RIGHT
// xlcd.h needs CURSOR_SHIFT_RIGHT to be 00000110, and the one below it to be 00000101


	OpenXLCD(EIGHT_BIT & LINES_5X7);	// open the LCD display driver
	while (BusyXLCD());				// wait for it to be ready
	WriteCmdXLCD(0x0c);				// turn on display, no cursor
	WriteCmdXLCD(0x06);				// right shift of cursor on char.
}

/* ----------------- LCD display code -------------------- */

// Delays for LCD display
void DelayFor18TCY( void ) 
{ 
  Nop(); 
  Nop(); 
  Nop(); 
  Nop(); 
  Nop(); 
  Nop(); 
  Nop(); 
  Nop(); 
  Nop(); 
  Nop(); 
  Nop(); 
} 
void DelayPORXLCD( void ) 
{ 
  Delay1KTCYx(255); //Delay of 15ms 
  Delay1KTCYx(255); //Delay of 15ms 
  return; 
} 
void DelayXLCD( void ) 
{ 
  Delay1KTCYx(100); //Delay of 5ms 
  return; 
} 

// This array points to the leftmost char of each LCD display line
char lineaddr[4] = {0x00, 0x40, 0x14, 0x54};

// Set the LCD display to the left end of specified line 0..3
void SetLineXLCD(char line) {
	while(BusyXLCD()) ;
	SetDDRamAddr(lineaddr[line & 0x03]);	// point to left end of line
	while(BusyXLCD()) ;
}

// ------------------- USART message I/O ------------------ //

// 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;
}

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

// -------------------- 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;
}

// Parse the reply in msg into the array volts
// returns 1 if error, 0 if all OK
int parse_reply_msg(char *msg) {
	unsigned char i;
	int val;

	if (*msg++ != 'V') return 1;		// ADC voltages

	for (i=0;i<NVOLTS;i++) {
		if ((val = atox(msg, 4)) == -1) return 1;
		msg += 4;
		volts[i] = val;	// save it
	}

	if (*msg++ != 'L') return 1;		// limit switches
	for (i=0;i<1;i++) {
		limits[i] = *msg++ - '0';	// ASCII 0 is no limit, 1 is limit
	}

	if (*msg++ != '\n') return 1; 
	if (*msg++ != '\0') return 1; 
	return 0;
}

// ----------------- translation code -------------------- //

// Remove deadband slop from joysticks - Sony's are very sloppy
unsigned char deadband(unsigned char stick) {
	unsigned char val;

	val = 0x80;
	if (stick > 0x80+DEADBAND) val = stick - DEADBAND;
	if (stick < 0x80-DEADBAND) val = stick + DEADBAND;
	return val;
}

// converts raw PS2 data in response[] to buttons[] and analog[].
void translate_response_to_controls() {

	// translate PS2 buttons from bits to one byte per button
	PS2_byte1.byte = 0xff ^ response[4]; 	// invert and copy button bytes to mapping structure
	PS2_byte2.byte = 0xff ^ response[5];
	buttons[0] = 0;		// an unused switch points here
	buttons[1] = PS2_Up;
	buttons[2] = PS2_Down;
	buttons[3] = PS2_Left;
	buttons[4] = PS2_Right;
	buttons[5] = PS2_Triangle;
	buttons[6] = PS2_Circle;
	buttons[7] = PS2_Cross;
	buttons[8] = PS2_Square;
	buttons[9] = PS2_L1;
	buttons[10] = PS2_L2;
	buttons[11] = PS2_R1;
	buttons[12] = PS2_R2;
	buttons[13] = PS2_L3;	// these are the joystick buttons
	buttons[14] = PS2_R3;
	buttons[15] = pin_BoxBtn1 ^ 1;	// Molex connector in box
	buttons[16] = pin_BoxBtn2 ^ 1;
	buttons[17] = pin_BoxBtn3 ^ 1;
	buttons[18] = pin_BoxBtn4 ^ 1;
	buttons[19] = pin_BoxBtn5 ^ 1;
	buttons[20] = pin_BoxBtn6 ^ 1;

	// copy PS2 joysticks to analog values 
	analog[0] = 0x80;			// an unused servo or motor points here
	analog[1] = deadband(PS2_LeftStickX);
	analog[2] = deadband(PS2_LeftStickY);
	analog[3] = deadband(PS2_RightStickX);
	analog[4] = deadband(PS2_RightStickY);
	
	// translate PS2 pressure pairs to analog values
	// Results in 01 when down not up, 80 when off (down=up), ff when up not down.
	analog[5] = 0x80 + PS2_RightPress/2    - PS2_LeftPress/2;
	analog[6] = 0x80 + PS2_UpPress/2       - PS2_DownPress/2;
	analog[7] = 0x80 + PS2_CirclePress/2   - PS2_SquarePress/2;
	analog[8] = 0x80 + PS2_TrianglePress/2 - PS2_CrossPress/2;
	analog[9] = 0x80 + PS2_L1Press/2       - PS2_L2Press/2;
	analog[10] = 0x80 + PS2_R1Press/2       - PS2_R2Press/2;
}

// Convert data in buttons[] and analog[] to motors[], servos[] and switches[].
// Uses the mode variables to make different forms of commands. 
void translate_controls_to_commands() {
	char i;
	short command, range;		// For doing some signed short magic on unsigned chars

	// calculate motor speed for each motor output
	for (i=0; i<NMOTORS; i++) {
		if (cg.conf.motor_mode[i] == 1) {	// speed output mode
			command = ((analog[cg.conf.mot_pctl[i]]-128) * cg.conf.mot_pgain[i]) / 100
				 	+ ((analog[cg.conf.mot_sctl[i]]-128) * cg.conf.mot_sgain[i]) / 100;
			if (command >  127) 
				command =  127;
			if (command < -127) 
				command = -127;
		}
		else {
			command = 0;			// disabled so no speed output
		}
		motors[i] = command + 128;
	}

	// calculate servo position for each servo output
	for (i=0; i<NSERVOS; i++) {
		switch (cg.conf.servo_mode[i]) {
			case  1:	// position control
				command = ((analog[cg.conf.pwm_ctl[i]]-128) * cg.conf.pwm_gain[i]) / 100;
				// obey range limit (always positive!)
				range = (cg.conf.pwm_range[i] * 127) / 100;
				if (command > range) 
					command = range;
				if (command < 0-range) 
					command = 0-range;
				break;
			case 2:		// rate of change of position
				command = servos[i] - 128;	// get current position
				// move the servo a bit, governed by rate gain and stick position
				command += ((analog[cg.conf.pwm_ctl[i]]-128) * cg.conf.pwm_gain[i]) / 500;
				// obey range limit (always positive!)
				range = (cg.conf.pwm_range[i]) + 25;
				if (command > range) 
					command = range;
				if (command < 0-range) 
					command = 0-range;
				// if zero button is pressed, then reset to midpoint of range
				if (buttons[cg.conf.pwm_zbtn[i]] == 1) command = 0;
				break;
			default:
				command = 0;	
		}	// servo mode
		servos[i] = command + 128;
	}
	
	// calculate switch commands
	for (i=0; i<NSWITCHES; i++) {
		// momentary mode gets the on value
		switch (cg.conf.switch_mode[i]) {
			case  MOM:
				switches[i] = buttons[cg.conf.on_btn[i]];
				break; 
			// on-off mode: change state when only the correct button pressed
			case ONOFF:	
				if (buttons[cg.conf.on_btn[i]] && !buttons[cg.conf.off_btn[i]]) {
					switches[i] = 1;
				}
				else if (buttons[cg.conf.off_btn[i]] && !buttons[cg.conf.on_btn[i]]) {
					switches[i] = 0;
				}
				break;
			default:
				switches[i] = 0;	
		}	// switch mode
	}		// for loop
}			// function


// build a command string for ROV
void build_command_msg(char *msg) {
	unsigned char i, checksum = 0;

	*msg++ = 'M';
	for (i=0;i<NMOTORS;i++) {
		checksum += motors[i];
		msg += sprintf(msg, "%02X", motors[i]);
	}

	*msg++ = 'S';
	for (i=0;i<NSWITCHES;i++) {
		checksum += switches[i];
		*msg++ = (switches[i] ? 'A' : '0');
	}

	*msg++ = 'P';
	for (i=0;i<NSERVOS;i++) {
		checksum += servos[i];
		msg += sprintf(msg, "%02X", servos[i]);
	}

	*msg++ = 'C';
	msg += sprintf(msg, "%02X", checksum);
	*msg++ = '\n'; 
	*msg = '\0';
}

// ---------------  PS2 controller functions ---------------- //

// initialize the PS2 controller for analog mode, long output data format
void initialize_controller(char chan) {
	// Send a series of configuration commands to get the pressure values
	rw_ps2_packet(chan, (rom char *)command_enter_config);
	rw_ps2_packet(chan, (rom char *)command_analog_on);
	rw_ps2_packet(chan, (rom char *)command_pressure_on);
	rw_ps2_packet(chan, (rom char *)command_exit_config);
}

// send a command character to the PS2 controller on the SPI port 
//   and receive a reply character at the same time
void ps2_txrx(unsigned char data_out, unsigned char *reg_in){
	char send = data_out;
	bitrev(data_out);
	SSPBUF = (data_out);
	while(!SSPSTATbits.BF);
	*reg_in = SSPBUF;
	bitrev(*reg_in);
}

// *** This could be improved to monitor the ack line to know when the PS2 
// *** is done with this packet, since otherwise it may get garbage or hang? 

// write a command to and receive a reply from PS2 controller
// The command is a canned string from ROM
// The reply is placed in the char array response[]
void rw_ps2_packet(char chan, rom char *commandp) { 
	char i, count;

	count = response[0] = *commandp++;	// read length count from command array
	if (chan == 1) 
		pin_PS2_ATT_B = 0;
	else
		pin_PS2_ATT_A = 0;		// pull ATT low to signal gamepad
	for (i=1; i<=count; i++) {
		ps2_txrx(*commandp++, &response[i]);	// write command, read reply data
	}
	pin_PS2_ATT_B = 1;
	pin_PS2_ATT_A = 1;			// pull ATT high at end of command
	Delay100TCYx(255);			// verified 100 is too short
}

// -------------------- Display functions -------------------- //

// display the control values before translating to ROV commands 
void display_controls(char row) {
	char i;
	// display the buttons on the LCD 
	SetLineXLCD(row);
	for (i=1;i<NBUTTONS;i++) {
		sprintf(buf, "%1X", buttons[i]);  // print the PS2 button data in hex
		putsXLCD(buf);
	}
	// display the analog values on next line
	SetLineXLCD(row+1);
	for (i=1;i<NANALOG;i++) {
		sprintf(buf, "%02X", analog[i]);  // print the PS2 analog data in hex
		putsXLCD(buf);
	}
}

// display all the command info in pretty form
void display_commands(unsigned char *mots, unsigned char *sws, unsigned char *sers) {
	char i;

	// display the ROV motor speeds on top 2 lines
	for (i=0;i<NMOTORS;i++) {
		if (i%5 == 0) {
			SetLineXLCD(i/5);
			sprintf(buf, "Mot%1d:", i);
			putsXLCD(buf);
		}
		sprintf(buf, "%02X ", mots[i]);
		putsXLCD(buf);
	}
	// display the ROV servos 
	SetLineXLCD(2);
	putrsXLCD("PWMs:");
	for (i=0;i<NSERVOS;i++) {
		sprintf(buf, "%02X ", sers[i]);  // print the ROV servo data in hex
		putsXLCD(buf);
	}
	// display the ROV switches 
	SetLineXLCD(3);
	putrsXLCD("Switches:");
	for (i=0;i<NSWITCHES;i++) {
		sprintf(buf, "%1X", sws[i]);  // print the ROV switch data in hex
		putsXLCD(buf);
	}
}

// display all the telemetry info in pretty form
void display_all_telems(void) {
	char i;

	for (i=0; i<NVOLTS; i++) {
		SetLineXLCD(i);
		display_telem(i);	// show current telemetry reading
		putrsXLCD(telem_label_names[cg.conf.telem_label[i]]);
	}
}

// display all the telemetry info in raw, inscrutable hex form
void display_all_volts(void) {
	char i;

	for (i=0; i<NVOLTS; i++) {
		SetLineXLCD(i);
		sprintf(buf, "Volts %1d: %04X ", i, volts[i]);
		putsXLCD(buf);
	}
}
/*-----------------------------------------------*/
/*----------------   M A I N   ------------------*/
/*-----------------------------------------------*/
void main (void) {		
	unsigned char i, st;
	char samp = 0;		// where we are in averaging

	Initial(); 	
	initialize_controller(0);	// get the PS2 talking the right language

	while (BusyXLCD());
	WriteCmdXLCD(1);		// clear screen
	SetLineXLCD(0);
	putrsXLCD("     ROVotron");		// display splash screen
	SetLineXLCD(1);
	putrsXLCD(" ROV Control System");
	SetLineXLCD(3);
	putrsXLCD("     Rev. 0.20");
	delay_2sec();

	if (read_conf_from_eeprom()) {	// get config data... is it bad?
		while (BusyXLCD());
		WriteCmdXLCD(1);		// clear screen
		SetLineXLCD(1);
		putrsXLCD("   Initializing");
		SetLineXLCD(2);
		putrsXLCD("configuration data");
		delay_2sec();
		cg.conf.magic = 'C';
		for (i=1;i<sizeof(cg.conf);i++) {
			cg.conf_chars[i] = 0;
		}
		write_conf_to_eeprom();		// fill config array if empty
	}

	WriteCmdXLCD(1);		// clear screen
	for (i=0;i<NSERVOS;i++) 
		servos[i] = 128;		// initialize servos to safe position

	// main loop gets controller commands, sends to ROV
	while(1) {
		rw_ps2_packet(0, (rom char *)command_poll_18);	// get controller data
		if (PS2_Start && !was_start)
			display_mode = display_mode ^ 1;	// toggle between telem and command display
		was_start = PS2_Start;				// for button history
		translate_response_to_controls();	// turn raw controller data into useful data
//		display_controls(2);
		translate_controls_to_commands();	// make ROV data from that
//		if (display_mode)
//			display_commands(motors, switches, servos);	// display the ROV data
//		else
			display_all_telems();			// or display telemetry if asked
//			display_all_volts();			// or display volts if asked
		build_command_msg(command_msg);
//		SetLineXLCD(0);		// 3/18: 32 msec cycle with these in code, 29 msec without
//		putsXLCD(command_msg);
		if (PS2_Select) {
			for (i=0;i<NSERVOS;i++) 
				servos[i] = 128;			// initialize servos to safe position
			for (i=0;i<NMOTORS;i++) 
				motors[i] = 128;			// initialize motors to safe position
		}
		send_msg(command_msg);				// give commands to ROV
		st = receive_msg(reply_msg, 'V');	// get response from ROV
		if (!st) {
			parse_reply_msg(reply_msg); 
			for (i=0;i<NVOLTS;i++) {
				smooth_volts[i] += volts[i];
			}
			samp++;
		}

		if (samp == 10) {
			for (i=0;i<NVOLTS;i++) {
				volts[i] = (int)(smooth_volts[i] / samp);
				smooth_volts[i] = 0;
			}
			samp = 0;
//			display_all_volts();			// or display volts if asked
			translate_to_telems(volts);
		}

		if (PS2_Select) {
			config_loop();				// do some configuration
			while (BusyXLCD());
			WriteCmdXLCD(1);			// clear screen
		}
	}	
}