/*------------------------------------------------------------------------
| Filename: drive_demo.c
|
| Author:       Nicholas Tay, UWA 1997
|               Thomas Braunl UWA 2000, 2004
|
| Description:  Drives EyeBot vehicle
|		( Revised from drive_rc.c )
|
-------------------------------------------------------------------------- */

#include "eyebot.h"

#include <stdio.h>
#include <math.h>
#ifndef PI
#define PI 3.1415926535
#endif

#define functions 9

char fname[functions][32]=
    {"Forward",  "Backward", "Rotate Left", "Rotate Right",
     "Curve Left\n(FORWARD)", "Curve Right\n(FORWARD)", "Curve Left\n(BACKWARD)",
     "Curve Right\n(BACKWARD)", "SetPos [0,0,0]"};

float fvel[functions][2] =
    { { 0.2,  0.0}, {-0.2,  0.0}, { 0.0, 0.6}, {0.0, -0.6},
      { 0.2,  0.2}, { 0.2, -0.2}, {-0.2, 0.2},
      {-0.2, -0.2}, { 0.0,  0.0} };
      

int main ()
{ VWHandle vw;
  SpeedType s;
  PositionType pos;
  int k = 0, fnum = 0;

  vw = VWInit(VW_DRIVE,1); /* init v-omega interface */
  if(vw == 0) { LCDPutString("VWInit Error!\n"); return 0; }
  VWStartControl(vw,7,0.3,7,0.1);

  s.v = 0.0;  s.w = 0.0;
  LCDMenu("+","-","GO","STP");
  
  while(k != KEY4)
  { LCDClear();
    LCDMenu("+", "-", "GO", " ");
    LCDPrintf("%s\n", fname[fnum]);
    VWGetPosition(vw,&pos);
    LCDPrintf("x = %5.2f\n", pos.x);
    LCDPrintf("y = %5.2f\n", pos.y);
    LCDPrintf("p = %5.2f\n", pos.phi);
   
    s.v = fvel[fnum][0];
    s.w = fvel[fnum][1];
    k = KEYGet();
    switch(k) {
      case KEY1: fnum = (fnum+1) % functions;
                 break;
      case KEY2: fnum = (fnum-1 +functions) % functions;
                 break;
      case KEY3: if (fnum<8)
                 { VWSetSpeed(vw,s.v,s.w);
                   LCDMenu(" ", " ", " ", "STP");
                   KEYWait(KEY4); /* continue until key pressed */
                 }
                 else if (fnum==8) 
                   VWSetPosition(vw,0.0,0.0,0.0);
                 break;
      default:   break;
    }
    VWSetSpeed(vw,0,0); /* stop */
  }

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