/* program to follow a dark line on light paper

   original version by Sara Marku, Marija Ivica, Thu Nguyen, Ruth Wu
 */

/* establish constants for clarity
 */

/* identify sensors */
#define left  0
#define right 1

/* identify sensor results */
#define light 0
#define dark  1

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

int main () {

printf ("demonstration program:  Scribbler 2 robot follows a line\n");

printf ("establish Bluetooth connection to robot\n");
  //rConnect("/dev/rfcomm0");             // connection on Linux
  //rConnect ("/dev/tty.IPRE6-365897-DevB");   // curry
  rConnect ("/dev/tty.Fluke2-0958-Fluke2");  // perlis

  eSpeakConnect();
  eSpeakSetGender("female");
  int sensors[2];

  rBeep(.07, 500);
  eSpeakTalk("Following the line");

  printf ("start following line\n");

  int backward_count = 0;
  while(1) {
    rGetLine(sensors, 3);
printf ("line sensors:  %d/%d (left/right)\n", sensors[left], sensors[right]);
    if (sensors[left] == dark)
      { // left sensor over line
        backward_count = 0;
        if (sensors[right] == dark)
           { // line under both left and right sensors:  go straight
             rForward(0.3, 0.5);
             printf ("move forward\n");
           }
        else 
           { // line under left, not right sensor:  move left
             rTurnLeft(0.3, 0.3);
             printf ("turn left\n");
           }
      }
    else 
      { // left sensor NOT over line
       if (sensors[right] == dark)
           { // line under right, not left sensor:  move right
             backward_count = 0;
             rTurnRight (0.3, 0.3);
             printf ("turn right\n");
           }
        else 
           { // line under neither sensor: back up to find iine
             if (backward_count >= 3)
               { // line lost — give up
                 break;
               }
             else
               {
                 rBackward(0.3, 0.3);
                 backward_count++;
                 printf ("move backward\n");
               }
           }
       }
  }

  eSpeakTalk("No more line to follow");
  rBeep(.7, 500);

  eSpeakDisconnect();
  rDisconnect();

  return 0;
}