#include <Ultrasonic.h>
#include <Servo.h>
#define servo1 11 
#define red 4
#define green 5
#define blue 6
//Define os pinos para o trigger e echo
Ultrasonic sensor1(8,7);
Ultrasonic sensor2(10,9);
Servo s1;
long distanciaSensor1, distanciaSensor2;
int pos1; //posicao do servo

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(red, OUTPUT);
  pinMode(blue, OUTPUT);
  pinMode(green, OUTPUT);  
  s1.attach(servo1);
  s1.write(0); // Iniciar servo 1 na posição zero
}

void loop() {
  // put your main code here, to run repeatedly:             
  digitalWrite(red, HIGH);  
  digitalWrite(green, LOW);
  digitalWrite(blue, LOW);  
  distanciaSensor1 = sensor1.Ranging(CM); //retorna em CM 
  delay(20);   
  while (distanciaSensor1 < 20) {
        for (pos1 = 0; pos1 < 90; pos1++) {
            s1.write(pos1);
            delay(40);        
        }   
        distanciaSensor1 = sensor1.Ranging(CM);     
        distanciaSensor2 = sensor2.Ranging(CM);     
        while (distanciaSensor1 < 20 || distanciaSensor2 < 20) {     
           digitalWrite(red, LOW);  
           digitalWrite(green, HIGH);
           digitalWrite(blue, LOW);         
           distanciaSensor1 = sensor1.Ranging(CM);     
           distanciaSensor2 = sensor2.Ranging(CM);     
        }
        digitalWrite(red, HIGH);  
        digitalWrite(green, LOW);
        digitalWrite(blue, LOW);
        for(pos1 = 90; pos1 > 0; pos1--)
        {
          s1.write(pos1);
          delay(40);       
        }    
  }
  
}