Programación de robot con sensor ultrasonico

Estoy haciendo un robot en forma de automóvil que debe remover obstáculos que están alrededor de el como se muestra en la siguiente figura

Ver el archivo adjunto 274171

El robot tiene forma de automóvil, por lo que utilice 4 motores de corriente directa, uno para cada rueda junto con 2 puentes H L298N y un Arduino Mega. Ademas de un sensor ultrasonico HC-SR04 para detectar los obstáculos como se muestra en la siguiente figura:

Ver el archivo adjunto 274172

Y el código de programación es el siguiente:

C-like:
#include <NewPing.h>

int ENA_IZQUIERDO = 22;
int IN1_IZQUIERDO = 24;
int IN2_IZQUIERDO = 26;
int IN3_IZQUIERDO = 28;
int IN4_IZQUIERDO = 30;
int ENB_IZQUIERDO = 32;
int ENA_DERECHO = 40;
int IN1_DERECHO = 42;
int IN2_DERECHO = 44;
int IN3_DERECHO = 46;
int IN4_DERECHO = 48;
int ENB_DERECHO = 50;
int Distancia;

int TRIGGER_PIN = 51;
int ECHO_PIN = 53;
int MAX_DISTANCE = 200;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

void setup() {
  pinMode(ENA_IZQUIERDO, OUTPUT);
  pinMode(IN1_IZQUIERDO, OUTPUT);
  pinMode(IN2_IZQUIERDO, OUTPUT);
  pinMode(IN3_IZQUIERDO, OUTPUT);
  pinMode(IN4_IZQUIERDO, OUTPUT);
  pinMode(ENB_IZQUIERDO, OUTPUT);
  pinMode(ENA_DERECHO, OUTPUT);
  pinMode(IN1_DERECHO, OUTPUT);
  pinMode(IN2_DERECHO, OUTPUT);
  pinMode(IN3_DERECHO, OUTPUT);
  pinMode(IN4_DERECHO, OUTPUT);
  pinMode(ENB_DERECHO, OUTPUT);

}

void loop() {
  unsigned int uS = sonar.ping();
  Distancia = sonar.convert_cm(uS);

  while(Distancia > 55){
    giro();
  }
 
  delay(500);
  adelante();
  delay(1000);
  detenerse();
  delay(500);
  atras();
  delay(1000);
  detenerse();
  delay(500);

}

void adelante(){
  digitalWrite(ENA_IZQUIERDO, HIGH);
  digitalWrite(ENB_IZQUIERDO, HIGH);
  digitalWrite(IN1_IZQUIERDO, LOW);
  digitalWrite(IN2_IZQUIERDO, HIGH);
  digitalWrite(IN3_IZQUIERDO, LOW);
  digitalWrite(IN4_IZQUIERDO, HIGH);

  digitalWrite(ENA_DERECHO, HIGH);
  digitalWrite(ENB_DERECHO, HIGH);
  digitalWrite(IN1_DERECHO, LOW);
  digitalWrite(IN2_DERECHO, HIGH);
  digitalWrite(IN3_DERECHO, LOW);
  digitalWrite(IN4_DERECHO, HIGH); 
}

void detenerse(){
  digitalWrite(ENA_IZQUIERDO, LOW);
  digitalWrite(ENB_IZQUIERDO, LOW);
  digitalWrite(ENA_DERECHO, LOW);
  digitalWrite(ENB_DERECHO, LOW);
}

void atras(){
  digitalWrite(ENA_IZQUIERDO, HIGH);
  digitalWrite(ENB_IZQUIERDO, HIGH);
  digitalWrite(IN1_IZQUIERDO, HIGH);
  digitalWrite(IN2_IZQUIERDO, LOW);
  digitalWrite(IN3_IZQUIERDO, HIGH);
  digitalWrite(IN4_IZQUIERDO, LOW);

  digitalWrite(ENA_DERECHO, HIGH);
  digitalWrite(ENB_DERECHO, HIGH);
  digitalWrite(IN1_DERECHO, HIGH);
  digitalWrite(IN2_DERECHO, LOW);
  digitalWrite(IN3_DERECHO, HIGH);
  digitalWrite(IN4_DERECHO, LOW);
}

void giro(){
  digitalWrite(ENB_IZQUIERDO, HIGH);
  digitalWrite(IN3_IZQUIERDO, LOW);
  digitalWrite(IN4_IZQUIERDO, HIGH);
}

La idea es que el robot gire sobre su propio eje y cuando el sensor detecte un obstáculo, se detenga y avance hacia adelante, desplazando el obstáculo fuera del circulo y que después retroceda hacia el centro. El ciclo se repite hasta que no haya alguna lata dentro del circulo

Sin embargo lo que ocurre es que el robot solo avanza de adelanta hacia atrás y el sensor no parece detectar nada, alguien me podría decir que estoy haciendo mal? o hay algo que debería cambiar?
 
Nada más ver los delays ...

Si pones delay 500 durante medio segundo ni ves ni esquivas nada, te estellas con todo irremediablemente. Quita todos los delays y luego vas viendo que más pasa.


Me quede corto. No miras el sonar cada medio segundo, lo miras cada cuatro segundos y medio y mientras tanto te vas estrellando con todo lo que pillas.
 
Nada más ver los delays ...

Si pones delay 500 durante medio segundo ni ves ni esquivas nada, te estellas con todo irremediablemente. Quita todos los delays y luego vas viendo que más pasa.


Me quede corto. No miras el sonar cada medio segundo, lo miras cada cuatro segundos y medio y mientras tanto te vas estrellando con todo lo que pillas.
Tal vez no me explique bien, el robot no debe esquivar los obstáculos, al contrario debe chocar con ellos y moverlos de lugar.

Imagen pista.jpg

Los obstáculos son latas de refresco, la idea es que el robot gire sobre su propio eje hasta que el sensor detecte una lata, entonces deja de girar y avanza hacia adelante moviendo la lata y después retrocede al centro.

La razón por la que puse los delays es para que los motores se mantuvieran apagados antes de hacer un cambio de sentido, es decir que avance hacia adelante, se detenga, se quede detenido durante medio segundo y después se mueva hacia atrás. Si hago el cambio de sentido de forma inmediata el robot se levanta sobre 2 ruedas.

Ademas los delays sirven para poder controlar por cuanto tiempo el robot se debe de estar moviéndose, ya sea hacia adelante o hacia atrás.
¿ Cual figura ?, no se ve nada
No se porque no se pueden ver, las adjunto aquí nuevamente

Esta es la forma en que el robot debe moverse
Imagen pista.jpg

Este es un diagrama de como tengo conectado todo
robot diagram.JPG
 
Si no vuelves a leer la distancia no la puedes adivinar, tienes que leer el sensor dentro del while.

Código:
void loop() {
  unsigned int uS = sonar.ping();
  Distancia = sonar.convert_cm(uS);

  while(Distancia > 55){
    giro();

    //Agregar esta línea:
    Distancia = sonar.convert_cm(uS);
  }
 
  delay(500);
  adelante();
  delay(1000);
  detenerse();
  delay(500);
  atras();
  delay(1000);
  detenerse();
  delay(500);

}

Osea que miras que es lo que dice el sonar. Si está mas lejos de 55cm entonces giras mientras la distancia sea mayor de 55, es decir por toda la eternidad porque como no vuelves a medir la distancia, el valor que sea será siempre el mismo.
Si dentro del while vuelves a medir la distancia, pues vale, irás girando hasta que "enfoques" a algo a menos de 55.
Bueno, si se supone que tus motores recorren la misma distancia por que si porque al ser 1s recorre lo mismo, pues puede. Lo normal no es hacerlo por tiempo.
 
Si no vuelves a leer la distancia no la puedes adivinar, tienes que leer el sensor dentro del while.

Código:
void loop() {
  unsigned int uS = sonar.ping();
  Distancia = sonar.convert_cm(uS);

  while(Distancia > 55){
    giro();

    //Agregar esta línea:
    Distancia = sonar.convert_cm(uS);
  }
 
  delay(500);
  adelante();
  delay(1000);
  detenerse();
  delay(500);
  atras();
  delay(1000);
  detenerse();
  delay(500);

}

Osea que miras que es lo que dice el sonar. Si está mas lejos de 55cm entonces giras mientras la distancia sea mayor de 55, es decir por toda la eternidad porque como no vuelves a medir la distancia, el valor que sea será siempre el mismo.
Si dentro del while vuelves a medir la distancia, pues vale, irás girando hasta que "enfoques" a algo a menos de 55.
Bueno, si se supone que tus motores recorren la misma distancia por que si porque al ser 1s recorre lo mismo, pues puede. Lo normal no es hacerlo por tiempo.
Gracias, otra duda dices que lo normal no es controlar los motores por tiempo, entonces cual es la manera normal?
 
Arriba