#include <Servo.h>

#define AMPLITUDE 130
#define QUADRANTES 5

//O servo vai de 0 a 90. Nesse caso, deve ser calibrado para parar nos 45,
//e assim ele irá rodar para trás abaixo dos 45 e para frente acima dos 45.
#define FF_DIR 0
#define FF_ESQ 90
#define RW_DIR 90
#define RW_ESQ 0
#define STOP 45

#define ESQUERDA -1
#define RETO 0
#define DIREITA 1

Servo rodaDir;
Servo rodaEsq;

int sensorEsq = 0;
int sensorDir = 1;

int direcao;

void setup()
{
  //Serial.begin(4800);
  
  pinMode(sensorEsq, INPUT);
  pinMode(sensorDir, INPUT);
  rodaDir.attach(10);
  rodaEsq.attach(11);

  parar();
}

void loop()
{
  int readingDir = map(analogRead(sensorDir), 0, 400, 0, 1000);
  int readingEsq = map(analogRead(sensorEsq), 0, 400, 0, 1000);
  int diferenca = readingDir - readingEsq;
  delay(40);
  
 // Serial.print("Dir: "); Serial.print(readingDir); Serial.print(" - Esq: "); Serial.print(readingEsq); Serial.print(" - Diff: "); Serial.println(diferenca);

  //Decide para onde ir
  if(readingDir >= 400 || readingEsq >= 400) {
    //Serial.print("Andar para ");
    if(diferenca >= -150 && diferenca <= 150) {
      //Serial.println("frente");
      avancar();
    } else if(diferenca > 0) {
      //Serial.println("direita");
      direita();
    } else {
      //Serial.println("esquerda");
      esquerda();
    }
  } else {
    //Serial.println("parar");
    parar();
  }
}

void parar() {
  rodaDir.write(STOP);
  rodaEsq.write(STOP);
}

void avancar() {
  rodaDir.write(FF_DIR);
  rodaEsq.write(FF_ESQ);
}

void recuar() {
  rodaDir.write(RW_DIR);
  rodaEsq.write(RW_ESQ);
}

void direita() {
  rodaDir.write(STOP);
  rodaEsq.write(FF_ESQ);
}

void esquerda() {
  rodaDir.write(FF_DIR);
  rodaEsq.write(STOP);
}