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

#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 ircode=0;
char	unsigned oldir=0;

// IR Codes, connected in PC7..PC4
#define FWD		0x80
#define REV		0x40
#define TRIGHT	0x60
#define TLEFT	0xa0
#define	STOP	0x90
#define SIT		0xc0
#define STAND	0x20	

void tenhut(void)
{
	servo_out(0,8,0xFF);		// turn on servos
	servo_out(9,8,0xFF);
	// all legs stand up
	move(1,0,1);
	move(2,0,1);
	move(3,0,1);
	move(4,0,1);
	move(5,0,1);
	move(6,0,2);
	move(7,0,1);
	move(8,0,1);
	move(9,0,1);
	move(10,0,1);
	move(11,0,1);
	move(12,0,1);
	move(13,0,1);
	move(14,0,1);
	move(15,0,1);
	move(16,0,1);
	if(wait(15)!=0) return;
	move(1,90,1);
	move(2,90,1);
	move(3,90,1);
	move(4,90,1);
	move(5,90,1);
	move(6,90,1);
	move(7,90,1);
	move(8,90,1);
	move(9,90,1);
	move(10,90,1);
	move(11,90,1);
	move(12,90,1);
	move(13,90,1);
	move(14,90,1);
	move(15,90,1);
	move(16,90,1);
	if(wait(15)!=0) return;
	move(1,0,1);
	move(2,0,1);
	move(3,0,1);
	move(4,0,1);
	move(5,0,1);
	move(6,0,2);
	move(7,0,1);
	move(8,0,1);
	move(9,0,1);
	move(10,0,1);
	move(11,0,1);
	move(12,0,1);
	move(13,0,1);
	move(14,0,1);
	move(15,0,1);
	move(16,0,1);
	if(wait(15)!=0) return;
	move(1,-90,1);
	move(2,-90,1);
	move(3,-90,1);
	move(4,-90,1);
	move(5,-90,1);
	move(6,-90,1);
	move(7,-90,1);
	move(8,-90,1);
	move(9,-90,1);
	move(10,-90,1);
	move(11,-90,1);
	move(12,-90,1);
	move(13,-90,1);
	move(14,-90,1);
	move(15,-90,1);
	move(16,-90,1);
	if(wait(15)!=0) return;
	servo_out(0,8,0x00);		// turn off servos
	servo_out(9,8,0x00);
}


long x;
////////////////////////////////////////////////////////
// Main program beings here 
// This  program blinks LED-3 on the evaluation board 

void main ()
{	
	DI();							// Disable Interrupts
	for(x=0; x<100000; x++){};
	init_led_gpio();				// Initializes LED ports (Port A and C)
 	init_timer0();					// Intialize Timer-0
	EI();							// Enable Interrupts
	if(wait(15)!=0) return;

// allow the servo driver to wake up
	while(1)
	{
		tenhut();
	}
}	// End of main program