/* * * * * * * * * * * * * * * * * * * * * * * * * * *
 *                 Progetto Giacomino                *
 *   Robot evita-ostacoli con sensore a ultrasuoni   *
 *                                                   *
 *  Materiali usati:                                 *
 *  - Motor Shield by Adafruit                       *
 *  - Sensore Ultrasuoni                             *
 *  - Due fotocellule IR come encoder (contapassi)   *
 *                                                   *
 *   Codice scritto dal Gruppo Lunedino (@ GOLEM)    *
 * * * * * * * * * * * * * * * * * * * * * * * * * * */

/******* COSE RIGUARDANTI I MOTORI *******/
// Libreria per il Motor Shield
#include <AFMotor.h>

// I due motori collegati agli ingressi 1 e 2 della Motor Shield
AF_DCMotor motorsx(1);
AF_DCMotor motordx(2);

// Costanti di velocità dei motori (/255)...
// ...quando deve girare
const byte TURNSPEED  = 140;
// ...quando deve andare diritto
const byte MAXSPEED   = 180;

/******* COSE RIGUARDANTI IL SENSORE ULTRASUONI *******/
// Costanti per i pin del sensore ultrasuoni
const byte TRIGGERPIN = A1;
const byte ECHOPIN    = A0;

// Valore di soglia del sensore per discriminare un ostacolo
// (se < SOGLIA si è in presenza di ostacolo)
const int DISTANZA_SOGLIA_VICINA  = 500;
const int DISTANZA_SOGLIA_LONTANA = 800;

// Variabile per ricordarsi se ha incontrato un ostacolo
bool ostacolo = false;

// Variabile per memorizzare la distanza dell'ostacolo
unsigned long int distanza_old;

/******* COSE RIGUARDANTI GLI ENCODER *******/
// Costanti per associare il colore "nero"=1 e "bianco"=0
const byte ENCODER_NERO   = 1;
const byte ENCODER_BIANCO = 0;

// Valore (in scala /1023) per stabilire se il colore
// letto è nero (>SOGLIA) oppure bianco (<SOGLIA)
const int ENCODER_SOGLIA = 400;

// Numero di "fette" sulla ruota
const byte TACCHERUOTA = 20;

byte passi_distanza;


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

  // Set-Up per il sensore a ultrasuoni
  pinMode(TRIGGERPIN, OUTPUT);
  pinMode(ECHOPIN, INPUT);

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

void loop() {
  /* INIZIO Misurazione della distanza */
  long int distanza;

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

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

  digitalWrite(TRIGGERPIN, LOW);
  distanza = pulseIn(ECHOPIN, HIGH);
  /* FINE Misurazione della distanza */

  // Casomai la distanza venisse negativa, ricomincia da capo
  if (distanza < 0)
    return;

  // Se tutto è andato a buon fine stampa la distanza via seriale (debug)
  Serial.println(distanza);


  // Blocco decisionale: se becco un ostacolo torno leggermente indietro e ruoto
  if (distanza < DISTANZA_SOGLIA_VICINA && ostacolo == false) {

    // Memorizzo che ho beccato un ostacolo
    ostacolo = true;

    /* INIZIO movimento di 10 passi indietro */
    // Serve per allontanarsi dall'ostacolo

    // Imposto una velocità di manovra (più lenta)
    motordx.setSpeed(TURNSPEED);
    motorsx.setSpeed(TURNSPEED);
    // Marcia indietro
    motorsx.run(BACKWARD);
    motordx.run(BACKWARD);

    // Variabile di appoggio per contare i passi
    byte passi = 0;
    // Leggo il colore della riga sulla ruota e memorizzo 1 se nero e 0 se bianco in "old"
    // confrontando con la soglia
    bool colore_old = analogRead(A5) > ENCODER_SOGLIA ? ENCODER_NERO : ENCODER_BIANCO;

    do {
      // Faccio un'altra lettura, come prima
      bool colore = analogRead(A5) > ENCODER_SOGLIA ? ENCODER_NERO : ENCODER_BIANCO;

      // Se la lettura vecchia è diversa da quella attuale (ovvero è cambiato colore)...
      if ( colore != colore_old ) {
        // ... ho fatto un passo
        passi++;
        // ... memorizzo che ho cambiato colore
        colore_old = colore;

        // Per monitorare scrivo a quanti passi sono (debug)
        Serial.print("S:");
        Serial.println(passi);
      }
    }
    // Ripeto finché non conta 10 passi
    while (passi < 10);

    /* FINE movimento di 10 passi indietro */
    
    /* INIZIO rotazione finché non ho spazio libero */
    // Il robot ruota su sé stesso e cerca la direzione con meno ostacoli
    
    // Imposto un motore avanti e uno indietro (ruota)
    motordx.run(BACKWARD);
    motorsx.run(FORWARD);
    
    do {
      // Valuto la distanza di un eventuale ostacolo
      digitalWrite(TRIGGERPIN, LOW);
      delayMicroseconds(2);
      
      digitalWrite(TRIGGERPIN, HIGH);
      delayMicroseconds(10);
      
      digitalWrite(TRIGGERPIN, LOW);
      distanza = pulseIn(ECHOPIN, HIGH);
    }
    // Ripeto finché non ho sufficiente spazio libero
    while (distanza > DISTANZA_SOGLIA_LONTANA );
    /* FINE rotazione di 360° */
    
    // azzero il contatore
    passi = 0;
    
    // Faccio un ulteriore paio di passi ruotando
    do {
      // Faccio un'altra lettura, come prima
      bool colore = analogRead(A5) > ENCODER_SOGLIA ? ENCODER_NERO : ENCODER_BIANCO;

      // Se la lettura vecchia è diversa da quella attuale (ovvero è cambiato colore)...
      if ( colore != colore_old ) {
        // ... ho fatto un passo
        passi++;
        // ... memorizzo che ho cambiato colore
        colore_old = colore;

        // Per monitorare scrivo a quanti passi sono (debug)
        Serial.print("S:");
        Serial.println(passi);
      }
    }
    // Ripeto finché non conta 2 passi
    while (passi < 2);

  }
  
  // Altrimenti (se non ci sono più ostacoli)...
  else if (distanza > DISTANZA_SOGLIA_LONTANA) {
    // Motori avanti
    motordx.run(FORWARD);
    motorsx.run(FORWARD);
    // Massima velocità
    motordx.setSpeed(MAXSPEED);
    motorsx.setSpeed(MAXSPEED);
  }
}