#include "eyebot.h"
// SIMULATION-ONLY functions to place robot in driving area
// T. Braunl, June 2019

int main()
{ int run=1, rob, q,r,s,t, x,y,phi, wx,wy,wz,wp;

  rob = OSMachineID();
  LCDSetPrintf(0,0, "SIM command testing for robot %d\n", rob);
  LCDMenu("DRIVE", "STOP", "RESET", "END");
  SIMGetRobot(rob, &q, &r, &s, &t); // start position

  do
  { SIMGetRobot(rob, &wx, &wy, &wz, &wp);
    VWGetPosition (&x, &y, &phi);
    //LCDSetColor(RED,BLACK);
    LCDSetPrintf(2, 0, "WORLD x, y, z, phi");
    LCDSetPrintf(3, 0, "%td %5d %5d %5d  ", wx,wy,wz,wp);
    //LCDSetColor(BLUE,BLACK);
    LCDSetPrintf(2,30, "ROBOT x, y, phi");
    LCDSetPrintf(3,30, "%5d %5d %5d", x,y,phi);
    switch (KEYRead())
    { case KEY1: VWSetSpeed(50,0); break;
      case KEY2: VWSetSpeed( 0,0); break;
      case KEY3: SIMSetRobot(rob, q, r, s, t); break;
      case KEY4: run=0; break;
    }
  } while (run);
}

