// SONAR: Maxbotix EZ0

#define maxSonares 6          // Quantidade maxima de sonares
#define pinSonar1 A0          // Sonar 1
#define pinSonar2 A1          // Sonar 2

#define triggerPinSonar1 9   // Trigger Sonar 1
#define triggerPinSonar2 8   // Trigger Sonar 2

// AJUSTAR! /////////////////////////////////////////////////////////////////////
#define numSonares 2   // Quantidade de sonares sendo utilizados (max 6)
#define amostras 1     // quantidade de amostras do sonar utilizadas para calcular o valor médio
#define minLimit 6     // limite minimo dos sonares ~15cm para emitir alarme
#define maxLimit 40    // limite maximo dos sonares ~100cm para emitir alarme
/////////////////////////////////////////////////////////////////////////////////

unsigned int pinSonares[] = {pinSonar1, pinSonar2};
unsigned int triggerSonares[] = {triggerPinSonar1, triggerPinSonar2};

void setupSonar(int pin)
{
  pinMode(pin, OUTPUT);
  digitalWrite(pin, HIGH);
  delay(250);
  digitalWrite(pin, LOW);
}

void turnOffSonar(int pin)
{
  pinMode(pin, OUTPUT);
  digitalWrite(pin, LOW);
}

float readSonarVal(int pinSonar, int triggerPin)
{
  float val = 0;
  digitalWrite(triggerPin, HIGH);//ativa o sonar
  delay(49);
  for(int i=0; i<amostras; i++)
    val = val + analogRead(pinSonar);
  digitalWrite(triggerPin, LOW);//desativa o sonar
  delay(20);
  val = (val/amostras)/2; //divide por dois pra corrigir a conversao AD quando ligado no 5V
  return val;
}

void setup()  
{
  // Open serial communications and wait for port to open:
  Serial.begin(57600);
  
  delay(500); //delay para inicializacao e calibracao dos sonares
  for (int i=0; i<numSonares; i++)  //liga os sonares que serao utilizados 
    setupSonar(triggerSonares[i]);
  for (int i=numSonares; i<maxSonares; i++)  //desliga os sonares que nao serao utilizados
    turnOffSonar(triggerSonares[i]);
}

void loop() // run over and over
{ 
  // le os valores de cada sonar
  float valSonares[numSonares]; 
  float minVal;
  for (int i=0; i<numSonares; i++)
  {
    valSonares[i] = readSonarVal(pinSonares[i], triggerSonares[i]);
    Serial.print("sonar ");
    Serial.print(i+1);
    Serial.print(" = ");
    Serial.print(valSonares[i]);
    Serial.print(" inch \t (");
    Serial.print(valSonares[i]*2.54);
    Serial.print(" cm)\t\t");
    
    if (i==0 || minVal > valSonares[i]) //determin menor valor entre todos os sonares
      minVal = valSonares[i];
  }
  Serial.println("");
  
}