Coche Autonomo a partir Coche RC + Arduino. Problemas con el codigo.

Hola a tod@s,

Hace un tiempo compré un coche en un bazar chino con la intencion de convertirlo en un coche autonomo, que esquivara obstaculos y demas. Finalmente hace un par de dias di con la tecla para controlar el movimiento del coche desde arduino.

(Si alguien tiene interes en que pase fotos, no hay problema, pero para mi caso no importan demasiado, creo)

A lo que es el cochecillo, con su traccion trasera y direccion delantera (lo que impide que pueda girar sobre si mismo) le he añadido dos HC-SR04 (ese sensor ultrasonico con el que medir distancias) montados sobre dos servos 9g: uno delante y otro detras.

El codigo con el que estoy empezando, porque cuando funciona habra mil cosas que cambiar o mejorar, pretende que el coche mida lo que tiene delante, si encuentra algo cerca, mida lo que tiene detras a los lados y maniobre hacia atras hacia la mejor de las opciones. Despues, avanza con el “volante” hacia el otro lado y comienza de nuevo (como salir de un aparcamiento).

El problema es que el coche siempre detecta que detras hay algo a “0cm”, o al menos ese es el resultado que obtengo y por alguna razon avanza sin parar.

He probado a cambiar el sensor, los cables, a intercambiar todo (cables, servo, sujeciones) de alante a atras y de atras a adelante. Sigue midiendo 0 detras.

En el loop(), aunque comentada, hay una llamada a “ProbarMedidas()”, que me imprime medidas “normales”, en ningun caso 0cm.

Ahi va el codigo:

#include <Servo.h> // Carga la librería Servo.h que contiene métodos para trabajar con servos

const int maxDist = 40;
const int minDist = 20;

//ECHO HC-SR04
const int fTrigPin = 4;       //Trigger frontal
const int fEchoPin = 5;       //Echo frontal
const int bTrigPin = 6;       //Trigger trasero
const int bEchoPin = 7;       //Echo trasero
long cmF, cmB, durationF, durationB;
//TRACCION Y DIRECCION
const int forward = 13;             //Forward pin 
const int reverse = 12;             //Reverse pin 
const int left = 11;                //Left pin 
const int right = 10;               //Right pin
//SERVOS
Servo fServo;  
Servo bServo;

void setup() { 
  Serial.begin(9600);
  pinMode(forward, OUTPUT);
  pinMode(reverse, OUTPUT);
  pinMode(left, OUTPUT);
  pinMode(right, OUTPUT); 
  pinMode(fTrigPin, OUTPUT);
  pinMode(fEchoPin, INPUT);  
  pinMode(bTrigPin, OUTPUT);
  pinMode(bEchoPin, INPUT);
  bServo.attach(2);
  fServo.attach(3);
  fServo.write(90);
  bServo.write(90);
  delay(1500);
}

/*  Ya que no se puede hacer un Switch/Case con un String, sino solo con Char o Int, cada direccion tiene un numero asignado, de la siguiente manera:
    DIRECCION: "Go(1, 200)"  -> FWD 200ms               SERVO: "MedirF(0)"    -> Medir delante a la Izquierda

                    1                                         0      1      2
             \      |       /                                  \     |     /        ^
        F     6     |     2                                F     \   |   /          |
        -------------------------                            --------------------   |
        B     5     |     3                                B     /   |   \          |
             /      |      \                                   /     |     \        ^
                    4                                         2      1      0

 
 */

void loop() {
  AutoMove();
  //ProbarMedidas();
}

int Go(int dir, int duration){
  switch(dir){
    case 1:
      digitalWrite(forward, HIGH);
      delay(duration);
      digitalWrite(forward, LOW);
    break;
    case 2:
      digitalWrite(forward, HIGH);
      digitalWrite(right, HIGH);
      delay(duration);
      digitalWrite(forward, LOW);
      digitalWrite(right, LOW);
    break;
    case 3:
      digitalWrite(reverse, HIGH);
      digitalWrite(right, HIGH);
      delay(duration);
      digitalWrite(reverse, LOW);
      digitalWrite(right, LOW);
    break;
    case 4:
      digitalWrite(reverse, HIGH);
      delay(duration);
      digitalWrite(reverse, LOW);
    break;
    case 5:
      digitalWrite(reverse, HIGH);
      digitalWrite(left, HIGH);
      delay(duration);
      digitalWrite(reverse, LOW);
      digitalWrite(left, LOW);
    break;
    case 6:
      digitalWrite(forward, HIGH);
      digitalWrite(left, HIGH);
      delay(duration);
      digitalWrite(forward, LOW);
      digitalWrite(left, LOW);
    break;
    default:
      digitalWrite(forward, LOW);
      digitalWrite(reverse, LOW);
      digitalWrite(left, LOW);
      digitalWrite(right, LOW);
    break;
  }
}
long MedirF(int lado){
  switch (lado){
    case 0:     //IZQUIERDA
      fServo.write(180-45);
    break;      
    case 1:     //CENTRO
      fServo.write(180-90);
    break;
    case 2:     //DERECHA
     fServo.write(180-135);
    break;
    default:
      fServo.write(180-90);
    break;
  }
  delay(20);                                      // Dejo tiempo para que el Servo llegue a su posicion antes de empezar a medir
  digitalWrite(fTrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(fTrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(fTrigPin, LOW);
  durationF = pulseIn(fEchoPin, HIGH);
  cmF = microsecondsToCentimeters(durationF);
  return cmF;
}



long MedirB(int lado){
  switch (lado){
    case 0:     //IZQUIERDA
      bServo.write(180-45);   
    break;      
    case 1:     //CENTRO
      bServo.write(180-90);
    break;
    case 2:     //DERECHA
     bServo.write(180-135);
    break;
    default:
      bServo.write(180-90);
    break;
  }
  delay(20);
  digitalWrite(bTrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(bTrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(bTrigPin, LOW);
  durationB = pulseIn(bEchoPin, HIGH);
  cmB = microsecondsToCentimeters(durationB);
  return cmB;
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}


void ProbarMedidas(){
  MedirB(0);
  Serial.print((String)"B(0) = " + cmB +  " cm." + "\t\t\t");
  delay(1000);
  MedirB(1);
  Serial.print((String)"B(1) = " + cmB +  " cm." + "\t\t\t");
  delay(1000);
  MedirB(2);
  Serial.println((String)"B(1) = " + cmB +  " cm.");
  delay(1000);
  MedirF(0);
  Serial.print((String)"F(0) = " + cmF +  " cm." + "\t\t\t");
  delay(1000);
  MedirF(1);
  Serial.print((String)"F(1) = " + cmF +  " cm." + "\t\t\t");
  delay(1000);
  MedirF(2);
  Serial.println((String)"F(1) = " + cmF +  " cm.");
  delay(1000);
}

void AutoMove(){
  if(MedirF(1) > maxDist){                     //MAS DE 30CM DELANTE
    Go(1, 250);                                   //AVANZA
  }else if(minDist <= MedirF(1) <= maxDist){        //MENOS DE 30CM DELANTE PERO MAS DE 20CM
    Go (1, 100);                               //AVANZA UN POCO
  }else if (0 <= MedirF(1) <= minDist){           //ENTRE 0CM Y 20CM
    if(MedirB(1) > maxDist){                        //MAS de 30CM LIBRES DETRAS
      if(MedirF(0)>MedirF(2)){                    //SI DELANTE IZQ > DELANTE DER
        Go (4, 150);                                //AVANZA HACIA ATRAS
        Go (6, 125);                                //AVANZA HACIA ADELANTE IZQ
      }else{                                      //SI DELANTE DER > DELANTE IZQ
        Go (4, 150);                                //AVANZA HACIA ATRAS
        Go (2, 125);                                //AVANZA HACIA ADELANTE DER
      }
    }else if(MedirB(0)>=MedirB(2)){             //SI DETRAS IZQ > DETRAS DER
      Go(3, 150);                                 //AVANZA HACIA ATRAS DER
      Go(6, 150);                                 //AVANZA HACIA ADELANTE IZQ
    }else if(MedirB(0)<MedirB(2)){                                     //SI DETRAS DER > DETRAS IZQ
      Go(5, 150);                                 //AVANZA HACIA ATRAS IZQ
      Go(2, 150);                                 //AVANZA HACIA ADELANTE DER
    }
  }
  Serial.println((String)"FWD: " + cmF + " cm.");
  Serial.println((String)"BCK: " + cmB + " cm.");
}

Hola de nuevo a tod@s,

Finalmente he hecho funcionar el coche RC hackeado.

Aun no tengo clara la causa del problema, pero he visto que la alimentacion de 4 pilas de 1.2V que llegaba al arduino, permitia que al sensor que mide la distancia le llegaran solo 3.8V, cuando su funcionamiento ideal es a 5V, asi que he añadido dos mas.

Ademas, encontre algunas cosas "raras" en el codigo, que no entiendo del todo pero que, tras cambiarlas, todo ha ido de maravilla.

He procurado comentar el codigo por si hay alguien que quiera hacer algo parecido. Es un coche mas bien tontorron, porque hasta hace dos dias me conformaba con que hiciera lo que se suponia que debia hacer, pero todo lo correspondiente a la "IA" del coche esta metido en la funcion AutoMove() asi que deberia ser facil de toquetear.

Ahora me toca seguir con la logica del coche, a ver si lo hago un pelin mas listo con mis medios.

Espero que algun dia no muy lejano algun novato como yo saque algo util de todo esto.

El codigo:

#include <Servo.h> // Carga la librería Servo.h que contiene métodos para trabajar con servos

const int maxDist = 40;     // Distancia maxima permitida antes de dejar de avanzar y plantearse otras cosas
const int minDist = 20;      // Distancia "critica"
const int maniobraCorta = 100;      //Delay que tendra luego una maniobra, si es corta
const int maniobraLarga = 250;      //Delay que tendra luego una maniobra, si es larga
const int maniobraServo = 250;      //Delay que tendra luego una maniobra del servo
const int recuperacion = 150;        //Delay de recuperacion. Si avanzo Xms y luego inmediatamente retrocedo Xms, el momento heredado del avance no permitira que retroceda todo el tiempo, sino que perdere una parte en "frenar".

//ECHO HC-SR04
const int fTrigPin = 4;       //Trigger frontal
const int fEchoPin = 5;       //Echo frontal
const int bTrigPin = 6;       //Trigger trasero
const int bEchoPin = 7;       //Echo trasero
int medirF0;            //Resultado de hacer pruebas y querer meter las llamadas a las funciones MedirB() y MedirF()
int medirF1;            // en lugar de comparar directamente las llamadas a dichas funciones. 
int medirF2;
int medirB0;            //       if(MedirB(1) == MedirF(1))      equivale a...          medirB = medirB(1);
int medirB1;            //                                                                          medirF = medirF(1);
int medirB2;            //                                                                          if(medirB == medirF)

                            //                                                      no? :S

long cmF, cmB, durationF, durationB;
//TRACCION Y DIRECCION
const int forward = 13;             //Forward pin 
const int reverse = 12;             //Reverse pin 
const int left = 11;                //Left pin 
const int right = 10;               //Right pin
//SERVOS
Servo fServo;  
Servo bServo;

void setup() { 
  Serial.begin(9600);
  pinMode(forward, OUTPUT);
  pinMode(reverse, OUTPUT);
  pinMode(left, OUTPUT);
  pinMode(right, OUTPUT); 
  pinMode(fTrigPin, OUTPUT);
  pinMode(fEchoPin, INPUT);  
  pinMode(bTrigPin, OUTPUT);
  pinMode(bEchoPin, INPUT);
  bServo.attach(2);
  fServo.attach(3);
  fServo.write(90);
  bServo.write(90);
  delay(maniobraServo);
}

/*  Ya que no se puede hacer un Switch/Case con un String, sino solo con Char o Int, cada direccion tiene un numero asignado, de la siguiente manera:
    DIRECCION: "Go(1, 200)"  -> FWD 200ms               SERVO: "MedirF(0)"    -> Medir delante a la Izquierda
                    1                                         0      1      2
             \      |       /                                  \     |     /        ^
        F     6     |     2                                F     \   |   /          |
        -------------------------                            --------------------   |
        B     5     |     3                                B     /   |   \          |
             /      |      \                                   /     |     \        ^
                    4                                         2      1      0

 
 */

void loop() {
  AutoMove();
  
  //ProbarMedidas();             --->  NO SE USA, SOLO PARA "DEBUG"
}

void Go(int dir, int duration){
  switch(dir){
    case 1:
    {                                             //       <----------- Esas llaves "{  }" solo son obligatorias en caso de que declare
                                                  //                          variables, no?
      Serial.println("Go1");
      digitalWrite(forward, HIGH);
      delay(duration);
      digitalWrite(forward, LOW);
      break;
    }
    case 2:
    {
      Serial.println("Go2");
      digitalWrite(forward, HIGH);
      digitalWrite(right, HIGH);
      delay(duration+recuperacion);
      digitalWrite(forward, LOW);
      digitalWrite(right, LOW);
    }
    break;
    case 3:
    {
      Serial.println("Go3");
      digitalWrite(reverse, HIGH);
      digitalWrite(right, HIGH);
      delay(duration+recuperacion);
      digitalWrite(reverse, LOW);
      digitalWrite(right, LOW);
    }
    break;
    case 4:
    {
      Serial.println("Go4");
      digitalWrite(reverse, HIGH);
      delay(duration);
      digitalWrite(reverse, LOW);
    }
    break;
    case 5:
    {
      Serial.println("Go5");
      digitalWrite(reverse, HIGH);
      digitalWrite(left, HIGH);
      delay(duration+recuperacion);
      digitalWrite(reverse, LOW);
      digitalWrite(left, LOW);
    }
    break;
    case 6:
    {
      Serial.println("Go6");
      digitalWrite(forward, HIGH);
      digitalWrite(left, HIGH);
      delay(duration+recuperacion);
      digitalWrite(forward, LOW);
      digitalWrite(left, LOW);
    }
    break;
    default:
    {
      Serial.println("GoDef");
      digitalWrite(forward, LOW);
      digitalWrite(reverse, LOW);
      digitalWrite(left, LOW);
      digitalWrite(right, LOW);
    }
    break;
  }
}
long MedirF(int lado){
  switch (lado){
    case 0:     //IZQUIERDA
      fServo.write(180-45);
    break;      
    case 1:     //CENTRO
      fServo.write(180-90);
    break;
    case 2:     //DERECHA
     fServo.write(180-135);
    break;
    default:
      fServo.write(180-90);
    break;
  }
  delay(maniobraServo);                                      
  digitalWrite(fTrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(fTrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(fTrigPin, LOW);
  durationF = pulseIn(fEchoPin, HIGH);
  cmF = microsecondsToCentimeters(durationF);
  return cmF;
}



long MedirB(int lado){
  switch (lado){
    case 0:     //IZQUIERDA
      bServo.write(180-45);   
    break;      
    case 1:     //CENTRO
      bServo.write(180-90);
    break;
    case 2:     //DERECHA
     bServo.write(180-135);
    break;
    default:
      bServo.write(180-90);
    break;
  }
  delay(maniobraServo);
  digitalWrite(bTrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(bTrigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(bTrigPin, LOW);
  durationB = pulseIn(bEchoPin, HIGH);
  cmB = microsecondsToCentimeters(durationB);
  return cmB;
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}


void AutoMove(){
  
  medirF0 = MedirF(0);
  medirF1 = MedirF(1);
  medirF2 = MedirF(2);
  medirB0 = MedirB(0);
  medirB1 = MedirB(1);
  medirB2 = MedirB(2);
  
  if(medirF1 > maxDist){                                                    //SI MAS DE 30CM DELANTE
    Go(1, maniobraLarga);
  }else if((minDist <= medirF1) && (medirF1 <= maxDist)){       //MENOS DE 40CM DELANTE PERO MAS DE 20CM
    Go(1, maniobraCorta);
  }else if (0 <= medirF1 <= minDist){                                   //ENTRE 0CM Y 20CM
    if(medirB1 > maxDist){                                                        //MAS de 40CM LIBRES DETRAS
      if(medirF0 >= medirF2){                                                          //SI DELANTE IZQ > DELANTE DER
        Go(4, maniobraLarga);
        Go(6, maniobraLarga);
      }else{                                                                                 //SI  DELANTE DER > DELANTE IZQ
        Go(4, maniobraLarga);
        Go(2, maniobraLarga);
      }
    }else if(medirB0 >= medirB2){                                              //MENOS DE 40CM DETRAS Y DETRAS IZQ > DETRAS DER
      Go(3, maniobraLarga);
      Go(6, maniobraLarga);
    }else if(medirB0 < medirB2){                                                //MENOS DE 40CM DETRAS Y DETRAS DER > DETRAS IZQ
      Go(5, maniobraLarga);
      Go(2, maniobraLarga);
    }
  }
  Serial.println((String)"FWD: " + cmF + " cm.");
  Serial.println((String)"BCK: " + cmB + " cm.");
}

void ProbarMedidas(){
  cmB = MedirB(0);
  Serial.print((String)"B(0) = " + cmB +  " cm." + "\t\t\t");
  delay(1000);
  cmB = MedirB(1);
  Serial.print((String)"B(1) = " + cmB +  " cm." + "\t\t\t");
  delay(1000);
  cmB = MedirB(2);
  Serial.println((String)"B(2) = " + cmB +  " cm.");
  delay(1000);
  cmF = MedirF(0);
  Serial.print((String)"F(0) = " + cmF +  " cm." + "\t\t\t");
  delay(1000);
  cmF = MedirF(1);
  Serial.print((String)"F(1) = " + cmF +  " cm." + "\t\t\t");
  delay(1000);
  cmF = MedirF(2);
  Serial.println((String)"F(2) = " + cmF +  " cm.");
  delay(1000);
}