/* This program moves the Scribbler 2 robot forward until an obstacle
   is detected
*/

#include <stdio.h>
#include <MyroC.h>

int main ()
{
  /* preliminaries */
  printf ("program to move a robot forward until an obstacle is detected.\n");
  rConnect ("/dev/rfcomm0");
  rSetForwardness("scribbler-forward");

  int pitch = 880;  /* the tone for the note A in the middle of a piano */

  /* check if both the left and right obstacle sensors show no obstacles */
  /* average sensor results over 3 readings */
  while ( (!rGetIRTxt("left", 3)) && (!rGetIRTxt("right", 3)))
    {
      rForward (0.1, 0.1);  /* move forward at 1/10 speed for 0.1 seconds */
      rBeep (0.5, pitch);   /* beep for half a second */
      pitch += 20;          /* increase pitch by 20 for next iteration */
    }

  /* wrap up */
  rDisconnect ();
  printf ("obstacle detected --- stop robot motion\n");

  return 0;
}