// VW PID calibration program
// T. Braunl, UWA 2023
#include "eyebot.h"
#define des 200  // [mm/s]

int main()
{ int k, Kp=100, Ki=0, Kd=0, Kx=0;
  int v,w;
  int index=0, step=0, x_size, y_size, y_max, add=0;

  LCDGetSize(&x_size, &y_size);
  VWControl(Kp,Ki,Kd,Kx);
  y_max = y_size-100;
  
  do 
  { LCDLine(0,y_max-1,   x_size-1,y_max-1,   BLUE );
    LCDLine(0,y_max-des, x_size-1,y_max-des, GREEN);
    //LCDMenu("START","","","");
    //KEYWait(KEY1);
    LCDMenu("NEXT","+","-","STOP");
    VWSetSpeed(des,0); // [mm/s]
    do
    { k= KEYRead();
      if (k==KEY1) index = (index+1)%4;
      if (k==KEY2 || k==KEY3)
      { if (k==KEY2) add=10; else add=-10;
        switch (index)
        { case 0: Kp+=add; break;
          case 1: Ki+=add; break;
          case 2: Kd+=add; break;
          case 3: Kx+=add; break;
        }
        VWControl(Kp,Ki,Kd,Kx);
        OSWait(100);
      }
      VWGetSpeed(&v,&w);
      LCDSetPrintf(16,0, "%d %d %d %d  [%d x %d] (%3d %3d %3d)      ",
                   Kp,Ki,Kd,Kx, x_size,y_size, step,v, w);
      LCDPixel(step, y_max-v, WHITE);
      step++;
      if (step>=x_size) step=0;
      OSWait(5);
    } while (k != KEY4);
    VWSetSpeed(0,0); // Stop
    LCDMenu("START","","","END");
    do k=KEYGet(); while (k!=KEY1 && k!=KEY4);
    LCDClear();
  } while (k!=KEY4);
  return 0;
}