// SetSpeed Drive function calibration program
// T. Braunl, UWA 2021
#include "eyebot.h"
int rv=0, rw=0;

void check()
{ int x,y,p,v,w;
  VWGetPosition(&x, &y, &p);
  VWGetSpeed(&v, &w);
  LCDSetPrintf(1,0, "Position\n%6d x %6d y  (%3d phi)    ", x,y,p);
  LCDSetPrintf(3,0, "Speed\n%6d v %6d w    ", v,w);
}

void set(int *val)
{ int k;
  LCDMenu("INC", "DEC", "", "END");
  do
  { switch(k=KEYGet())
    { case KEY1: *val+=10; break;
      case KEY2: *val-=10; break;
    }
    LCDSetPrintf(1,0, "v = %3d  ", rv);
    LCDSetPrintf(2,0, "w = %3d  ", rw);
  } while (k!=KEY4);
  LCDMenu("SET V", "SET W", "CHECK", "STOP");
}

int main()
{ int k=0, oldk=0;

  LCDPrintf("SetSpeed Testing\n");
  LCDSetPrintf(1,0, "v = %3d  ", rv);
  LCDSetPrintf(2,0, "w = %3d  ", rw);
  VWSetSpeed(0,0);
  LCDMenu("SET V", "SET W", "CHECK", "STOP");

  do
  { k=KEYRead(); if(!k && oldk==KEY3) k=KEY3;
    oldk=k;
    switch(k)
    { case KEY1: set(&rv); break;
      case KEY2: set(&rw); break;
      case KEY3: VWSetSpeed(rv,rw);
                 check(); break;
      case KEY4: VWSetSpeed(0,0); // stop
    }
    OSWait(250); // 0.25 sec
  } while (k!=KEY4);
}
