011-003

const int PinEchoAnteriore = 7;
const int PinTriggerAnteriore = 6;
const int PinEchoSinistra = 11;
const int PinTriggerSinistra = 10;
const int PinEchoDestra = 9;
const int PinTriggerDestro = 8;
const int motorinoL1 = 2;
const int motorinoL2 = 3;
const int motorinoR1 = 4;
const int motorinoR2 = 5;
const int RitardoMedio = 20;

volatile float MassimaDistanzaAnteriore = 25.00;
volatile float DurataPerAnteriore, DistanzaAnterioreInCm;
volatile float DurataPerSinistra, DistanzaSinistraInCm;
volatile float DurataPerDestra, DistanzaDestraInCm;
volatile float MassimaDistanzaSinistra;
volatile float MassimaDistanzaDestra = 20.00;
 
void setup() {
  // serial
  Serial.begin(9600);
  // setta sensori ultrasonici
  pinMode(PinTriggerAnteriore, OUTPUT);
  pinMode(PinEchoAnteriore, INPUT);
  pinMode(PinTriggerSinistra, OUTPUT);
  pinMode(PinEchoSinistra, INPUT);
  pinMode(PinTriggerDestro, OUTPUT);
  pinMode(PinEchoDestra, INPUT);
  // setta i motorini
  pinMode(motorinoL1, OUTPUT);
  pinMode(motorinoL2, OUTPUT);
  pinMode(motorinoR1, OUTPUT);
  pinMode(motorinoR2, OUTPUT);
}

void loop() {
  ControlloDistanzaAnteriore();
  if (DistanzaAnterioreInCm < MassimaDistanzaAnteriore) {
    Serial.println("Troppo vicino!");
    ControlloDistanzaSinistra();
    delay(RitardoMedio);
    ControlloDistanzaDestra();
    delay(RitardoMedio);
    if (DistanzaSinistraInCm < DistanzaDestraInCm)
      MuoviaDestra();
    else if (DistanzaSinistraInCm > DistanzaDestraInCm) {
      MuoviaSinistra();
    }
  }
  else {
    Serial.println("OK");
    VaiAvanti();
  }

  ControlloDistanzaSinistra();
  if (DistanzaSinistraInCm < MassimaDistanzaSinistra) {
    Serial.println("a Sinistra sei troppo vicino");
    delay(RitardoMedio);
    ControlloDistanzaSinistra();
    delay(RitardoMedio);
    ControlloDistanzaDestra();
    delay(RitardoMedio);
    if (DistanzaSinistraInCm > DistanzaDestraInCm)
      VaiAvanti();
    else if (DistanzaSinistraInCm < DistanzaDestraInCm) {
      MuoviaDestra();
    }
  }

  ControlloDistanzaDestra();
  if (DistanzaDestraInCm < MassimaDistanzaDestra) {
    Serial.println("a Destra sei troppo vicino");
    delay(RitardoMedio);
    ControlloDistanzaDestra();
    delay(RitardoMedio);
    ControlloDistanzaSinistra();
    delay(RitardoMedio);
    if (DistanzaDestraInCm > DistanzaSinistraInCm)
      VaiAvanti();
    else if (DistanzaDestraInCm < DistanzaSinistraInCm) {
      MuoviaSinistra();
    }
  }
}

void ControlloDistanzaAnteriore() {
  digitalWrite(PinTriggerAnteriore, LOW); 
  delayMicroseconds(4);
  digitalWrite(PinTriggerAnteriore, HIGH); 
  delayMicroseconds(10);
  digitalWrite(PinTriggerAnteriore, LOW);
  DurataPerAnteriore = pulseIn(PinEchoAnteriore, HIGH); 
  DistanzaAnterioreInCm = DurataPerAnteriore * 10 / 292 / 2; 
  Serial.print("Distanza: ");
  Serial.print(DistanzaAnterioreInCm);
  Serial.println(" cm");
}

void ControlloDistanzaSinistra() {
  digitalWrite(PinTriggerSinistra, LOW);
  delayMicroseconds(4);
  digitalWrite(PinTriggerSinistra, HIGH);  
  delayMicroseconds(10);
  digitalWrite(PinTriggerSinistra, LOW);
  DurataPerSinistra = pulseIn(PinEchoSinistra, HIGH);  
  DistanzaSinistraInCm = DurataPerSinistra * 10 / 292 / 2; 
  Serial.print("Distanza a sinistra: ");
  Serial.print(DistanzaSinistraInCm);
  Serial.println(" cm");
}

void ControlloDistanzaDestra() {
  digitalWrite(PinTriggerDestro, LOW); 
  delayMicroseconds(4);
  digitalWrite(PinTriggerDestro, HIGH); 
  delayMicroseconds(10);
  digitalWrite(PinTriggerDestro, LOW);
  DurataPerDestra = pulseIn(PinEchoDestra, HIGH); 
  DistanzaDestraInCm = DurataPerDestra * 10 / 292 / 2; 
  Serial.print("Distanza a destra: ");
  Serial.print(DistanzaDestraInCm);
  Serial.println(" cm");
}

void VaiIndietro() {
  Serial.println("Indietro.");
  digitalWrite(motorinoL1, HIGH);
  digitalWrite(motorinoL2, LOW);
  digitalWrite(motorinoR1, HIGH);
  digitalWrite(motorinoR2, LOW);
}

void VaiAvanti() {
  Serial.println("Avanti.");
  digitalWrite(motorinoL1, LOW);
  digitalWrite(motorinoL2, HIGH);
  digitalWrite(motorinoR1, LOW);
  digitalWrite(motorinoR2, HIGH);
}

void MuoviaSinistra() {
  Serial.println("Sinistra.");
  digitalWrite(motorinoL1, LOW);
  digitalWrite(motorinoL2, HIGH);
  digitalWrite(motorinoR1, HIGH);
  digitalWrite(motorinoR2, LOW);
}

void MuoviaDestra() {
  Serial.println("Destra.");
  digitalWrite(motorinoL1, HIGH);
  digitalWrite(motorinoL2, LOW);
  digitalWrite(motorinoR1, LOW);
  digitalWrite(motorinoR2, HIGH);
}