// Dead Reckoning Test Program
// T. Braunl, UWA 2021
#include "eyebot.h"

int main()
{ int k, el,er, v,w, x,y,p;

  LCDMenu("RESET","FORWARD","LEFT","END");
  while ((k=KEYRead())!=KEY4)
  { switch (k)
    { case KEY1: ENCODERReset(1); ENCODERReset(2);
                 VWSetPosition(0,0,0);
                 break;
      case KEY2: VWSetSpeed(10,0); OSWait(500);
                 VWSetSpeed(0,0);
                 LCDPrintf("Forward motion\n");
                 break; // Initial straight forward
      case KEY3: VWSetSpeed(0,10); OSWait(500);
                 VWSetSpeed(0,0);
                 LCDPrintf("Turn left motion\n");
                 break; // Initial turn left			            
    }
    // Should not mix with VW functions
    el = ENCODERRead(1);  er = ENCODERRead(2);
    LCDPrintf("%4dL, %4dR |", el,er);

    VWGetSpeed(&v,&w);
    LCDPrintf("%3dv, %3dw |", v,w);

    VWGetPosition(&x, &y, &p);
    LCDPrintf("%3dx, %3dy, %4dp\n", x,y,p);
    OSWait(1000);
  }
  return 0;
}
