#include "eyebot.h"
#define SAFE 1000
#define FL 5    // PSD code for front-left
#define FR 6    // PSD code for front-right

int main ()
{ BYTE img[QVGA_SIZE];

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

  while (KEYRead() != KEY4)
  { CAMGet(img);    // demo
    LCDImage(img);  // only
    if (PSDGet(PSD_FRONT)>SAFE && PSDGet(FL)>SAFE && PSDGet(FR)>SAFE)
    { LCDSetPrintf(18,0, "Drive Straight");
      VWStraight(200, 200); // start driving 100mm at 200mm/s
      OSWait(100);
    }
    else
    { LCDSetPrintf(18,0, "Drive Curve   ");
      VWCurve(200, 40, 200); // curve 200mm, 40 deg., 200mm/s
      OSWait(1000);
    }
  }
  return 0;
}
