// C++ code
//
#include <Servo.h>
#define redLed  4
#define greenLed 2
const int tempoAbertura = 2000; 

long readUltrasonicDistance(int triggerPin, int echoPin){
  pinMode(triggerPin, OUTPUT);  // Clear the trigger
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  // Sets the trigger pin to HIGH state for 10 microseconds
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  // Reads the echo pin, and returns the sound wave travel time in microseconds
  return pulseIn(echoPin, HIGH);
}

Servo servo_6;

void setup(){
  Serial.begin(9600);
  servo_6.attach(6, 500, 2500);
  pinMode(redLed, OUTPUT);
  pinMode(greenLed, OUTPUT);
}

void loop(){
  if(0.01723 * readUltrasonicDistance(10, 9) > 50){
    servo_6.write(0);
    Serial.println("Cancela FECHADA...");
    digitalWrite(redLed, HIGH);
    digitalWrite(greenLed, LOW);
    delay(500);
  }
  else {
    Serial.println("Cancela ABERTA...");
    servo_6.write(90);
    digitalWrite(redLed, LOW);
    digitalWrite(greenLed, HIGH);
    delay(500); // Wait for 500 millisecond(s)
   
  } 
    
  
}