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

#1
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:
#2
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:
#3
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);
    }
  }
}
 
#5
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!
 
#7
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.
 
#8
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:
#9
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);
    }
 
#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:
#11
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.
 

Temas similares

Arriba