// EyeBot Demo Program: Drive, T. Bräunl, 2024
#include "eyebot.h"

int main()
{ int i, d, x=0,y=0,p=0, v=0,w=0;

  LCDMenu("START","","","");
  LCDSetPrintf(0,0,"POsition:");
  while(!KEYRead())
  { VWGetPosition(&x,&y,&p);
    LCDSetPrintf(1,0,"pos %4d %4d (%4d)    ", x,y,p);
    OSWait(500);
  }

  for (i=0; i<2; i++)       // run twice
  {
    LCDSetPrintf(3,0, "Drive 500mm 100mm/s\n");
    VWStraight(500, 100);  // 1m in ca. 5s
    while (!VWDone())
    { VWGetPosition(&x,&y,&p);
      VWGetSpeed(&v,&w);
      LCDSetPrintf(4,0,"pos %4d %4d (%4d)  sp %4d %4d", x,y,p, v,w);
      if ((d=PSDGet(PSD_FRONT)) < 100)
      { VWSetSpeed(0,0);    // STOP if obstacle in front
        LCDSetPrintf(6,0, "STOP, distance %d\n", d);
      }
      OSWait(500);
    }
    
    LCDSetPrintf(8,0,"Rotate 180 60 deg/s\n");
    VWTurn(180, 60);   // half turn in ca. 3s
    VWWait();          // wait until completed
  }
}
