/* Simple program that makes the robot take a tour around the arena
   if the robot is positioned parallel to a wall. Make sure the "soccer foot"
   isn't raised and thereby blocking the front sensor before you run the 
   program.*/

#include "robocom.h"
#include <action.h>

void myfirstuserprocess(int mypid);
int dist=1000;
int walls=4;

void myfirstuserprocess(int mypid)
{
 /* We measure the distance to the nearest object in front of the robot 
    (note that the sensors on the two eyebots apparently are numbered in
    different orders.) */
  
  dist = ActionPSDGet(mypid,1,0);
  
  while(walls) {
    /* The robot drives towards the wall it is facing 
       if it is to far from it */
    while(dist>200) {
      lprintf("%d\n",dist);
      ActionDriveStraight(mypid,1,3,30);
      
      /* OSSleep is used to avoid queuing a lot of unwanted
	 drive commands. Since the ActionDriveStraight func. is
	 non-blocking several drive commands will be queued during
	 the execution of another drive command and thus the robot
	 will drive too far if you dont use OSSleep in the example */
      
      OSSleep(10); 
      dist = ActionPSDGet(mypid,1,0);
    }
    /* The robot turns 90 degrees */
    ActionDriveTurn(mypid,1,90,90);
    OSSleep(150);
    dist = ActionPSDGet(mypid,1,0);
    dist = ActionPSDGet(mypid,1,0);
    walls--;
  }
  lprintf("done at...%d\n", dist);
  QuitRobocom(mypid);
}

int main(int argc, char* argv[])
{
  InitRobocom();
  InitAction();
  InitProcess(&myfirstuserprocess,42, -1);
  RunProcesses();
  return 0;
}