/*------------------------------------------------------------------------
| Filename: drivedemo.c
| Author:       Nicholas Tay, UWA 1997
|               Thomas Braunl UWA 2000, 2004, 2018
| Description:  Drives EyeBot robot using RobIOS V-Omega functions|
-------------------------------------------------------------------------- */

#include "eyebot.h"
#define functions 11

char fname[functions][32]=
    {"Forward",  "Backward", "Rotate Left", "Rotate Right", "Curve Left Forward",
     "Curve Right Forward", "Curve Left Back", "Curve Right Back",
     "Straight", "Turn", "Curve"};

//velocities
int vel[functions][2] =
    { { 300,  0}, {-300,   0}, {   0, 60}, {0, -60},
      { 200, 45}, { 200, -45}, {-200, 45}, {-200,-45},
      { 300,  0}, {   0,  60}, { 200, 0} };


int main (){
  int   x, y, phi, v=0, w=0, v_act, w_act;
  int   fnum = 0, done = 0, stopped = 1;

  LCDMenu("NEXT", "ZERO", "GO", "END");
  do {
    LCDSetPrintf( 0,0, "%s             \n", fname[fnum]);
    VWGetPosition(&x, &y, &phi);
    LCDSetPrintf( 1,0, "x = %5d    ", x);
    LCDSetPrintf( 2,0, "y = %5d    ", y);
    LCDSetPrintf( 3,0, "p = %5d    ", phi);
    v = vel[fnum][0];
    w = vel[fnum][1];
    VWGetSpeed(&v_act, &w_act);
    LCDSetPrintf( 5,0, "DESIRED vs ACTUAL SPEED");
    LCDSetPrintf( 6,0, "v = %5d  %5d  ", v, v_act);
    LCDSetPrintf( 7,0, "w = %5d  %5d  ", w, w_act);

    switch(KEYRead()) {
      case KEY1: fnum = (fnum+1) % functions;
                 break;
      case KEY2: VWSetPosition(0,0,0);
                 break;
      case KEY3: if (stopped)
                 { if (fnum< 8)  VWSetSpeed(v,w);  // drive
                   if (fnum==8)  VWStraight(300, v);
                   if (fnum==9)  VWTurn(180, w);
                   if (fnum==10) VWCurve(300, 90, v);
                   LCDMenuI(3, "STOP", BLACK, WHITE);
                   stopped = 0;
                 }
                 else // driving
                 { VWSetSpeed(0,0);  // stop
                   LCDMenuI(3, "GO", BLACK, YELLOW);
                   stopped = 1;
                 }
                 break;
      case KEY4: done = 1;
                 break;
    }
    
    OSWait(250); // wait in ms
    if (fnum>=8 && VWDone())  // Auto stop
    { VWSetSpeed(0,0);  // stop
      LCDMenuI(3, "GO", BLACK, YELLOW);
      stopped = 1;
    }
  } while (!done);
  
  VWSetSpeed(0,0);  // stop
  return 0;
}