// Detecting Collisions with an Ultrasonic Distance Sensor on a Servo using the Adafruit Motor
// Shield v1
// This is a combination of various code sources, and is still a work in progress.
#include <AFMotor.h>
#include <HCSR04.h>
#include <Servo.h>

// set up instances of each motor
AF_DCMotor motorL(1);
AF_DCMotor motorR(3);


// set up ultrasonic sensor
UltraSonicDistanceSensor HCSR04(A4, A5); // trig - LEFT, echo - RIGHT

// set up Servo
Servo myservo;

boolean crash = false;

void checkDistance() {
  float distance;
  distance = HCSR04.measureDistanceCm();
   if (distance < 8) // crash distance is 10 cm or less
  {
    crash = true;
  }
}

void goBackward(int speed, int duration) {
  motorL.setSpeed(speed);
  motorR.setSpeed(speed);

  motorL.run(BACKWARD);
  motorR.run(BACKWARD);

  delay(duration);
  motorL.run(RELEASE);
  motorR.run(RELEASE);

}

void goForward(int duration, int speed)
{
  long a, b;
  boolean noMove = true;
  a = millis();
  do
  {
    checkDistance();
    if (crash == false) {
      motorL.setSpeed(speed);
      motorR.setSpeed(speed);

      motorL.run(FORWARD);
      motorR.run(FORWARD);
    }
    if (crash == true) {
      motorL.run(RELEASE);
      motorR.run(RELEASE);
      goBackward(120, 1000);
      myservo.write(45);
      checkDistance();
      delay(2000);
      myservo.write(90);
      motorL.setSpeed(speed);
      motorR.setSpeed(speed);
      motorL.run(FORWARD);
      motorR.run(BACKWARD);
      delay(500);
      crash = false;
    }
    
    b = millis() - a;
    if (b >= duration)
    {
      noMove = false;
    }
  }
  while (noMove != false);
  // stop motors
  motorL.run(RELEASE);
  motorR.run(RELEASE);
}

void setup() {
  myservo.attach(10);
  myservo.write(90);
  Serial.begin(9600);
  delay(5000);
}

void loop() {
  goForward(1000, 150);
  Serial.println(HCSR04.measureDistanceCm());
  delay(10);
}