/*********************************************/
/* Random Drive with Camera Display          */
/* Thomas Braunl, UWA 2003                   */
/*********************************************/
#include "eyebot.h"
#include <stdlib.h>
#define M_PI 3.14159265358979323
#define SAFETY 300

int main ()
{ PSDHandle front, left, right;
  VWHandle  vw;
  float     dir;
  colimage  img;

  LCDMenu("", "", "", "END");

  vw=VWInit(VW_DRIVE,1);
  VWStartControl(vw, 7.0,0.3,10.0,0.1);
  front = PSDInit(PSD_FRONT);
  left  = PSDInit(PSD_LEFT);
  right = PSDInit(PSD_RIGHT);
  PSDStart(front | left | right , TRUE);
  CAMInit(NORMAL);

  while(KEYRead() != KEY4)
  { 
	CAMGetColFrame(&img,0);  /* for demonstration only */
    LCDPutColorGraphic(&img);
    if ( PSDGet(left) >SAFETY && PSDGet(front)>SAFETY
     &&  PSDGet(right)>SAFETY && !VWStalled(vw) )
     {  
	      VWDriveStraight(vw, 0.5, 0.3);
     }
    else
    { 
      VWDriveStraight(vw,-0.04,0.3);
      VWDriveWait(vw);
      
      dir = M_PI * ((float)rand()/(float)RAND_MAX - 0.5); /* random angle -90 .. +90 */
      VWDriveTurn(vw, dir, 0.6);
		  VWDriveWait(vw);
    }
    OSWait(10);
  }
  VWRelease(vw);
  return 0;
}