#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 ()
{ int dir, l, f, r, id, stop=0;

  id = OSMachineID(); // unique ID
// testing
  fprintf(stderr, "my id %d\n", id);  // to console

//  if (ME) LCDMenu("", "", "", "END");

  do
  { 
    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;
      VWTurn(dir, 45);      // turn random angle
      VWWait();
    }
    OSWait(100);
    //if (ME) stop = KEYRead() == KEY4;
  } while (!stop);
  return 0;
}
