// Adafruit Motor shield library
#include <AFMotor.h>

AF_DCMotor motorsx(1);
AF_DCMotor motordx(2);
const byte TRIGGERPIN = A0;
const byte ECHOPIN = A1;

// variabile per ricordarsi se ha beccato un ostacolo
bool ostacolo = false;

void setup() {
  // Seriale per verificare le risposte del sensore ultrasuono
  Serial.begin(9600);

  // Pin per il sensore a ultrasuoni
  pinMode(TRIGGERPIN, OUTPUT);
  pinMode(ECHOPIN, INPUT);

  // Set-up dei motori: inizialmente spenti
  motorsx.run(RELEASE);
  motordx.run(RELEASE);

}

void loop() {
  // Misurazione della distanza
  int distanza;

  digitalWrite(TRIGGERPIN, LOW);
  delayMicroseconds(2);

  digitalWrite(TRIGGERPIN, HIGH);
  delayMicroseconds(10);

  digitalWrite(TRIGGERPIN, LOW);
  distanza = pulseIn(ECHOPIN, HIGH);

  // Debugging
  Serial.println(distanza);
  // Fine misurazione

  // Blocco decisionale: se becco un ostacolo avvio rotazione
  if (distanza < 600 && ostacolo == false) {
    ostacolo = true;
    motordx.run(BACKWARD);
    motorsx.run(FORWARD);
    motordx.setSpeed(150);
    motorsx.setSpeed(150);
  }
  // altrimenti riprendo la marcia normale a massima velocità
  else if (distanza >= 600 && ostacolo == true) {
    ostacolo = false;
    motordx.run(FORWARD);
    motorsx.run(FORWARD);
    motordx.setSpeed(255);
    motorsx.setSpeed(255);
  }

  // la pausa è superflua
  //delay(10);
}