/*------------------------------------------------------------------------
| Filename: demodrive.c
|
| Author:       Nicholas Tay, UWA 1997
|
| Description:  Drives EyeBot vehicle
|		( Revised from drive_rc.c )
|
-------------------------------------------------------------------------- */

#include "eyebot.h"
#include <math.h>

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

void VEHStop(VWHandle vw)
{
  VWSetSpeed(vw,0,0);
}

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


int main ()
{

  /* -----------= Variable declarations =-------------- */
  VWHandle vw;
  SpeedType s;
  PositionType start;
  int i;
  int choice = 0;

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

  vw = VWInit(VW_DRIVE,1); /* init v-omega interface */
  if(vw == 0)
    {
      LCDPutString("VWInit Error!\n");
      OSWait(200); return 1;
    }
  s.v = 0.0;  s.w = 0.0;
  LCDMenu("+","-","GO","STP");

  VWSetPosition(vw,0,0,0);
  VWGetPosition(vw,&start);
  VWStartControl(vw,7,0.3,7,0.1);
  i = KEYRead();
  while(i != KEY4)  {
    switch(i) {
      case KEY1:
	choice = (choice+1)%8;
	break;

      case KEY2:
	choice = (choice+ 7)% 8;
	break;

      case KEY3:
	VWSetSpeed(vw,s.v,s.w);
	KEYWait(KEY4);
	VEHStop(vw);
	break;

      default:
	break;
    }

    LCDSetPos(0,0);
    switch(choice) {
			  /*1234567890123456 1234567890123456*/
			  /*|              | |              |*/
      case 0: LCDPutString("Forward        \n               ");
	      s.v = 0.2; s.w = 0;
	      break;
      case 1: LCDPutString("Backward       \n               ");
	      s.v = -0.2; s.w = 0;
	      break;
      case 2: LCDPutString("Rotate Left    \n               ");
	      s.v =0; s.w = 0.2;
	      break;
      case 3: LCDPutString("Rotate Right   \n               ");
	      s.v=0; s.w = -0.2;
	      break;
      case 4: LCDPutString("Curve Left     \n(FORWARD)      ");
	      s.v = 0.2; s.w = 0.2;
	      break;
      case 5: LCDPutString("Curve Right    \n(FORWARD)      ");
	      s.v = 0.2; s.w = -0.2;
	      break;
      case 6: LCDPutString("Curve Left     \n(BACKWARD)     ");
	      s.v = -0.2; s.w = -0.2;
	      break;
      case 7: LCDPutString("Curve Right    \n(BACKWARD)     ");
	      s.v = -0.2; s.w = 0.2;
	      break;
    }
    i = KEYRead();
  }

  VEHStop(vw);
  VWRelease(vw); /* exit driver */
  return 0;
}