#include "eyebot.h"
#define SAFE 300

int main ()
{ BYTE img[QVGA_SIZE];
  int dir, l, f, r;

  LCDMenu("", "", "", "END");
  CAMInit(QVGA);

  while(KEYRead() != KEY4)
  { CAMGet(img);    // demo
    LCDImage(img);  // only
    l = PSDGet(PSD_LEFT);
    f = PSDGet(PSD_FRONT);
    r = PSDGet(PSD_RIGHT);
    LCDSetPrintf(18,0, "PSD L%3d F%3d R%3d", l, f, r);
    if (l>SAFE && f>SAFE && r>SAFE)
      VWStraight(100, 200); // start driving 100mm at 200mm/s
    else
    { VWStraight(-25, 50); VWWait(); // back up
      dir = 180 * ((float)rand()/RAND_MAX-0.5);
      LCDSetPrintf(19,0, "Turn %d", dir);
      VWTurn(dir, 45); VWWait(); // turn [-90..+90]
      LCDSetPrintf(19,0, "          ");
    }
    OSWait(100);
  } // while
  return 0;
}
