#include "eyebot.h"
#include <stdio.h>
#define SAFE 300
#define PSD_FRONT 1
#define PSD_LEFT  2
#define PSD_RIGHT 3

// NOTE: Only 1 robot (ID==1) will use LCD or KEYs
#define ME (id==1)

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

  id = OSMachineID(); // unique ID
  if (ME) LCDSetPrintf(20,0, "my id %d", id);
  if (ME) LCDMenu("", "", "", "END");
  CAMInit(QVGA);

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