Robot UMR com sensor de ultrasom

Tenho feito alguns experimentos com o UMR (Unidade Móvel Robótica) que adquirí no mercado livre.
Um de meus objetivos está sendo fazer o robot andar aleatóriamente em uma sala fechada tentando não ficar preso nem bater nos vários objetos como cadeiras, bancadas, mesas e pessoas ocasionalmente.
Um resultado pode ser observado neste vídeo no youtube. Estiveram pesquisando junto comigo o Prof. Adiel e o aluno Alex, ambos da USS.
O UMR é controlado por uma placa Arduíno, com 2 servos para acionamento e um servo de 180 graus para mover o sensor ultrassom.

O código que utilizado para este experimento (rodado no Arduíno IDE, Linux) é:

#include

#include //Inclue a biblioteca Servos

#define NBR_SVR 3 // Numero de Servos ligados. Maximo de 48 arduino MEGA, 12 para outros ar#define FIRST_SERVO_PIN 8

#define SVR_SON 0 // Servo dotendo  sonar

#define SVR_ESQ 1 // Servo esquerdo

#define SVR_DIR 2 // Servo direito

#define ANTIHOR 1000

#define HORARIO 2000

#define PARADO 70

#define SON_ESQ 0

#define SON_FRT 90

#define SON_DIR 180

#define DISTPAD 40

#define DISTLAT 25

#define CICLE_FWD 3

#define HEARTBEAT 10

#define MAXLOOK 5

// enums

enum SonSvr{Esq,VireEsq,Frt,VireFrt,Dir,VireDir};

enum Estado{Parar, Parado, Mover, Movendo};

//iniciando a função e passando os pinos

Ultrasonic ultrasonic(12,13);

Servo Servos[NBR_SVR] ;

long distanciaF, distanciaE, distanciaD;

long hbCounter = 0;

int sonsvrCount=0;

int estadoCount=0;

unsigned char leituraEdisponivel = 0, leituraFdisponivel = 1, leituraDdisponivel = 0;

unsigned char presoCount = 0;

SonSvr sonsvr;

Estado estado;

void setup() {

  Servos[SVR_SON].attach(8,880,2400);

  Servos[SVR_ESQ].attach(9,880,2400);

  Servos[SVR_DIR].attach(10,880,2400);

  Servos[SVR_ESQ].write(PARADO); //enviando um ângulo de 70 graus o servo para

  Servos[SVR_DIR].write(PARADO);

  estado = Parado;

  Servos[SVR_SON].write(SON_FRT);

  sonsvr = VireEsq;

  // delay inicial

  delay(5000);

  distanciaF = ultrasonic.Ranging(CM);

}

void loop() {

  

  // incrementa contador de heartbeat

  hbCounter++;

  if(hbCounter > 3*MAXLOOK*HEARTBEAT) hbCounter = 0;

  // delay do HEARTBEAT

  delay(HEARTBEAT);

  

  if(sonsvr == VireEsq) {

    Servos[SVR_SON].write(SON_ESQ);

    sonsvr = Esq;

    sonsvrCount=0;

  } else if(sonsvr == VireFrt) {

    Servos[SVR_SON].write(SON_FRT);

    sonsvr = Frt;

    sonsvrCount=0;

  } else if(sonsvr == VireDir) {

    Servos[SVR_SON].write(SON_DIR);

    sonsvr = Dir;

    sonsvrCount=0;

  }

  sonsvrCount++;

  if(sonsvrCount>=MAXLOOK*HEARTBEAT) {

    // sonar em posicao de leitura

    if(sonsvr == Esq) {

      distanciaE = ultrasonic.Ranging(CM);

      if(distanciaE) leituraEdisponivel = 1;

      else leituraEdisponivel = 0;

      sonsvr = VireDir;

    } else if(sonsvr == Frt) {

      distanciaF = ultrasonic.Ranging(CM);

      if(distanciaF) leituraFdisponivel = 1;

      else leituraFdisponivel = 0;

      sonsvr = VireEsq;

    } else if(sonsvr == Dir) {

      distanciaD = ultrasonic.Ranging(CM);

      if(distanciaD) leituraDdisponivel = 1;

      else leituraDdisponivel = 0;

      sonsvr = VireFrt;

    }

    sonsvrCount=0;

  }

  // tomada de decisao de movimento

  estadoCount++;

  if(estado == Parar)

  {

    estadoCount=0;

    parar();

    estado = Parado;

  }

  if(estado == Parado) {

    if(estadoCount >= 2*HEARTBEAT) {

      estado = Mover;

    }

  }

  if(estado == Mover) {

    estadoCount = 0;

    estado = Movendo;

  }

  if(estado == Movendo)

  {

    if(estadoCount >= 8*HEARTBEAT) {

      estado = Parar;

    }

    if (distanciaF <= DISTPAD || distanciaE <= DISTLAT || distanciaD <= DISTLAT)

    {

      parar();

      leituraFdisponivel=0;

    

      //  preciso mudar de direcao!

      if(distanciaD >= DISTPAD && leituraDdisponivel)

      {

        // girar a direita

        direitaRT();

        leituraDdisponivel = 0;

      } else if(distanciaE > DISTPAD && leituraEdisponivel) {

          // girar a esquerda

          esquerdaRT();

          leituraEdisponivel = 0;

      } else {

        presoCount++;

      }

      if(presoCount>6) {

        // ficou preso

        presoCount = 0;

        irAtras();

      }

    }

    else

    {

      // pode ir para frente ...

      irFrente();

    }

  }

}

void irFrente() {

    Servos[SVR_ESQ].writeMicroseconds (ANTIHOR); // antihorario

    Servos[SVR_DIR].writeMicroseconds (HORARIO); // horario

    

}

void irAtras() {

    Servos[SVR_DIR].writeMicroseconds (ANTIHOR); // antihorario

    Servos[SVR_ESQ].writeMicroseconds (HORARIO); // horario

    

}

void parar()

{

  Servos[SVR_ESQ].write(PARADO); //enviando um ângulo de 70 graus o servo para

  Servos[SVR_DIR].write(PARADO);

  estado = Parado;

}

void esquerdaRT()

{

    Servos[SVR_ESQ].writeMicroseconds(HORARIO); // horario

    Servos[SVR_DIR].writeMicroseconds(HORARIO); // horario

}

void direitaRT()

{

    Servos[SVR_ESQ].writeMicroseconds (ANTIHOR); // antihorario

    Servos[SVR_DIR].writeMicroseconds (ANTIHOR); // antihorario

}

void irDireita()

{

    Servos[SVR_ESQ].writeMicroseconds(ANTIHOR); // horario

    Servos[SVR_DIR].write(PARADO);

}

void irEsquerda()

{

    Servos[SVR_DIR].writeMicroseconds(HORARIO); // horario

    Servos[SVR_ESQ].write(PARADO);

}