/*
This is a sketch for the Adafruit assembled Motor Shield for Arduino v2
It won't work with v1.x motor shields! Only for the v2's with built in PWM
control

For use with the Adafruit Motor Shield v2
---->  http://www.adafruit.com/products/1438
*/

#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 MotorR(4);
AF_DCMotor MotorL(3);

//ultrasonic setup:
 int distancecm=0;
 const int trigPin = 10; // trig pin connected to Arduino's pin 10
 const int echoPin = 11; // echo pin connected to Arduino's pin 11
// defines variables
long duration;
int distance;

void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input

  // Set the speed to start, from 0 (off) to 255 (max speed)
  // sometimes the motors don't have the same speed, so use these values tomake your SMARS move straight
  MotorL.setSpeed(120);
  MotorR.setSpeed(120);
  MotorL.run(FORWARD);
  MotorR.run(FORWARD);
  // turn on motor
  MotorL.run(RELEASE);
  MotorR.run(RELEASE);
}

// main program loop
void loop() {

 distancecm=mdistance();   //if the distance is less than 5cm, SMARS will go backward for 1 second, and turn right for 1 second
  if(distance<15){
    MotorL.run(BACKWARD);
    MotorR.run(BACKWARD);
    delay(1000);
    MotorL.run(FORWARD);
    MotorR.run(BACKWARD);
    delay(1000);
  }
  else {
    MotorL.run(FORWARD); //otherwise it will continue forward
    MotorR.run(FORWARD);
  }
}

//ultrasonic distance mesurement function
int mdistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  distance= duration*0.034/2;
  // Prints the distance on the Serial Monitor
  Serial.print("Distance: ");
  Serial.println(distance);

}