//////////////////////////////////////////////////
// Arduino-Gertduino - RPi I/F
// 2016.02.07
//////////////////////////////////////////////////

#include <TimerOne.h>					// TimerOne library

#define TIMER1_INT_INTVL    (10 * 1000)			// unit: us

////////////////////////////////////////////////
// protocol
// Write: send "#W addr(%01X) val(%04X)", receive "#"
// Read:  send "#R addr(%01X) ", receive "#val(%04X)"
//
// Registers (16bit)
// 0h		(R/W)	movement; bit0 chA start, bit1 chA dir, bit7-2 chA pwm (0-63), bit8 chB start, bit9 chB dir, bit15-10 chB pwm (0-63)
// 1h		(R/W)	chA wheel limit
// 2h		(R/W)	chA wheel cnt
// 3h		(R/W)	chB wheel limit
// 4h		(R/W)	chB wheel cnt
// 5h		()		---
// 6h		()		---
// 7h		()		---
// 8h		()		---
// 9h		()		---
// Ah		()		---
// Bh		()		---
// Ch		()		---
// Dh		()		---
// Eh		(R)		bit7-0 chA wheel encoder current value (A0), bit15-8 chA wheel encoder previous value
// Fh		(R)		bit7-0 chB wheel encoder current value (A1), bit15-8 chB wheel encoder previous value
////////////////////////////////////////////////

#define	ST_WAIT_START	0
#define	ST_WAIT_RW	1
#define	ST_WAIT_ADDR	2
#define	ST_WAIT_VAL_0	3
#define	ST_WAIT_VAL_1	4
#define	ST_WAIT_VAL_2	5
#define	ST_WAIT_VAL_3	6
#define	ST_SEND_RES	7

volatile char		state;
volatile char		rw_mode;
volatile unsigned char	rw_addr;
volatile unsigned int	rw_data;
volatile unsigned int	rw_res;
volatile unsigned int	ctrl_reg[16];

///////////////////////////////////
// timer interrupt vector
///////////////////////////////////
void timer1_func()
{
	unsigned int	pre, cur;

	// dir A
	if ((ctrl_reg[0] & 0x0002) == 0)
		digitalWrite(12, LOW);
	else
		digitalWrite(12, HIGH);

	// dir B
	if ((ctrl_reg[0] & 0x0200) == 0)
		digitalWrite(13, LOW);
	else
		digitalWrite(13, HIGH);

	// pwm A
	analogWrite(3, ctrl_reg[0] & 0x00FC);			// bit 7-2

	// pwm B
	analogWrite(11, (ctrl_reg[0] & 0xFC00) >> 8);

	// brake A
	if ((ctrl_reg[0] & 0x0001) != 0) {
		if ((ctrl_reg[1] > 0 && ctrl_reg[2] >= ctrl_reg[1]) || ctrl_reg[2] > ctrl_reg[4]) // force brake A
			digitalWrite(9, HIGH);
		else
			digitalWrite(9, LOW);
	}
	else
		digitalWrite(9, HIGH);

	// brake B
	if ((ctrl_reg[0] & 0x0100) != 0) {
		if ((ctrl_reg[3] > 0 && ctrl_reg[4] >= ctrl_reg[3]) || ctrl_reg[4] > ctrl_reg[2]) // force brake B
			digitalWrite(8, HIGH);
		else
			digitalWrite(8, LOW);
	}
	else
		digitalWrite(8, HIGH);

	// wheel encoder A
	pre	= ctrl_reg[14] & 0xFF;
	cur	= (analogRead(0) >> 2) & 0xFF;	// 0-255

	ctrl_reg[14]	<<= 8;
	ctrl_reg[14]	|= cur;

	if (pre <= 64 && cur >= 127) {		// chA wheel encoder
		(ctrl_reg[2])++;
	}

	// wheel encoder B
	pre	= ctrl_reg[15] & 0xFF;
	cur	= (analogRead(1) >> 2) & 0xFF;	// 0-255

	ctrl_reg[15]	<<= 8;
	ctrl_reg[15]	|= cur;

	if (pre <= 64 && cur >= 127) {		// chB wheel encoder
		(ctrl_reg[4])++;
	}
}

char hex2bin(char c)
{
	if ('0' <= c && c <= '9')
		return (c - '0');
	if ('A' <= c && c <= 'F')
		return (c + 10 - 'A');
	if ('a' <= c && c <= 'f')
		return (c + 10 - 'a');
	return (-1);
}

void setup()
{
	int	i;

	// I/O mode
	pinMode(8,  OUTPUT);
	pinMode(9,  OUTPUT);
	pinMode(12, OUTPUT);
	pinMode(13, OUTPUT);

	// serial
	Serial.begin(9600);

	// init registers
	for (i = 0; i < 16; i++) {
		ctrl_reg[i]	= 0x0000;
	}

	// init state
	state	= ST_WAIT_START;

    // set timer
    Timer1.initialize(TIMER1_INT_INTVL);
    Timer1.attachInterrupt(timer1_func, TIMER1_INT_INTVL);
}

void loop()
{
	char	cbuf, cbuf_bin;
	char	wbuf[8];

	switch (state) {
	case ST_WAIT_START:		// wait sync mark (#)
		if (Serial.available() > 0) {
			cbuf = Serial.read();
			if (cbuf == '#')
				state	= ST_WAIT_RW;
		}
		break;

	case ST_WAIT_RW:		// wait R/W mode
		if (Serial.available() > 0) {
			cbuf = Serial.read();
			if (cbuf == 'W' || cbuf == 'R') {
				rw_mode	= cbuf;
				state	= ST_WAIT_ADDR;
			}
			else if (cbuf == '#')
				state	= ST_WAIT_RW;
			else
				state	= ST_WAIT_START;
		}
		break;

	case ST_WAIT_ADDR:		// wait address
		if (Serial.available() > 0) {
			cbuf 		= Serial.read();
			cbuf_bin	= hex2bin(cbuf);
			if (cbuf_bin >= 0) {
				rw_addr	= cbuf_bin;
				if (rw_mode == 'W')		// reg write
					state	= ST_WAIT_VAL_0;
				else {				// reg read
					rw_res	= ctrl_reg[rw_addr];
					state	= ST_SEND_RES;
				}
			}
			else if (cbuf == '#')
				state	= ST_WAIT_RW;
			else
				state	= ST_WAIT_START;
		}
		break;

	case ST_WAIT_VAL_0:		// wait write value
		if (Serial.available() > 0) {
			cbuf 		= Serial.read();
			cbuf_bin	= hex2bin(cbuf);
			if (cbuf_bin >= 0) {
				rw_data	= cbuf_bin * 0x1000;
				state	= ST_WAIT_VAL_1;
			}
			else if (cbuf == '#') {
				state	= ST_WAIT_RW;
			}
			else {
				state	= ST_WAIT_START;
			}
		}
		break;

	case ST_WAIT_VAL_1:		// wait write value
		if (Serial.available() > 0) {
			cbuf 		= Serial.read();
			cbuf_bin	= hex2bin(cbuf);
			if (cbuf_bin >= 0) {
				rw_data	+= cbuf_bin * 0x0100;
				state	= ST_WAIT_VAL_2;
			}
			else if (cbuf == '#') {
				state	= ST_WAIT_RW;
			}
			else {
				state	= ST_WAIT_START;
			}
		}
		break;

	case ST_WAIT_VAL_2:		// wait write value
		if (Serial.available() > 0) {
			cbuf 		= Serial.read();
			cbuf_bin	= hex2bin(cbuf);
			if (cbuf_bin >= 0) {
				rw_data	+= cbuf_bin * 0x0010;
				state	= ST_WAIT_VAL_3;
			}
			else if (cbuf == '#') {
				state	= ST_WAIT_RW;
			}
			else {
				state	= ST_WAIT_START;
			}
		}
		break;

	case ST_WAIT_VAL_3:		// wait write value
		if (Serial.available() > 0) {
			cbuf 		= Serial.read();
			cbuf_bin	= hex2bin(cbuf);
			if (cbuf_bin >= 0) {
				rw_data	+= cbuf_bin;
				ctrl_reg[rw_addr]	= rw_data;
				state	= ST_SEND_RES;
			}
			else if (cbuf == '#') {
				state	= ST_WAIT_RW;
			}
			else {
				state	= ST_WAIT_START;
			}
		}
		break;

	case ST_SEND_RES:		// return response
		if (rw_mode == 'W') {
			sprintf(wbuf, "#");
			Serial.write((unsigned char *)wbuf, 1);
		}
		else {
			sprintf(wbuf, "#%04X", rw_res);
			Serial.write((unsigned char *)wbuf, 5);
		}

		state	= ST_WAIT_START;
		break;

	default:
		state	= ST_WAIT_START;
		break;
	}
}
// end of file
