Un robot que detecta y evita obstáculos.

Una vez que su robot está montado y la electrónica funciona correctamente, es hora de darle su funcionalidad. Las características son «¿Quién soy yo?» «Y» ¿Por qué estoy en esta Tierra? De un robot Tantas preguntas existenciales que solo tú puedes responder. En el caso de un robot móvil, su actividad favorita es moverse. Y nada mejor que moverse de forma independiente para realizar sus propias tareas. Para que el robot pueda moverse independientemente, debe ser capaz de detectar su entorno, más precisamente los obstáculos, y poder rodearlos.

Equipo

• Robot móvil Rovy
• 4x TTGM
• Controlador de motor de CC (aquí usamos un escudo de prototipos Arduino Mega y 2x H-bridge SN754410)
• Batería de 7.4V
• Sensor de distancia HC-SR04 (o GP2Y0A21 u otro)
• Arduino Mega

Estructura

En este proyecto, utilizamos el robot móvil Rovy, pero esta solución se puede aplicar a cualquier tipo de robot móvil.

Hardware

• Microcontrolador

La tarjeta debe tener suficiente entrada / salida para conducir dos puentes H. Aquí usamos un Arduino Mega pero un UNO sería suficiente.

• Driver

Para controlar un motor de CC en velocidad y dirección, los puentes en H a menudo se utilizan como el SN754410.

• HC-SR04

Sensor de distancia ultrasónico para detectar obstáculos.

Diagrama de montaje

Principio del algoritmo.

En este robot, tenemos un sensor de distancia fija en la parte frontal del robot. La estrategia de evitación será bastante simple porque no hay mucha información aparte de la presencia de un obstáculo o no. Para agregar un poco de información, cuando el robot se encuentre con un obstáculo, haremos que se vea izquierda y derecha para mirar en la dirección más clara. Esto es equivalente a tener el sensor montado en un servomotor que barre de izquierda a derecha (con, notemos, un servomotor menos).

Vamos a describir los movimientos del robot y organizar la arquitectura del código en una máquina de estados . Esto permite describir claramente una serie de acciones según los eventos. En nuestro caso, el robot seguirá una serie de estados que le permitirán (o no) evitar obstáculos. Una forma simple de codificar esto en Arduino es usar switch..case.

Otra herramienta que utilizamos en este algoritmo es la biblioteca Timer.h que le permite secuenciar acciones. En nuestro caso, queremos que el sensor se lea solo cada 100 ms.

Software

El programa que se implementará en Arduino se puede dividir en pasos simples. Lea la medición del sensor, seleccione un estado del robot de acuerdo con el valor de medición y controle los motores de acuerdo con el estado seleccionado.

Leer el valor del sensor de distancia

Para leer el sensor continuamente sin interrumpir el funcionamiento del robot, utilizaremos la biblioteca Timer.h que permite iniciar una función en un intervalo de tiempo fijo. El funcionamiento de esta biblioteca es similar al ejemplo de BlinkWithoutDelay que usa la función millis ().

La biblioteca HC-SR04.h se usa para mayor claridad de lectura. Creamos una función de lectura que colocamos en el temporizador. Para llamar a la función, simplemente escriba sensorTimer->Update(); en lugar de readSensor();

//Bibliotheque
#include <SR04.h>

#include "Timer.h"


// Sensor definition
#define TRIG_PIN 3
#define ECHO_PIN 2
SR04 sr04 = SR04(ECHO_PIN,TRIG_PIN);
long dist,leftVal,rightVal;

Timer *sensorTimer = new Timer(100);

void setup(){
  sensorTimer->setOnTimer(&readSensor);
  sensorTimer->Start();
}

void loop(){
  sensorTimer->Update();
}

void readSensor() {
   dist=sr04.Distance();
   Serial.print(dist);
   Serial.println("cm");
   if(dist<40){
    if (sensorState==OBS_NO) sensorState=OBS_OK;
   }else if(dist>80){
    sensorState=OBS_NO; 
   }
}

Estrategia de evasión

Dependiendo del estado del sensor y del robot, se elige el procedimiento a seguir. Mientras no haya obstáculo, avanzamos. Si surge un obstáculo, se inicia el procedimiento de evitación:

• volvemos,
• miramos a la derecha (giramos a la derecha) y guardamos el valor del sensor
• luego a la izquierda (gire a la izquierda) y guarde el valor del sensor
• Dependiendo del valor del sensor en cada lado, giramos a la izquierda o derecha hasta que el robot ya no detecte un obstáculo.

void autonomousMode(){
    switch (sensorState){
      case OBS_NO:
        GoForward(Power);
        break;
      case OBS_OK:
        GoBackward(Power);
        sensorState=OBS_SEARCH_RIGHT;
        delay(200);
        break;
      case OBS_SEARCH_RIGHT:
        TurnRight(Power);
        sensorState=OBS_SEARCH_LEFT;
        delay(300);
        sensorTimer->Update();
        rightVal=dist;
        break;
      case OBS_SEARCH_LEFT:
        TurnLeft(Power);
        sensorState=OBS_ESCAPE;
        delay(2*300);
        sensorTimer->Update();
        leftVal=dist;
        break;
      case OBS_ESCAPE:
        if(leftVal-rightVal>=5){
          sensorState=OBS_ESC_LEFT;
        }else{
          sensorState=OBS_ESC_RIGHT;
        }
        break;
      case OBS_ESC_LEFT:
        TurnLeft(Power);
        delay(200);
        break;
      case OBS_ESC_RIGHT:
        TurnRight(Power);
        delay(200);
        break;
      default: //SFAULT
        MotorStop();
        break;
    } 
}

Funciones de movimiento del robot

Vea cómo conducir un motor de CC aquí.

void movementEnable(){
    digitalWrite(enableBridge1,HIGH); 
    digitalWrite(enableBridge2,HIGH);
    digitalWrite(enableBridge3,HIGH);
    digitalWrite(enableBridge4,HIGH);
}

void movementDisable(){
    digitalWrite(enableBridge1,LOW); 
    digitalWrite(enableBridge2,LOW);
    digitalWrite(enableBridge3,LOW);
    digitalWrite(enableBridge4,LOW);
}

void GoForward(int Power){
  analogWrite(MotorForward1,Power);
  analogWrite(MotorReverse1,0);
  analogWrite(MotorForward2,Power);
  analogWrite(MotorReverse2,0);
  analogWrite(MotorForward3,Power);
  analogWrite(MotorReverse3,0);
  analogWrite(MotorForward4,Power);
  analogWrite(MotorReverse4,0);
}

void GoBackward(int Power){
  analogWrite(MotorForward1,0);
  analogWrite(MotorReverse1,Power);
  analogWrite(MotorForward2,0);
  analogWrite(MotorReverse2,Power);
  analogWrite(MotorForward3,0);
  analogWrite(MotorReverse3,Power);
  analogWrite(MotorForward4,0);
  analogWrite(MotorReverse4,Power);
}

void TurnRight(int Power){
  analogWrite(MotorForward1,Power);
  analogWrite(MotorReverse1,0);
  analogWrite(MotorForward2,0);
  analogWrite(MotorReverse2,Power);
  analogWrite(MotorForward3,Power);
  analogWrite(MotorReverse3,0);
  analogWrite(MotorForward4,0);
  analogWrite(MotorReverse4,Power);
}

void TurnLeft(int Power){
  analogWrite(MotorForward1,0);
  analogWrite(MotorReverse1,Power);
  analogWrite(MotorForward2,Power);
  analogWrite(MotorReverse2,0);
  analogWrite(MotorForward3,0);
  analogWrite(MotorReverse3,Power);
  analogWrite(MotorForward4,Power);
  analogWrite(MotorReverse4,0);
}

void Stop(){
  analogWrite(MotorForward1,0);
  analogWrite(MotorReverse1,0);
  analogWrite(MotorForward2,0);
  analogWrite(MotorReverse2,0);
  analogWrite(MotorForward3,0);
  analogWrite(MotorReverse3,0);
  analogWrite(MotorForward4,0);
  analogWrite(MotorReverse4,0);
  movementDisable();
}

Full Code

//Bibliotheque
#include <SR04.h>
#include "Timer.h"

// Sensor definition
#define TRIG_PIN 3
#define ECHO_PIN 2
SR04 sr04 = SR04(ECHO_PIN,TRIG_PIN);
long dist,leftVal,rightVal;
enum sState{
  SFAULT,
  OBS_NO,
  OBS_OK,
  OBS_SEARCH_LEFT,
  OBS_SEARCH_RIGHT,
  OBS_ESCAPE,
  OBS_ESC_LEFT,
  OBS_ESC_RIGHT
};
int sensorState=OBS_NO;

// Motors definition
const int enableBridge1 = 22;
const int enableBridge2 = 23;
const int enableBridge3 = 24;
const int enableBridge4 = 25;

const int MotorForward1 = 11;
const int MotorReverse1 = 10;
const int MotorForward2 = 8;
const int MotorReverse2 = 9;
const int MotorForward3 = 7;
const int MotorReverse3 = 6;
const int MotorForward4 = 4;
const int MotorReverse4 = 5;

// Variables
int Power = 80; //Motor velocity

Timer *sensorTimer = new Timer(100);

/******************************************************************\
* PRIVATE FUNCTION: setup
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Initiate inputs/outputs
*
\******************************************************************/

void setup(){
//pinMode(Pin, INPUT/OUTPUT);
  pinMode(MotorForward1,OUTPUT);
  pinMode(MotorReverse1,OUTPUT);
  pinMode(MotorForward2,OUTPUT);
  pinMode(MotorReverse2,OUTPUT); 
  pinMode(MotorForward3,OUTPUT);
  pinMode(MotorReverse3,OUTPUT);
  pinMode(MotorForward4,OUTPUT);
  pinMode(MotorReverse4,OUTPUT);
  pinMode(enableBridge1,OUTPUT);
  pinMode(enableBridge2,OUTPUT);
  pinMode(enableBridge3,OUTPUT);
  pinMode(enableBridge4,OUTPUT);

  sensorTimer->setOnTimer(&readSensor);
  sensorTimer->Start();
  delay(500);
}

/******************************************************************\
* PRIVATE FUNCTION: loop
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Main Function of the code 
\******************************************************************/

void loop(){
  sensorTimer->Update();
  autonomousMode();
}


void autonomousMode(){
    switch (sensorState){
      case OBS_NO:
        GoForward(Power);
        break;
      case OBS_OK:
        GoBackward(Power);
        sensorState=OBS_SEARCH_RIGHT;
        delay(200);
        break;
      case OBS_SEARCH_RIGHT:
        TurnRight(Power);
        sensorState=OBS_SEARCH_LEFT;
        delay(300);
        sensorTimer->Update();
        rightVal=dist;
        break;
      case OBS_SEARCH_LEFT:
        TurnLeft(Power);
        sensorState=OBS_ESCAPE;
        delay(2*300);
        sensorTimer->Update();
        leftVal=dist;
        break;
      case OBS_ESCAPE:
        if(leftVal-rightVal>=5){
          sensorState=OBS_ESC_LEFT;
        }else{
          sensorState=OBS_ESC_RIGHT;
        }
        break;
      case OBS_ESC_LEFT:
        TurnLeft(Power);
        delay(200);
        break;
      case OBS_ESC_RIGHT:
        TurnRight(Power);
        delay(200);
        break;
      default: //SFAULT
        MotorStop();
        break;
    } 
}

/******************************************************************\
* PRIVATE FUNCTION: readSensor
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
*        
\*****************************************************************/
void readSensor() {
   dist=sr04.Distance();
   Serial.print(dist);
   Serial.println("cm");
   if(dist<40){
    if (sensorState==OBS_NO) sensorState=OBS_OK;
   }else if(dist>80){
    sensorState=OBS_NO; 
   }
}

/******************************************************************\
* PRIVATE FUNCTION: movementEnable
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
*   Enable motor control   
\*****************************************************************/
void movementEnable(){
    digitalWrite(enableBridge1,HIGH); 
    digitalWrite(enableBridge2,HIGH);
    digitalWrite(enableBridge3,HIGH);
    digitalWrite(enableBridge4,HIGH);
}

/******************************************************************\
* PRIVATE FUNCTION: movementDisable
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
*   Disable motor control   
\*****************************************************************/
void movementDisable(){
    digitalWrite(enableBridge1,LOW); 
    digitalWrite(enableBridge2,LOW);
    digitalWrite(enableBridge3,LOW);
    digitalWrite(enableBridge4,LOW);
}

/******************************************************************\
* PRIVATE FUNCTION: GoForward
*
* PARAMETERS:
* ~ int Power motor velocity
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
*        
\*****************************************************************/


void GoForward(int Power){
  analogWrite(MotorForward1,Power);
  analogWrite(MotorReverse1,0);
  analogWrite(MotorForward2,Power);
  analogWrite(MotorReverse2,0);
  analogWrite(MotorForward3,Power);
  analogWrite(MotorReverse3,0);
  analogWrite(MotorForward4,Power);
  analogWrite(MotorReverse4,0);
}

/******************************************************************\
* PRIVATE FUNCTION: GoBackward
*
* PARAMETERS:
* ~ int Power motor velocity
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
*        
\*****************************************************************/

void GoBackward(int Power){
  analogWrite(MotorForward1,0);
  analogWrite(MotorReverse1,Power);
  analogWrite(MotorForward2,0);
  analogWrite(MotorReverse2,Power);
  analogWrite(MotorForward3,0);
  analogWrite(MotorReverse3,Power);
  analogWrite(MotorForward4,0);
  analogWrite(MotorReverse4,Power);
}

/******************************************************************\
* PRIVATE FUNCTION: TurnRight
*
* PARAMETERS:
* ~ int Power motor velocity
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
*        
\*****************************************************************/

void TurnRight(int Power){
  analogWrite(MotorForward1,Power);
  analogWrite(MotorReverse1,0);
  analogWrite(MotorForward2,0);
  analogWrite(MotorReverse2,Power);
  analogWrite(MotorForward3,Power);
  analogWrite(MotorReverse3,0);
  analogWrite(MotorForward4,0);
  analogWrite(MotorReverse4,Power);
}

void TurnLeft(int Power){
  analogWrite(MotorForward1,0);
  analogWrite(MotorReverse1,Power);
  analogWrite(MotorForward2,Power);
  analogWrite(MotorReverse2,0);
  analogWrite(MotorForward3,0);
  analogWrite(MotorReverse3,Power);
  analogWrite(MotorForward4,Power);
  analogWrite(MotorReverse4,0);
}

void MotorStop(){
  analogWrite(MotorForward1,0);
  analogWrite(MotorReverse1,0);
  analogWrite(MotorForward2,0);
  analogWrite(MotorReverse2,0);
  analogWrite(MotorForward3,0);
  analogWrite(MotorReverse3,0);
  analogWrite(MotorForward4,0);
  analogWrite(MotorReverse4,0);
  movementDisable();
}

Result

Si desea obtener más información sobre este proyecto, no dude en dejar un comentario o enviarnos un mensaje.

Fuentes

Programa con Arduino

Conduce un motor DC con Arduino

Aprenda a usar un sensor de distancia HC-SR04

Rovy

Ver más