/*********************************************/
/* Random Drive                              */
/* Thomas Braunl, UWA 2002                   */
/*********************************************/
#include "eyebot.h"
#include <stdlib.h>
#include <math.h>
#define SAFETY 300

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

  LCDPrintf("Random Drive\n\n");
  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);

  while(KEYRead() != KEY4)
  { if ( PSDGet(left) >SAFETY && PSDGet(front)>SAFETY
     &&  PSDGet(right)>SAFETY && !VWStalled(vw) )
	      VWDriveStraight(vw, 0.5, 0.3);
    else
    { LCDPutString("back up, ");
      VWDriveStraight(vw,-0.04,0.3);
      VWDriveWait(vw);
      LCDPutString("turn\n");
      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;
}