#include "eyebot.h"
#define SAFE 300

int main ()
{ BYTE img[QVGA_SIZE];
  int dir, f,l,r,b, drive=0, turn=1, finish=0;

  LCDMenu("DRIVE", "STOP", "STRAIGHT", "END");
  CAMInit(QVGA);

  do
  { switch (KEYRead())
    { case KEY1: drive  = 1; turn=1; break;
      case KEY2: drive  = 0;         break;
      case KEY3: drive  = 1; turn=0; break;
      case KEY4: finish = 1;
    }
    CAMGet(img);    // demo
    LCDImage(img);  // only
    f = PSDGet(PSD_FRONT);
    l = PSDGet(PSD_LEFT);
    r = PSDGet(PSD_RIGHT);
    b = PSDGet(PSD_BACK);
    LCDSetPrintf(18,0, "PSD F %4d, L %4d, R %4d B %4d ", f,l,r,b);
    if (drive)
    { if (f>SAFE || !turn) VWStraight(100, 200); // start driving 100mm at 200mm/s
      else if (turn)
      { 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 (!finish);
  return 0;
}
