// SetSpeed Drive function calibration program
// T. Braunl, UWA 2021
#include "eyebot.h"

void check()
{ int x,y,p,v,w;
  LCDPrintf("%6d L, %6d R\n", ENCODERRead(1), ENCODERRead(2));
  VWGetPosition(&x, &y, &p);
  VWGetSpeed(&v, &w);
  LCDPrintf("%6d x %6d y %4d phi  %4d v %4d w\n", x,y,p);
}


void set(int *val)
{ int k;
  LCDMenu("INC", "DEC", "", "END");
  do
  { LCDPrintf("Current: %d\n", *val);
    switch(k=KEYGet())
    { case KEY1: *val+=10; break;
      case KEY2: *val-=10; break;
    }
  } while (k!=KEY4);
}


int main()
{ int k=0, lastk=0, rv=0, rw=0;

  LCDPrintf("SetSpeed Testing\n");
  VWSetSpeed(0,0);

  do
  { LCDMenu("SET V", "SET W", "CHECK", "STOP");
    lastk = k;
    switch(k=KEYGet())
    { case KEY1: set(&rv); VWSetSpeed(rv,rw); break;
      case KEY2: set(&rw); VWSetSpeed(rv,rw); break;
      case KEY3: check(); break;
      case KEY4: VWSetSpeed(0,0); // stop
    }
  } while (! (k==KEY4 && lastk==KEY4) );

  return 0;
}
