#include <AFMotor.h>
//SMARS Demo 1 with Line sensor
//This sketch makes the robot move inside a delimitated Area (you can make a perimeter with insulating tape)
//you'll need an Adafruit Motor shield V1 https://goo.gl/7MvZeo and a IR sensor https://goo.gl/vPWfzx
AF_DCMotor rightMotor(2);
AF_DCMotor leftMotor(1);
// declare variables
int lineNumber;
int trigPin = 10;    // Trigger
int echoPin = 11;    // Echo
long duration, cm, inches;
int speed_L = 200;  
int speed_R = 200;  
String motorSet = "";
#define MOTORS_CALIBRATION_OFFSET 0 // this sets offset to allow for differences between the two DC motors

#define MAX_SPEED 200 // sets speed of DC traction motors to 120/256 or about 47% of full speed - to reduce power draining.
int speedSet = 0;


void setup() {
  Serial.begin(9600);
    //Define inputs and outputs
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  // defines Right motor connector
  // defines Left motor connector
  //defines the variable where it will store the
  // sets up Serial library at 9600 bps
  //changes the following values to make the robot possible drive as straight as

  
  leftMotor.setSpeed(speed_L);
  rightMotor.setSpeed(speed_R); // sets L motor speed
  
  // sets R motor speed
  rightMotor.run(RELEASE);
  leftMotor.run(RELEASE); //turns L motor on
  //turns R motor on
}
void loop() {

  // the following operations will make the robot goes backward for 2 seconds and turns left for 1.5 seconds
//  rightMotor.run(RELEASE);
//  leftMotor.run(RELEASE);
//  recule();
//  delay(2000);
  delay(150);
  int dist = distance();
  dist = min(200, dist);
  if (dist <= 20) {
    while (dist <= 30) {
      moveBackward();
      dist = distance();
      dist = min(200, dist);
    }
    turnRight();
  } 
  else {
    moveForward();
  }
   
  
}
void turnRight() {
  motorSet = "RIGHT";
  leftMotor.run(FORWARD);      // turn motor 1 forward
  rightMotor.run(BACKWARD);     // turn motor 2 backward
  delay(400); // run motors this way for 400ms
  motorSet = "FORWARD";
  leftMotor.run(FORWARD);      // set both motors back to forward
  rightMotor.run(FORWARD);
}
//void avance()
//{
//    // turning left
//  rightMotor.run(FORWARD);
//  leftMotor.run(FORWARD);
//}
//void recule()
//{
//    //go backward
//  rightMotor.run(BACKWARD);
//  leftMotor.run(BACKWARD);
//}

int distance() {
  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
 
  // Read the signal from the sensor: a HIGH pulse whose
  // duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);
 
  // Convert the time into a distance
  cm = (duration/2) / 29.1;     // Divide by 29.1 or multiply by 0.0343
  inches = (duration/2) / 74;   // Divide by 74 or multiply by 0.0135
  
  Serial.print(inches);
  Serial.print("in, ");
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();
  
  return cm;
}

void moveForward() {
  motorSet = "FORWARD";
  leftMotor.run(FORWARD);      // turn it on going forward
  rightMotor.run(FORWARD);      // turn it on going forward
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    leftMotor.setSpeed(speedSet + MOTORS_CALIBRATION_OFFSET);
    rightMotor.setSpeed(speedSet);
    delay(5);
  }
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveBackward() {
  motorSet = "BACKWARD";
  leftMotor.run(BACKWARD);      // turn it on going forward
  rightMotor.run(BACKWARD);     // turn it on going forward
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    leftMotor.setSpeed(speedSet + MOTORS_CALIBRATION_OFFSET);
    rightMotor.setSpeed(speedSet);
    delay(5);
  }
}