#include <Servo.h>

Servo myservo1;

const int trigPin= 7;
const int echoPin = 6;

int duration;
int distance;

void setup() 
{
  Serial.begin(1000);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(13, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(11, OUTPUT);

  myservo1.attach(2);
}


void loop() 
{
// 100cm + green 90deg servo
// 51-100 orange 
// 0-50 red ==== 0 deg servo
  digitalWrite(trigPin, LOW);
  delayMicroseconds(10);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(20);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = 0.03435 * (duration / 2);

  if (distance >= 0 && distance <= 50)
  {

    digitalWrite(11, HIGH);
    digitalWrite(12, LOW);
    digitalWrite(13, LOW);
    myservo1.write(0);
    Serial.println("STOP!!");

  }

  else if (distance >= 51 && distance <=100)
  {
    
    digitalWrite(11, LOW);
    digitalWrite(12, HIGH);
    digitalWrite(13, LOW);
    Serial.println("READY!");

  }
  
  else if (distance >= 101)
  {
    
    digitalWrite(11, LOW);
    digitalWrite(12, LOW);
    digitalWrite(13, HIGH);

    myservo1.write(90);
    Serial.println("DRIVE!");

  }
  else
  {

    digitalWrite(11, LOW);
    digitalWrite(12, LOW);
    digitalWrite(13, LOW);

  }

    Serial.println("Distance: "); 
    Serial.println(distance);
    Serial.println(" cm");
    delay(1000);
 
}