/* avoid-obj.c
 *
 * Scribbler moves around avoiding objects. It turns to the side that does not
 * have an object, or turns around if only obstacles are found with sensors.
 *
 * The program uses 800 as a threshold for obstacle being seen using getObstacle.
 *
 * Authors: April O'Neill, Dilan Ustek
 * Date: 7 July 2011
 *
 * Modified by Jerod Weinman, 19 January 2015 and 1 January 2019
 */

#include <MyroC.h>


const int obstacleThreshold = 800; /* Sensor threshold for obstacles  */
const int numIterations = 70; /* number of  sense-move stages to run */
const int numSamples = 5;  /* number of sensor samples to take at each stage */

int 
main (void)
{
  rConnect ("/dev/rfcomm0");  
  rSetForwardnessTxt ("fluke-forward");

  int array[3]; /* Storage for sensor readings */
 
  /* loop to move avoiding obstacles for a specified number of iterations */
  for (int i=0; i < numIterations; i++)
  {
    rGetObstacleAll (array,numSamples);
    
    /* If no object seen, move forward */
    if (array[0]<obstacleThreshold && 
        array[1]<obstacleThreshold && 
        array[2]<obstacleThreshold)
      rMotors(1,1);
    
    /* If object seen on left only, turn right */
    else if (array[0]>obstacleThreshold && 
             array[1]<obstacleThreshold && 
             array[2]<obstacleThreshold)
      rTurnRight(1,0.5);
    
    /* If object seen only in middle, turn right */
    else if (array[0]<obstacleThreshold && 
             array[1]>obstacleThreshold && 
             array[2]<obstacleThreshold)
      rTurnRight(1,0.5);
    
    /* If object seen only on right, turn left */
    else if (array[0]<obstacleThreshold && 
             array[1]<obstacleThreshold && 
             array[2]>obstacleThreshold)
      rTurnLeft(1,0.5);
    
    /* If object seen on right and center, turn right */
    else if (array[0]>obstacleThreshold && 
             array[1]>obstacleThreshold && 
             array[2]<obstacleThreshold)
      rTurnRight(1,0.5);
    
    /* If object seen on center and right, turn left */
    else if (array[0]<obstacleThreshold && 
             array[1]>obstacleThreshold && 
             array[2]>obstacleThreshold)
      rTurnLeft(1,0.5);
    
    /* Else turn around */
    else 
    {
      rBeep(1,600);
      rTurnRight(1,2);
    }
  } // End of loop

  rDisconnect();

  return 0;
} // main