Arduino: Atasco con la programación de un bot de laberinto.

Hola a todos.
Estoy realizando un proyecto fin de curso que consiste en un robot resuelve laberinto, pero no doy con las ordenes adecuadas para que realice el método de girar siempre a la derecha. Si alguien con más conocimientos que yo (que serán la mayoría) me pudiese orientar se lo agradecería.
Dejo copiado el código y a la espera de vuestros buenos consejos. :)

Código:
[COLOR=#7E7E7E]// Ultrasonic - Library for HR-SC04 Ultrasonic Ranging Module.[/COLOR]
[COLOR=#7E7E7E]// Proyecto fin de curso:[/COLOR]
[COLOR=#7E7E7E]// PRADUINO_BOT_113[/COLOR]
[COLOR=#7E7E7E]// BOT RESUELVE LABERINTO[/COLOR]
[COLOR=#7E7E7E]// By: Eumelvi[/COLOR]

[COLOR=#7E7E7E]//se incluye una librería para el control de los sensores SRF05, [/COLOR]
[COLOR=#7E7E7E]//(la librería es para los HR-SC04, pero es válida para los SRF05)[/COLOR]
[COLOR=#7E7E7E]//cortesía de [URL="http://www.ardublog.com"]www.ardublog.com[/URL][/COLOR]
#include <[COLOR=#CC6600]Ultrasonic[/COLOR].h> 

[COLOR=#7E7E7E]//definir variables globales:[/COLOR]
[COLOR=#CC6600]Ultrasonic[/COLOR] ultra(7,6);   [COLOR=#7E7E7E]// asignamos los pines del sensor delantero llamado ultra(pulso7,Eco6)[/COLOR]
[COLOR=#CC6600]Ultrasonic[/COLOR] ultraDer(9,8);  [COLOR=#7E7E7E]// y para el derecho (Trig PIN,Echo PIN)[/COLOR]
[COLOR=#CC6600]int[/COLOR] motor1_a = 3; [COLOR=#7E7E7E]//asignamos los pines de los motores[/COLOR]
[COLOR=#CC6600]int[/COLOR] motor1_b = 4; [COLOR=#7E7E7E]//motor1=derecho[/COLOR]
[COLOR=#CC6600]int[/COLOR] motor2_c = 11; [COLOR=#7E7E7E]//motor2=izquierdo[/COLOR]
[COLOR=#CC6600]int[/COLOR] motor2_d = 12; [COLOR=#7E7E7E]//a-c=adelante/b-d=atrás[/COLOR]

[COLOR=#7E7E7E]//se definen las funciones de los movimientos de motores (direcciones de avance)[/COLOR]

[COLOR=#CC6600]void[/COLOR] adelante() [COLOR=#7E7E7E]//creamos función()[/COLOR]
{
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_a,[COLOR=#006699]HIGH[/COLOR]); [COLOR=#7E7E7E]//se ponen en alto las salidas[/COLOR]
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_b,[COLOR=#006699]LOW[/COLOR]);  [COLOR=#7E7E7E]//a-c y en bajo b-d[/COLOR]
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_c, [COLOR=#006699]HIGH[/COLOR]); 
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_d, [COLOR=#006699]LOW[/COLOR]);
}

[COLOR=#CC6600]void[/COLOR] atras()
{
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_a,[COLOR=#006699]LOW[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_b,[COLOR=#006699]HIGH[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_c, [COLOR=#006699]LOW[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_d, [COLOR=#006699]HIGH[/COLOR]);
}

[COLOR=#CC6600]void[/COLOR] paro()
{
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_a,[COLOR=#006699]LOW[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_b,[COLOR=#006699]LOW[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_c, [COLOR=#006699]LOW[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_d, [COLOR=#006699]LOW[/COLOR]);
}

[COLOR=#CC6600]void[/COLOR] derecha()
{
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_a,[COLOR=#006699]LOW[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_b,[COLOR=#006699]HIGH[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_c, [COLOR=#006699]HIGH[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_d, [COLOR=#006699]LOW[/COLOR]);
}

[COLOR=#CC6600]void[/COLOR] izquierda()
{
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_a,[COLOR=#006699]HIGH[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_b,[COLOR=#006699]LOW[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_c, [COLOR=#006699]LOW[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_d, [COLOR=#006699]HIGH[/COLOR]);
}

[COLOR=#CC6600]void[/COLOR] corrDer()
{
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_a,[COLOR=#006699]HIGH[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_b,[COLOR=#006699]LOW[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_c, [COLOR=#006699]LOW[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_d, [COLOR=#006699]LOW[/COLOR]);
}

[COLOR=#CC6600]void[/COLOR] corrIzq()
{
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_a,[COLOR=#006699]LOW[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR](motor1_b,[COLOR=#006699]LOW[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_c, [COLOR=#006699]HIGH[/COLOR]);
  [COLOR=#CC6600]digitalWrite[/COLOR] (motor2_d, [COLOR=#006699]LOW[/COLOR]);
}


[COLOR=#CC6600]void[/COLOR] [COLOR=#CC6600][B]setup[/B][/COLOR]() 
{
  [COLOR=#CC6600][B]Serial[/B][/COLOR].[COLOR=#CC6600]begin[/COLOR](9600); [COLOR=#7E7E7E]//inicializar las comunicaciones serie[/COLOR]

  [COLOR=#CC6600]pinMode[/COLOR](motor1_a, [COLOR=#006699]OUTPUT[/COLOR]); [COLOR=#7E7E7E]//pone en salida los pines de los motores[/COLOR]
  [COLOR=#CC6600]pinMode[/COLOR](motor1_b, [COLOR=#006699]OUTPUT[/COLOR]); 

  [COLOR=#CC6600]pinMode[/COLOR](motor2_c, [COLOR=#006699]OUTPUT[/COLOR]); 
  [COLOR=#CC6600]pinMode[/COLOR](motor2_d, [COLOR=#006699]OUTPUT[/COLOR]); 
}


[COLOR=#CC6600]void[/COLOR] [COLOR=#CC6600][B]loop[/B][/COLOR]()
{
  [COLOR=#CC6600][B]Serial[/B][/COLOR].[COLOR=#CC6600]print[/COLOR]([COLOR=#006699]"delante: "[/COLOR]);  [COLOR=#7E7E7E]//imprime en monitor[/COLOR]
  [COLOR=#CC6600][B]Serial[/B][/COLOR].[COLOR=#CC6600]print[/COLOR](ultra.[COLOR=#CC6600]Ranging[/COLOR](CM)); [COLOR=#7E7E7E]//escribe en CM or INC las medidas de los sensores[/COLOR]
  [COLOR=#CC6600][B]Serial[/B][/COLOR].[COLOR=#CC6600]print[/COLOR]([COLOR=#006699]" cm     "[/COLOR] );  [COLOR=#7E7E7E]//imprime cm despues de los datos medidos[/COLOR]
  [COLOR=#CC6600]delay[/COLOR](50); [COLOR=#7E7E7E]//espera hasta escribir la siguiente medicion[/COLOR]

  [COLOR=#CC6600][B]Serial[/B][/COLOR].[COLOR=#CC6600]print[/COLOR]([COLOR=#006699]"derecha: "[/COLOR]);
  [COLOR=#CC6600][B]Serial[/B][/COLOR].[COLOR=#CC6600]print[/COLOR](ultraDer.[COLOR=#CC6600]Ranging[/COLOR](CM)); [COLOR=#7E7E7E]// CM or INC[/COLOR]
  [COLOR=#CC6600][B]Serial[/B][/COLOR].[COLOR=#CC6600]println[/COLOR]([COLOR=#006699]" cm"[/COLOR] );
  [COLOR=#CC6600]delay[/COLOR](50);

  [COLOR=#CC6600]int[/COLOR] cm, cm2; [COLOR=#7E7E7E]//declara las variables de las medidas de los sensores[/COLOR]
  cm=ultra.[COLOR=#CC6600]Ranging[/COLOR](CM); [COLOR=#7E7E7E]//cm= a la medida en cm del sensor delantero[/COLOR]
  cm2=ultraDer.[COLOR=#CC6600]Ranging[/COLOR](CM); [COLOR=#7E7E7E]//cm2= a la medida del derecho[/COLOR]

  [COLOR=#7E7E7E]//aquí comienzan las órdenes y decisiones que tomará el bot.[/COLOR]

[COLOR=#CC6600]if[/COLOR](cm2>5&&cm2<10)
{
  adelante();[COLOR=#7E7E7E]//avanza[/COLOR]
  [COLOR=#CC6600]delay[/COLOR](10);[COLOR=#7E7E7E]//10ms[/COLOR]
}

[COLOR=#CC6600]if[/COLOR](cm<8&&cm2>5)
{
  derecha();
  [COLOR=#CC6600]delay[/COLOR](1);
}

[COLOR=#7E7E7E]/*[/COLOR]
[COLOR=#7E7E7E]if(cm2<9&&cm2>8)[/COLOR]
[COLOR=#7E7E7E]  {[/COLOR]
[COLOR=#7E7E7E]    adelante();   [/COLOR]
[COLOR=#7E7E7E]    delay(1); [/COLOR]
[COLOR=#7E7E7E]    paro(); [/COLOR]
[COLOR=#7E7E7E]  }[/COLOR]
[COLOR=#7E7E7E]  else [/COLOR]
[COLOR=#7E7E7E]  {[/COLOR]
[COLOR=#7E7E7E]    if (cm2<8)  [/COLOR]
[COLOR=#7E7E7E]    {  [/COLOR]
[COLOR=#7E7E7E]      izquierda();   [/COLOR]
[COLOR=#7E7E7E]      delay(1);[/COLOR]
[COLOR=#7E7E7E]      paro();   [/COLOR]
[COLOR=#7E7E7E]    }[/COLOR]
[COLOR=#7E7E7E]    else[/COLOR]
[COLOR=#7E7E7E]    {[/COLOR]
[COLOR=#7E7E7E]      derecha();[/COLOR]
[COLOR=#7E7E7E]      delay(1);[/COLOR]
[COLOR=#7E7E7E]      paro();  [/COLOR]
[COLOR=#7E7E7E]    }[/COLOR]
[COLOR=#7E7E7E]  }*/[/COLOR]
}
 
Última edición por un moderador:
Hola, primero que nada ¿qué tipo de motores utilizas? ¿utilizas un puente H?

Bueno he revisado tu código y he notado algunos detalles:

Código:
if(cm<8&&cm2>5)
{
  derecha();  [COLOR="Sienna"]//Invoca al método corrDer(), es el que debería realizar el movimiento, según el código[/COLOR]
  delay(1);    [COLOR="Sienna"]//dale más tiempo para que alcance a generar el movimiento [/COLOR]
}


Saludos!
 
Última edición:
Hola, primero que nada ¿qué tipo de motores utilizas? ¿utilizas un puente H?

Bueno he revisado tu código y he notado algunos detalles:

Código:
if(cm<8&&cm2>5)
{
  derecha();  [COLOR=Sienna]//Invoca al método corrDer(), es el que debería realizar el movimiento, según el código[/COLOR]
  delay(1);    [COLOR=Sienna]//dale más tiempo para que alcance a generar el movimiento [/COLOR]
}
Saludos!
hola Bajager, utilizo dos motores DC, un puente H basado en el L298 y dos sensores HY-SFR05.
derecha(); //NO Invoca al método corrDer(), si no a derecha()
delay(1); //con ese tiempo me hace el giro bien, si le pongo más, no
Voy a pegar el código modificado, hasta ahora es el que mejor ha funcionado, aunque aun no está completo y no realiza bien su cometido.
Con este codigo avanza detectando la pared derecha y manteniéndose entre 8 y 10cm, cuando el sensor delantero (cm) <10, gira a la izquierda, si es mayor, a la derecha.
Código:
#include <Ultrasonic.h> 

//definir variables globales:
Ultrasonic ultra(7,6);   // asignamos los pines del sensor delantero llamado ultra(pulso5,Eco6)
Ultrasonic ultraright(9,8);  // y para el derecho (Trig PIN,Echo PIN)
int motor1_a = 3; //asignamos los pines de los motores
int motor1_b = 4; //motor1=derecho
int motor2_c = 11; //motor2=izquierdo
int motor2_d = 12; //a-c=adelante/b-d=atras

//se definen las funciones de los movimientos de motores (direcciones de avance)

void adelante() 
{
  digitalWrite(motor1_a,HIGH); //se ponen en alto las salidas
  digitalWrite(motor1_b,LOW);  //a-c y en bajo b-d
  digitalWrite (motor2_c, HIGH); 
  digitalWrite (motor2_d, LOW);
}

void atras()
{
  digitalWrite(motor1_a,LOW);
  digitalWrite(motor1_b,HIGH);
  digitalWrite (motor2_c, LOW);
  digitalWrite (motor2_d, HIGH);
}

void paro()
{
  digitalWrite(motor1_a,LOW);
  digitalWrite(motor1_b,LOW);
  digitalWrite (motor2_c, LOW);
  digitalWrite (motor2_d, LOW);
}

void derecha()
{
  digitalWrite(motor1_a,LOW);
  digitalWrite(motor1_b,HIGH);
  digitalWrite (motor2_c, HIGH);
  digitalWrite (motor2_d, LOW);
}

void izquierda()
{
  digitalWrite(motor1_a,HIGH);
  digitalWrite(motor1_b,LOW);
  digitalWrite (motor2_c, LOW);
  digitalWrite (motor2_d, HIGH);
}
void corrDer()
{
  digitalWrite(motor1_a,LOW);
  digitalWrite(motor1_b,LOW);
  digitalWrite (motor2_c, HIGH);
  digitalWrite (motor2_d, LOW);
}
void corrIzq()
{
  digitalWrite(motor1_a,HIGH);
  digitalWrite(motor1_b,LOW);
  digitalWrite (motor2_c, LOW);
  digitalWrite (motor2_d, LOW);
}
void setup() 
{
  Serial.begin(9600); //inicializar las comunicaciones serie
  pinMode(motor1_a, OUTPUT); //pone en salida los pines de los motores
  pinMode(motor1_b, OUTPUT); 
  pinMode(motor2_c, OUTPUT); 
  pinMode(motor2_d, OUTPUT); 
}

void loop()
{
  long cm, cm2; //declara las variables de las medidas de los sensores
 
  cm=ultra.Ranging(CM); //cm= a la medida en cm del sensor delantero
  Serial.print("delante: ");  //imprime en monitor
  Serial.print(cm); //escribe en CM or INC las medidas de los sensores
  Serial.print(" cm     " );  //imprime cm despues de los datos medidos
  cm2=ultraright.Ranging(CM); //cm2= a la medida del derecho
  Serial.print("derecha: ");  //imprime en monitor
  Serial.print(cm2); //escribe en CM or INC las medidas de los sensores
  Serial.println(" cm     " );  //imprime cm despues de los datos medidos

  //aqui* comienzan las ordenes y decisiones que tomara el bot.

  //adelante();//avanza
  //delay(10);//10ms
  //paro();//y para

  if(cm2<10&&cm2>8)//si la distancia del sensor delantero es mayor de 10cm
    //o menor de 8cm y, ademas, la distancia del sensor derecho es menor de 8cm...
  {
    adelante();  //y avanza 
    delay(2);    //otro ms
  }
  else  
  {
    if(cm2>10)
    {
      corrDer();   //gira a la derecha
      delay(1);    //un ms
    }
    else
    {
      corrIzq();
      delay(1);
    }
  }
}
 
Habría que cálcular bien las distancias del laberinto para que haga el giro sin atorarse

Código:
//si quieres que mientras no haya obstaculo enfrente siga hacia delante, entonces la condición seria:
if(cm>10) {
   adelante();
   delay(2);
} 
else { //Si hay obstaculo, paras y checas el sensor derecho 
   paro(); 
   delay(100); //hay que darles un tiempo a los motores para que se detengan
   if(cm2 < 10) {
      izquierda(); 
      delay(1);
   }
   else { 
      derecha();
      delay(1);
}

Saludos!
 
Si sabes a priori el mapa del laberinto me parece que el algoritmo de girar siempre a la derecha no es la mejor opción ya que esta es muy lenta, si se permite podrías agregar botones pre configurados con las soluciones dependiendo de donde partes.
 
Aquí dejo el enlace del objetivo de la prueba: http://www.madridbot.org/pruebas_2013.htm#Laberinto
El código hace lo que promete, gracias, pero aun no consigo definirlo.
Saludos.
http://www.madridbot.org/pruebas_2013.htm#Laberinto

A lo que veo, tu robot se queda girando en la parte central del laberinto.

Una solución seria:

- Crea un arreglo bidimensional de 9 x 9 y llénalo de puros ceros, éste indicaría cada cuadrito de tu laberinto. Por ejemplo el cero indica que no has pasado por ese cuadrito
- Dependiendo de las medidas de cada cuadrito y con ayuda del sensor ultrasónico puedes ir calculando en que cuadrito te encuentras e ir modificando el arreglo, ve incrementando en uno cada vez que pases por ese cuadrito, de esta manera llevarás un control del camino recorrido y sus respectivas repeticiones
- Una vez que se conozca si ya se ha pasado por ese camino podrías evitar repetirlo y entonces buscar una salida apoyandote en los ceros de tu arreglo.


Trata de implementarlo (podría funcionar), saludos!
 
Última edición:
Gracias por responder.
Creo que he dado con algo, lo he probado y funciona, aunque hay que pulirlo un poco para que vaya mas fluido.
Saludos.

Código:
   if(cm2==12){
    adelante();//avanza
    delay(100);//100ms
    paro();//y para
    delay(100);
  }
  else  
  {
    if(cm2>12){
      corrDer();   //corrige a la derecha 
      delay(100);
      paro();
      delay(10);
      adelante();
      delay(10);
      paro();
      delay(10);
    }
    else{
      corrIzq();
      delay(50);
      paro();//y para
      delay(10);
    }
  }
    if(cm<10){
      izquierda();
      delay(100);
      paro();
      delay(10);
    }
 
Yo lo haría de esta forma, puede ser que no este bien hecha en lo que respecta al código. Pero podría asegurarte de que es funcional :)
Con respecto a la fluidez, eso nos lo indicas así lo vamos limpiando :D

Nota: Solo modifiqué los procedimientos 'setup' y 'loop':

Código:
//Estas variables mejor declararlas por fuera del procedimiento 'loop'
long cm, cm2; //Medidas de los sensores
//


void setup() 
{
  Serial.begin(9600); //inicializar las comunicaciones serie
  pinMode(motor1_a, OUTPUT); //pone en salida los pines de los motores
  pinMode(motor1_b, OUTPUT); 
  pinMode(motor2_c, OUTPUT); 
  pinMode(motor2_d, OUTPUT); 

  adelante(); //Empieza con el robot moviéndose hacia delante
}

void loop()
{
 
  cm=ultra.Ranging(CM); //cm= a la medida en cm del sensor delantero
  Serial.print("Delante: ");  //imprime en monitor
  Serial.print(cm); //escribe en CM or INC las medidas de los sensores
  Serial.print(" cm.     " );  //imprime cm despues de los datos medidos
  cm2=ultraright.Ranging(CM); //cm2= a la medida del derecho
  Serial.print("Derecha: ");  //imprime en monitor
  Serial.print(cm2); //escribe en CM or INC las medidas de los sensores
  Serial.println(" cm.     " );  //imprime cm despues de los datos medidos

  //MOVIMIENTO DEL ROBOT
  //Mientras no haya una pared delante (5cm aproximadamente), el robot no se detendrá a girar.
  //Y cuando haya una, el robot girará a la derecha o izquierda (dependiendo en si hay una pared).
  //El robot girará indefinidamente hasta que no encuentre pared por delante suyo,
  //cuando nuevamente volverá a moverse normalmente.


  if(cm<5) //Si hay una pared delante
  {
    if(cm2>5) //Si no hay una pared a la derecha
    {
      derecha(); //Giramos a la derecha
    }
    else //Si hay una pared a la derecha
    {
      izquierda(); //Giramos a la izquierda
    }
  }
  else //Si no hay una pared delante
  {
    adelante(); //Seguimos adelante
  }
}
 
Última edición:
Yo lo haría de esta forma, puede ser que no este bien hecha en lo que respecta al código. Pero podría asegurarte de que es funcional :)
Con respecto a la fluidez, eso nos lo indicas así lo vamos limpiando :D

Nota: Solo modifiqué los procedimientos 'setup' y 'loop':
Gracias tfmtoto, pero para que funcione bien hay que poner primero como premisa que cm2==12cm, y a partir de ahí ir implementando las diferentes opciones. De todas formas ya lo he terminado y ahora estoy con otro proyecto, un rastreador, así que cuando empiece a tener dificultades con el código espero vuestra ayuda.
Saludos.
 
Atrás
Arriba