/* Motion Following Pet Robot aka MollowBot
 * Using Grove kit & sensors
 * Conttributed by: William Hooi / OneMaker Group 2018
 * Inspired and based on motion follow robot by Calvin Kielas-Jensen
*/

#include <Servo.h>
#include <Ultrasonic.h>

Servo fServo;
Ultrasonic lSensor (6); //create left sensor object and attach to pin D6
Ultrasonic rSensor (7); //create right sensor object and attach to pin D7

int threshold = 25; //in cm
int angle = 80; //initial servo angle

long leftRanger;
long rightRanger;

void setup() {

  Serial.begin (9600);
  fServo.attach(3); //servo connected to pin D3
  fServo.write(angle);

}

void loop() {
  // put your main code here, to run repeatedly:

  leftRanger = lSensor.MeasureInCentimeters(); //obtain range in cm for left sensor
  rightRanger = rSensor.MeasureInCentimeters(); //obtain range in cm for right sensor
  Serial.print("Left:");
  Serial.print(leftRanger);
  Serial.print("Right:");
  Serial.println(rightRanger);

  follow(); //run a 'follow' function

}

void follow ()
{
  if (leftRanger <= threshold || rightRanger <=threshold)
  {
    if (leftRanger/rightRanger < 1)
    angle = angle-2; //move left

    if (leftRanger/rightRanger > 1)
    angle = angle+2; //move right

  }
  if (angle > 160)
  {
    angle = 160;
  }
  if (angle < 0)
  {
    angle =0;
  }
  fServo.write(angle);
  delay (15);
}