/********************************************************************/

#include <eZ8.h>
#include <stdio.h>
#include <sio.h> // non-standard I/O
#include "main.h"
#include "timer.h"
#include "test_button.h"
#include "uart.h"
#include "servo.h"

// Default user flash optionbits.
FLASH_OPTION1 = 0xFF;
FLASH_OPTION2 = 0xFF;

///////////////////////////////////////////////////////
// Global - Variable

char	subtmr=10;
char	t1=0;
char	t2=0;
char	t3=0;
char	t4=0;
char	t5=0;
int		lt1;
char	unsigned command=0;
char	unsigned motor=0;
int		paramL=0;
int		paramH=0;
extern	char rx[];
extern	char rxready;



#define	LEG1a	1
#define	LEG1b	2
#define	LEG2a	3
#define	LEG2b	4
#define LEG3a	11
#define	LEG3b	12
#define LEG4a	7
#define LEG4b	8
#define LEG5a	5
#define LEG5b	6
#define LEG6a	13
#define LEG6b	14

// Ascii to hex converter
// converts a single character input to binary hex
// limitation: input is assumed to be within ascii 1 to 0
// and A to F

int	ascii2hex(int inchar)
{
	if(inchar <0x3a)
	{
		inchar=inchar & 0x0f;
		return(inchar);
	}
	else
	{
		inchar=(inchar-'A') +10;
		return(inchar);
	}
}

////////////////////////////////////////////////////////
// Main program starts here 
// This  program blinks LED-3 on the evaluation board 

void main ()
{	
unsigned char	state=0;
	init_led_gpio();				// Initializes LED ports (Port A and C)
	DI();							// Disable Interrupts
 	init_timer0();					// Intialize Timer-0
	init_uart0();
	EI();							// Enable Interrupts

// allow the servo driver to wake up
	wait(10);

// switch on servos




	while(1)						// Infinite while loop
	{
		if(t1==0)
		{

			PAOUT ^=0x80;

			t1=5;
		}



// This is the main sequence program
		if(rxready==1)
		{
			// motor number
			motor= rx[0] & 0x0f;
			motor= motor * 10;
			motor= motor + (rx[1] & 0x0f);
			command= rx[2];
			paramH=rx[3];
			paramL=rx[4];
			putchar(0x03);			// send acknowledge
			rxready=0;
			state=1;
		}
		if(rxready==2)		//checksum error handler
		{
			putchar(0x04);
			rxready=0;
		}

// Parse command
		if(state==1)
		{
			paramH	=ascii2hex(paramH);
			paramL = ascii2hex(paramL);
			paramL = paramH*16 + paramL;
			// move command handler
			if(command=='m') servo_out(motor,2,paramL);
			if(command=='s') servo_out(motor,1,paramL);
			if(command=='i') servo_out(motor,8,paramL);


			state=0;
		}

	}
}	// End of main program