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

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