Robot evita ostacoli con Arduino, L298N, servo e HC-SR04

/*
  ROBOT CAR EVITA OSTACOLI CON ARDUINO,
  L298N, SERVOMOTORE SG90 E HC-SR04

  HC-SR04:
  TRIG -> D2
  ECHO -> D3

  Servo:
  segnale -> D5

  L298N:
  ENA -> D11
  IN1 -> D10
  IN2 -> D9
  IN3 -> D8
  IN4 -> D7
  ENB -> D6
*/

#include <Servo.h>

Servo servo1;

// Posizione richiesta e posizione precedente del servo
int pos = 0;
int posprec = 0;

// Parametri del movimento del servo
int gradi = 0;
int incremento = 1;

// Pin L298N
const int ENA = 11;
const int IN1 = 10;
const int IN2 = 9;
const int IN3 = 8;
const int IN4 = 7;
const int ENB = 6;

// Correzione tra i due motori
int gap = 0;

// Velocità PWM dei motori
// Nel montaggio di esempio è consigliato un intervallo 120-255
int velocita = 255;

// Tempo indicativo per una rotazione di circa 90°
int rotazione90 = 500;

// Distanza letta dal sensore
float cm = 0;

// ----------------------------------------------------------
// AVANTI
// ----------------------------------------------------------
void avanti() {

  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);

  analogWrite(ENA, constrain(velocita + gap, 0, 255));
  analogWrite(ENB, constrain(velocita, 0, 255));
}

// ----------------------------------------------------------
// ROTAZIONE A SINISTRA
// ----------------------------------------------------------
void sx90() {

  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);

  analogWrite(ENA, constrain(velocita + gap, 0, 255));
  analogWrite(ENB, constrain(velocita, 0, 255));

  delay(rotazione90);

  // Arresta i motori al termine della rotazione
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

// ----------------------------------------------------------
// ROTAZIONE A DESTRA
// ----------------------------------------------------------
void dx90() {

  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);

  analogWrite(ENA, constrain(velocita + gap, 0, 255));
  analogWrite(ENB, constrain(velocita, 0, 255));

  delay(rotazione90);

  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

// ----------------------------------------------------------
// MUOVE IL SERVO E MISURA LA DISTANZA
// ----------------------------------------------------------
void sensore() {

  gradi = posprec;
  incremento = 1;

  // Se dobbiamo spostarci verso un angolo minore,
  // il servo deve procedere al contrario
  if (pos <= posprec) {
    incremento = -1;
  }

  // Movimento progressivo del servo
  for (posprec = gradi; posprec != pos; posprec += incremento) {
    servo1.write(posprec);
    delay(15);
  }

  // Impulso di trigger HC-SR04
  digitalWrite(2, LOW);
  delayMicroseconds(2);

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

  digitalWrite(2, LOW);

  // Conversione della durata dell'eco in centimetri
  unsigned long durata = pulseIn(3, HIGH, 30000UL);

  if (durata == 0) {
    cm = 999.0; // nessun eco valido entro il timeout
  } else {
    cm = durata / 58.0;
  }
}

void setup() {

  // Breve attesa all'accensione
  delay(3000);

  // Servo al centro
  servo1.attach(5);
  servo1.write(90);
  posprec = 90;

  // L298N
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // HC-SR04
  pinMode(2, OUTPUT);
  pinMode(3, INPUT);

  // Motori fermi all'avvio
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

void loop() {

  // Guarda davanti
  pos = 90;
  sensore();

  // Se davanti ci sono più di 30 cm liberi, avanza
  if (cm > 30) {

    avanti();
  }
  else {

    // Ostacolo: ferma i motori
    analogWrite(ENA, 0);
    analogWrite(ENB, 0);

    // Guarda a destra
    pos = 0;
    sensore();

    if (cm > 30) {

      dx90();
    }
    else {

      // Guarda a sinistra
      pos = 180;
      sensore();

      if (cm > 30) {

        sx90();
      }
      else {

        // Nessuno spazio sufficiente:
        // esegue due rotazioni a sinistra
        sx90();
        sx90();
      }
    }
  }
}

int velocita = 255;

int rotazione90 = 500;