/*
| ---------------------------------------------------------------------------
| Filename: omni_simple.c
|
| Author:       Chih Yenn, Voo
|		UWA 16/04/99
|
| Description:
| This is a simple demo program for the OMNI robot using the lowlevel motor
| drivers WITHOUT using the omni library
| ---------------------------------------------------------------------------
*/ 

#include "eyebot.h"

MotorHandle     motor_fr;
MotorHandle     motor_fl;
MotorHandle     motor_br;
MotorHandle     motor_bl;

char    z;

/* --------------------= VEHStop =---------------------- */

void OMNIStop()
{
	MOTORDrive (motor_fl,0);
	MOTORDrive (motor_fr,0);
	MOTORDrive (motor_bl,0);
	MOTORDrive (motor_br,0);
	OSWait(100);
}

/* ----------------------------------------------------- */

/* --------------------= LCD Setup---------------------- */

void LCDSet()
{
	LCDClear();
	LCDMode(SCROLLING|NOCURSOR);
	LCDMenu("DEMO","","","END");
}
/* ----------------------------------------------------- */
int main ()
{

/* clear display and write message */

	LCDSet();

	LCDPrintf("Roll on OMNI...\n");
/* init motors and release the motor handlers */

	motor_fl = MOTORInit(MOTOR_FL);
	motor_fr = MOTORInit(MOTOR_FR);
	motor_bl = MOTORInit(MOTOR_BL);
	motor_br = MOTORInit(MOTOR_BR);


        OMNIStop();

	if(!motor_fl||!motor_fr||!motor_bl||!motor_br) 
		{
			LCDPrintf("Init Error!\n");
			KEYGet();
			return(-1);
		}
	
/*Drive Demo*/

        z = KEYRead();
	while (z!=KEY4){
	switch(z) {
	 case KEY1:
		LCDPutString("DEMO\n");
		LCDPutString("Forward\n");
		MOTORDrive (motor_fl,60);
		MOTORDrive (motor_fr,60);
        	MOTORDrive (motor_bl,60);
		MOTORDrive (motor_br,60);
		OSWait(300);
		LCDSet();
		OMNIStop();
		LCDPutString("Reverse\n");
		MOTORDrive (motor_fl,-60);
		MOTORDrive (motor_fr,-60);
        	MOTORDrive (motor_bl,-60);
		MOTORDrive (motor_br,-60);		
		OSWait(300);
		LCDSet();
		OMNIStop();
		LCDPutString("Slide-L\n");
		MOTORDrive (motor_fl,60);
		MOTORDrive (motor_fr,-60);
        	MOTORDrive (motor_bl,-60);
		MOTORDrive (motor_br,60);		
		OSWait(300);
		LCDSet();
		OMNIStop();
		LCDPutString("Slide-R\n");
		MOTORDrive (motor_fl,-60);
		MOTORDrive (motor_fr,60);
        	MOTORDrive (motor_bl,60);
		MOTORDrive (motor_br,-60);		
		OSWait(300);
		LCDSet();
		OMNIStop();
		LCDPutString("Turn-Spot\n");
		MOTORDrive (motor_fl,60);
		MOTORDrive (motor_fr,-60);
        	MOTORDrive (motor_bl,60);
		MOTORDrive (motor_br,-60);		
		OSWait(300);
		LCDSet();
		OMNIStop();
		break;



	 default: break;
        	
	  }	
        z=KEYRead();
	}


	MOTORRelease (motor_fl|motor_fr|motor_bl|motor_br);

	return(0);
}