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

void check()
{ int x,y,p;
  printf("%6d L, %6d R  ", ENCODERRead(1), ENCODERRead(2));
  VWGetPosition(&x, &y, &p);
  printf("%6d x, %6d y, %4d phi\n\n", x,y,p);
  OSWait(1000);
}


int main()
{ check();

  printf("Straight forward\n");
  VWStraight(500, 100);  // 500 mm at 100mm/s
  check();

  printf("Straight back\n");
  VWStraight(-500, 100); // back
  check();

  printf("Turn left\n");
  VWTurn(180, 60);       // 180 deg. at 60 deg./s
  check();

  printf("Turn right\n");
  VWTurn(-180, 60);      // back
  check();

  VWSetSpeed(0,0);
  return 0;
}
