/*
ETEC Aristóteles Ferreira
Ensino Médio Integrado com Técnico em Eletrônica
Controle de Motor com Dois Sensores Ultrassonicos

Andrey Celestino
Manuela Marques
Maria Luiza Rodrigues
*/ 

//max servo 2400

#include <Servo.h>

#define trigE 4
#define echoE 9

#define trigD 2
#define echoD 11

//Long guarda mais informações
float distE = 0;
float distD = 0;

float servoPos;
int meio = 90;

int senseE, senseD;

Servo servo;

void setup() {
  pinMode(trigE, OUTPUT);
  pinMode(echoE, INPUT);
  pinMode(trigD, OUTPUT);
  pinMode(echoD, INPUT);
  servo.attach(3);
  Serial.begin(9600);
}

void loop() {
//Pega a distância esquerda
  digitalWrite(trigE, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigE, LOW);

  distE = pulseIn(echoE, HIGH) * 0.034 / 2;

//Pega a distância direita
  digitalWrite(trigD, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigD, LOW);

  distD = pulseIn(echoD, HIGH) * 0.034 / 2;

//Posição do servo
  if(distD >= 30){
    distD = 30;
  }

  if(distE >= 30){
    distE = 30;
  }

  if(distD < 5 && distE < 5){
    distD = meio  distE = meio;
  }

  servoPos = map(distD - distE, -30, 30, 0, 180);
/*
00 - 05 ponto morto
05 - 15 baixo
15 - 20 meio
20 - 30 alto

maior - menor = posservo
*/

//Define o servo
if((distD > 15 && distD < 20 && distE > 15 && distE < 20) || (distD < 5 && distE <5) ){
  servo.write(meio);
}

else{
  servo.write(servoPos);
}

  if(distD <= 5 && distE <= 5){
    servo.write(meio);
  }

Serial.print("Servo = ");
Serial.println(servoPos);


delay(10);

}