/*********************************************/
/* Random Drive with Camera Display          */
/* Thomas Braunl, UWA 2003                   */
/*********************************************/
#include "eyebot.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>

#define SAFETY 300
#define FRONT  1
#define LEFT   2
#define RIGHT  3


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

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

  while(1){
    CAMGet(img);    // for demonstration only
    LCDImage(img);
    l = PSDGet(LEFT);
    f = PSDGet(FRONT);
    r = PSDGet(RIGHT);
    LCDSetPrintf(0,0, "Distances L F R: %3d %3d %3d   ", l, f, r);

    if (l>SAFETY && f>SAFETY && r>SAFETY && !VWStalled())
      VWStraight( 100, 10); // 100mm at 10mm/s
    else {
      VWStraight(-25, 5);   // back up
      VWWait();
      dir = (((float)rand()/(float)RAND_MAX - 0.5))*180; // random angle -90 .. +90
      VWTurn( dir, 45);
      VWWait();
    }

    OSWait(10);
  }

  return 0;
}