/* Moves the Scribbler around avoiding objects, turning to the side that does
 * not have an object or turning around if only obstacles are found with
 * sensors.
 */

#include <MyroC.h>

int 
main (void)
{
  const int MAX_ITERATIONS = 60; // move for a specific number of iterations
  const int OBSTACLE_THRESHOLD = 800;
  const int NUM_SAMPLES = 10;

  rConnect ("/dev/rfcomm0");  
  rSetForwardnessTxt ("fluke-forward");

  int array[3]; /* array to be given to rGetObstacleAll to store all 3 values */
 
  // loop to move avoiding obstacles for a specified number of iterations
  for (int i=0; i <= MAX_ITERATIONS; i++)
    {
      rGetObstacleAll (array, NUM_SAMPLES);

      // If no object seen, move forward
      if ( array[0] < OBSTACLE_THRESHOLD && 
	   array[1] < OBSTACLE_THRESHOLD && 
	   array[2] < OBSTACLE_THRESHOLD)
        rMotors (1,1);

      // If object seen on left only, turn right
      if (array[0] > array[2] < OBSTACLE_THRESHOLD)
        rTurnRight (1,0.5);

      // If object seen only in middle, turn right
      if (array[0] < OBSTACLE_THRESHOLD && 
	  array[1] > OBSTACLE_THRESHOLD &&
	  array[2] < OBSTACLE_THRESHOLD)
        rTurnRight (1,0.5);

      // If object seen only on right, turn left
      if (array[0] < array[2] > OBSTACLE_THRESHOLD)
        rTurnLeft (1,0.5);

      // Else turn around
      else 
        {
          rBeep (1,600);
          rTurnRight (1,2);
        }
    } // End of loop

  rDisconnect();

  return 0;
} // main