//////////////////////////////////////////////////
// Arduino - RPi I/F, with motion detection
// (1) Original  2016.02.07 - 2016.02.28
// (2) V1.01     2016.11
//////////////////////////////////////////////////

#include <TimerOne.h>			// TimerOne library
#include <NAxisMotion.h>
#include <Wire.h>

#define	MAX_ANGLE_DIFF	250		// unit: 1/100 degree

#define TIMER_MSEC  10

//#define	PWM_MIN_VAL	150	// min value of motor move
#define PWM_MIN_VAL     190             // (rev v1.01; value changed for more power)
#define	PWM_MAX_VAL	255		// max value of motor move

NAxisMotion	msns;
volatile char	sns_req_flag;

////////////////////////////////////////////////
// protocol
// Write: send "#W addr(%01X) val(%04X)", receive "#"
// Read:  send "#R addr(%01X) ", receive "#val(%04X)"
//
// Registers (16bit)
// 0h		(R/W)	movement; bit0 ON/OFF, bit2-1 mode, bit15-8 pwm (0-255)
// 1h		(R/W)	wheel limit (for forward/back motion)
// 2h		(R/W)	Euler Heading target (unit: 1/100 degree)
// 3h		()	---
// 4h		()	---
// 5h		()	---
// 6h		()	---
// 7h		()	---
// 8h		()	---
// 9h		()	---
// Ah		()	---
// Bh		(R)	chR wheel cnt
// Ch		(R)	chL wheel cnt
// Dh		(R)	Euler Heading reading (unit: 1/100 degree)
// Eh		(R)	bit7-0 chR wheel encoder current value (A0), bit15-8 chR wheel encoder previous value
// Fh		(R)	bit7-0 chL wheel encoder current value (A1), bit15-8 chL wheel encoder previous value
////////////////////////////////////////////////

#define	COM_ST_WAIT_START	0
#define	COM_ST_WAIT_RW		1
#define	COM_ST_WAIT_ADDR	2
#define	COM_ST_WAIT_VAL_0	3
#define	COM_ST_WAIT_VAL_1	4
#define	COM_ST_WAIT_VAL_2	5
#define	COM_ST_WAIT_VAL_3	6
#define	COM_ST_SEND_RES		7

volatile char		com_state	= COM_ST_WAIT_START;
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];

////////////////////////////////////////////////
// motor control
////////////////////////////////////////////////

#define	MTR_ST_STOP	0
#define	MTR_ST_FWD	1
#define	MTR_ST_BAK	2
#define	MTR_ST_RIGHT	3
#define	MTR_ST_LEFT	4

volatile char		mtr_state	= MTR_ST_STOP;
volatile int		mtr_rate;


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


long diff_angle(unsigned int target, unsigned int value)
{
	long	retval;

	retval	= (long)value - (long)target;
	while (retval > 18000)		// unit: 1/100 deg
		retval	-= 36000;
	while (retval < -18000)		// unit: 1/100 deg
		retval	+= 36000;

	return retval;		// positive...too right, negative...too left (clockwise)
}


///////////////////////////////////
// timer interrupt vector
///////////////////////////////////
void timer1_func()
{
	long		diff_agl;
        char		too_left, too_right;
	char		chL_stop, chL_dir;
	char		chR_stop, chR_dir;
	unsigned char	chL_pwm, chR_pwm;

	/////////////////////////////////////
	// check angle (clockwise)
	/////////////////////////////////////
	diff_agl	= diff_angle(ctrl_reg[0x02], ctrl_reg[0x0D]);	// target, current

	if (diff_agl <= (long)(MAX_ANGLE_DIFF) * (long)(-1) )
		too_left	= 1;
	else
		too_left	= 0;

	if (diff_agl >= (long)(MAX_ANGLE_DIFF) )
		too_right	= 1;
	else
		too_right	= 0;

	/////////////////////////////////////
	// change state
	/////////////////////////////////////
	chL_pwm	= chR_pwm	= (ctrl_reg[0x00] >> 8);	// original setting

	switch (mtr_state) {
	case MTR_ST_STOP:
		chL_stop	= 1;
		chR_stop	= 1;
		break;

	//////////////
	// forward
	//////////////
	case MTR_ST_FWD:
		chL_stop	= 0;
		chR_stop	= 0;
		chL_dir		= 0;
		chR_dir		= 0;

		if (too_right == 1) {		// rotate left
			chL_pwm		= 0;
			chR_pwm		= PWM_MIN_VAL;
		}
		else if (too_left == 1) {	// rotate right
			chL_pwm		= PWM_MIN_VAL;
			chR_pwm		= 0;
		}
		break;

	//////////////
	// back
	//////////////
	case MTR_ST_BAK:
		chL_stop	= 0;
		chR_stop	= 0;
		chL_dir		= 1;
		chR_dir		= 1;

		if (too_right == 1) {		// rotate right
			chL_pwm		= PWM_MIN_VAL;
			chR_pwm		= 0;
		}
		else if (too_left == 1) {	// rotate left
			chL_pwm		= 0;
			chR_pwm		= PWM_MIN_VAL;
		}
		break;

	//////////////
	// rotate right
	//////////////
	case MTR_ST_RIGHT:
		if (too_right == 0 && too_left == 0) {
			chL_stop	= 1;
			chR_stop	= 1;
		}
		else {
			chL_stop	= 0;
			chR_stop	= 0;

			if (too_right == 1) {
				chL_dir		= 1;
				chR_dir		= 0;
				chL_pwm	= 0;
//				chR_pwm	= PWM_MIN_VAL;
                                chR_pwm = 0;          // rev v1.01; no more move
			}
			else {	// too_left
				chL_dir		= 0;
				chR_dir		= 1;
				chL_pwm	= PWM_MIN_VAL;
				chR_pwm	= PWM_MIN_VAL;
			}
		}
		break;

	//////////////
	// rotate left
	//////////////
	case MTR_ST_LEFT:
		if (too_right == 0 && too_left == 0) {
			chL_stop	= 1;
			chR_stop	= 1;
		}
		else {
			chL_stop	= 0;
			chR_stop	= 0;

			if (too_right == 1) {
				chL_dir		= 1;
				chR_dir		= 0;
				chL_pwm	= PWM_MIN_VAL;
				chR_pwm	= PWM_MIN_VAL;
			}
			else {	// too_left
				chL_dir		= 0;
				chR_dir		= 1;
//				chL_pwm	= PWM_MIN_VAL;
                                chL_pwm = 0;      // rev v1.01; no more move
				chR_pwm	= 0;
			}
		}
		break;

	default:
		chL_stop	= 1;
		chR_stop	= 1;
		break;
	}

	/////////////////////////////////////
	// I/O output (A ... right, B ... left)
	/////////////////////////////////////
	digitalWrite(9,  chR_stop);
	digitalWrite(12, chR_dir);
	analogWrite(3,   chR_pwm);

	digitalWrite(8,  chL_stop);
	digitalWrite(13, chL_dir);
	analogWrite(11,  chL_pwm);

	/////////////////////////////////////
	// trigger to check sensor (in main loop)
	/////////////////////////////////////
	sns_req_flag	= 1;
}


void setup()
{
	int	i;

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

	// serial
	Serial.begin(9600);

	// motion sensor
	I2C.begin();
	msns.initSensor();
	msns.setOperationMode(OPERATION_MODE_NDOF);
//	msns.setUpdateMode(MANUAL);

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

	// init state
	com_state	= COM_ST_WAIT_START;
	mtr_state	= MTR_ST_STOP;

    // set timer
	sns_req_flag	= 0;

    Timer1.initialize(1000 * TIMER_MSEC);
    Timer1.attachInterrupt(timer1_func, 1000 * TIMER_MSEC);
}


void loop()
{
	char	      cbuf, cbuf_bin;
	char	      wbuf[8];
	float	      eh;
	unsigned int  eh_int;

	/////////////////////////////////////
	// check sensor
	/////////////////////////////////////
	if (sns_req_flag == 1) {
		// Bh		(R)		chR wheel cnt
		// Ch		(R)		chL wheel cnt
		// Dh		(R)		Euler Heading reading (unit: 1/100 degree)
		unsigned int	pre, cur;

		/////////////////////////////////////
		// motion (angle) sensor
		/////////////////////////////////////
		sns_req_flag = 0;
		eh  	= (msns.readEulerHeading()) * 100;		// 0 - 36000
		eh_int	= (unsigned int)eh;
		ctrl_reg[0x0D]	= eh;

		/////////////////////////////////////
		// read wheel encoder A
		/////////////////////////////////////
		pre	= ctrl_reg[0x0E] & 0xFF;
		cur	= (analogRead(0) >> 2) & 0xFF;	// 0-255
		ctrl_reg[0x0E]	<<= 8;
		ctrl_reg[0x0E]	|= cur;
		if (pre <= 64 && cur >= 127) {		// chR wheel encoder
			(ctrl_reg[0x0B])++;
		}

		/////////////////////////////////////
		// read wheel encoder B
		/////////////////////////////////////
		pre	= ctrl_reg[0x0F] & 0xFF;
		cur	= (analogRead(1) >> 2) & 0xFF;	// 0-255
		ctrl_reg[0x0F]	<<= 8;
		ctrl_reg[0x0F]	|= cur;
		if (pre <= 64 && cur >= 127) {		// chL wheel encoder
			(ctrl_reg[0x0C])++;
		}
	}

	/////////////////////////////////////
	// serial communication
	/////////////////////////////////////
	switch (com_state) {
	case COM_ST_WAIT_START:		// wait sync mark (#)
		if (Serial.available() > 0) {
			cbuf = Serial.read();
			if (cbuf == '#')
				com_state	= COM_ST_WAIT_RW;
		}
		break;

	case COM_ST_WAIT_RW:		// wait R/W mode
		if (Serial.available() > 0) {
			cbuf = Serial.read();
			if (cbuf == 'W' || cbuf == 'R') {
				rw_mode	= cbuf;
				com_state	= COM_ST_WAIT_ADDR;
			}
			else if (cbuf == '#')
				com_state	= COM_ST_WAIT_RW;
			else
				com_state	= COM_ST_WAIT_START;
		}
		break;

	case COM_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
					com_state	= COM_ST_WAIT_VAL_0;
				else {				// reg read
					rw_res	= ctrl_reg[rw_addr];
					com_state	= COM_ST_SEND_RES;
				}
			}
			else if (cbuf == '#')
				com_state	= COM_ST_WAIT_RW;
			else
				com_state	= COM_ST_WAIT_START;
		}
		break;

	case COM_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;
				com_state	= COM_ST_WAIT_VAL_1;
			}
			else if (cbuf == '#') {
				com_state	= COM_ST_WAIT_RW;
			}
			else {
				com_state	= COM_ST_WAIT_START;
			}
		}
		break;

	case COM_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;
				com_state	= COM_ST_WAIT_VAL_2;
			}
			else if (cbuf == '#') {
				com_state	= COM_ST_WAIT_RW;
			}
			else {
				com_state	= COM_ST_WAIT_START;
			}
		}
		break;

	case COM_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;
				com_state	= COM_ST_WAIT_VAL_3;
			}
			else if (cbuf == '#') {
				com_state	= COM_ST_WAIT_RW;
			}
			else {
				com_state	= COM_ST_WAIT_START;
			}
		}
		break;

	case COM_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;
				com_state	= COM_ST_SEND_RES;

				if (rw_addr == 0) {
					// Change motor control state if necessary
					// 0h		(R/W)	movement; bit0 ON/OFF, bit2-1 mode, bit15-8 pwm (0-255)
					// Bh		(R)		chR wheel cnt
					// Ch		(R)		chL wheel cnt

				 	if ((rw_data & 0x0001) == 0) {	// OFF
						mtr_state	= MTR_ST_STOP;
				 	}
				 	else if ((rw_data & 0x0006) == 0) {	// FORWARD
						mtr_state	= MTR_ST_FWD;
						ctrl_reg[0x0B]	= 0;
						ctrl_reg[0x0C]	= 0;
				 	}
				 	else if ((rw_data & 0x0006) == 2) {	// BAK
						mtr_state	= MTR_ST_BAK;
						ctrl_reg[0x0B]	= 0;
						ctrl_reg[0x0C]	= 0;
				 	}
				 	else if ((rw_data & 0x0006) == 4) {	// RIGHT
						mtr_state	= MTR_ST_RIGHT;
						ctrl_reg[0x0B]	= 0;
						ctrl_reg[0x0C]	= 0;
				 	}
				 	else if ((rw_data & 0x0006) == 6) {	// LEFT
						mtr_state	= MTR_ST_LEFT;
						ctrl_reg[0x0B]	= 0;
						ctrl_reg[0x0C]	= 0;
				 	}
				}
			}
			else if (cbuf == '#') {
				com_state	= COM_ST_WAIT_RW;
			}
			else {
				com_state	= COM_ST_WAIT_START;
			}
		}
		break;

	case COM_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);
		}

		com_state	= COM_ST_WAIT_START;
		break;

	default:
		com_state	= COM_ST_WAIT_START;
		break;
	}
}

// end of file
