/*
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
*/ 

#include <Servo.h>

#define trigE 2
#define echoE 5

#define trigD 4
#define echoD 6

int distE = 0;
int distD = 0;

int maoD = 0;
int maoE = 0;


Servo servo;

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

void loop() {
  /*
  digitalWrite(trigE, HIGH);
  delayMicroseconds(5);
  digitalWrite(trigE, LOW);

  float duraE = pulseIn(echoE, HIGH);

  digitalWrite(trigD, HIGH);
  delayMicroseconds(5);
  digitalWrite(trigD, LOW);

  float duraD = pulseIn(echoD, HIGH);

  float distE = duraE / 58;
  float distD = duraD / 58;
  */

//Leitura esquerda
  digitalWrite(trigE, HIGH);
  delayMicroseconds(5);
  digitalWrite(trigE, LOW);

  float xE = pulseIn(echoE, HIGH);
  delayMicroseconds(5);

  digitalWrite(trigE, HIGH);
  delayMicroseconds(5);
  digitalWrite(trigE, LOW);

  float yE = pulseIn(echoE, HIGH);
  delayMicroseconds(5);

  digitalWrite(trigE, HIGH);
  delayMicroseconds(5);
  digitalWrite(trigE, LOW);

  float zE = pulseIn(echoE, HIGH);
  delayMicroseconds(5);

//Leitura direita
  digitalWrite(trigD, HIGH);
  delayMicroseconds(5);
  digitalWrite(trigD, LOW);

  float xD = pulseIn(echoD, HIGH);
  delayMicroseconds(5);

  digitalWrite(trigD, HIGH);
  delayMicroseconds(5);
  digitalWrite(trigD, LOW);

  float yD = pulseIn(echoD, HIGH);
  delayMicroseconds(5);

  digitalWrite(trigD, HIGH);
  delayMicroseconds(5);
  digitalWrite(trigD, LOW);

  float zD = pulseIn(echoD, HIGH);
  delayMicroseconds(5);

//Pegar médias
float mediaE = (xE + yE + zE) / 3;
float mediaD = (xD + yD + zD) / 3;

//Transformar em centimetros
float distE = mediaE / 58;
float distD = mediaD / 58;

//Distância da mão
maoD = map(distD, 100, 0, 90, 180);
maoE = map(distE, 0, 100, 0, 90);


if((distD || distE > 101) || (distD >= 95 && distE >= 95)){
  servo.write(90);
}

if(distD < 95 && distD < distE){
  servo.write(maoD);
}

if(distE < 95 && distE < distD){
  servo.write(maoE);
}

}