#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"
#define trigPin A1
#define echoPin A0


// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();

// Select which 'port' M1, M2, M3 or M4. In this case, M1
Adafruit_DCMotor *myMotor = AFMS.getMotor(1);
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(2);
Adafruit_DCMotor *myMotor3 = AFMS.getMotor(3);
Adafruit_DCMotor *myMotor4 = AFMS.getMotor(4);

void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Adafruit Motorshield v2 - DC Motor test!");
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  AFMS.begin();  // create with the default frequency 1.6KHz
  Serial.println("AFMS began."); //not reaching here
 
  // Set the speed to start, from 0 (off) to 255 (max speed)
   myMotor->setSpeed(100);
  myMotor->run(FORWARD);

   myMotor2->setSpeed(100);
  myMotor2->run(BACKWARD);

   myMotor3->setSpeed(100);
  myMotor3->run(BACKWARD);

   myMotor4->setSpeed(100);
  myMotor4->run(FORWARD);
}

void loop() {
  long duration, distance;
  digitalWrite(trigPin, LOW);  // Added this line
  delayMicroseconds(2); // Added this line
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); // Added this line
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration *0.034/2;
  if (distance <= 5){
    myMotor ->run(RELEASE);
    myMotor2->run(RELEASE);
    myMotor3->run(RELEASE);
    myMotor4->run(RELEASE);
    }
  else {
    Serial.print(distance);
    Serial.println(" cm");
    myMotor->run(FORWARD);
    myMotor2->run(BACKWARD);
    myMotor3->run(BACKWARD);
    myMotor4->run(FORWARD);
    myMotor ->setSpeed(100); 
    myMotor2->setSpeed(100); 
    myMotor3->setSpeed(100); 
    myMotor4->setSpeed(100); 
    }
}