Um robô que detecta e evita obstáculos

Xukyo

il y a 2 ans

Com o seu robô montado e a parte eletrônica funcionando corretamente, é hora de lhe dar sua funcionalidade. A funcionalidade é o « Quem sou eu? » e o « O que estou fazendo aqui? « de um robô. São perguntas existenciais que só você poderá responder.

No caso de um robô móvel, sua atividade favorita é se deslocar. E nada melhor do que deslocar-se autonomamente para cumprir suas próprias tarefas. Para que o robô possa mover-se de forma autônoma, precisa ser capaz de detectar o ambiente ao redor, mais precisamente os obstáculos, e conseguir desviar-se deles.

Material

Robô móvel Rovy
4 x TTGM
DC Motor Driver (aqui usamos um Arduino Mega prototyping shield e 2 pontes H SN754410)
Bateria de 7.4V
Sensor de distância HC-SR04 (ou GP2Y0A21 ou outro)
Arduino Mega

Estrutura

Neste projeto utilizamos o robô móvel Rovy, mas esta solução pode ser aplicada a qualquer tipo de robô móvel.

Hardware

Microcontrolador

A placa deve ter entradas e saídas suficientes para controlar duas pontes H. Aqui usamos um Arduino Mega, mas um UNO bastaria.

Driver

Para controlar a velocidade e a direção de um motor CC, é comum utilizar pontes H como a SN754410.

HC-SR04

Sensor de distância a ultrassom para detecção de obstáculos

Esquema de montagem

Princípio do algoritmo

Este robô dispõe de um sensor de distância fixo na parte dianteira. A estratégia para desvio de obstáculos é bem simples, pois não exige muita informação além da presença ou não de obstáculos. Para acrescentar alguns dados, vamos fazer o robô olhar para esquerda e direita ao encontrar um obstáculo e determinar qual lado está mais desimpedido. Isto equivale a ter o sensor montado num servomotor que varre da esquerda para a direita (mas sem o servomotor, vejam só).

Para descrever os movimentos do robô e organizar a arquitetura do código, vamos criar uma máquina de estados. Isto permite determinar claramente uma sequência de ações em função dos acontecimentos. No nosso caso, o robô seguirá uma sequência de estados que vai lhe permitir evitar obstáculos (ou não). Uma maneira simples de programar isto no Arduino é usando o switch…case.

Outra ferramenta que utilizamos neste algoritmo é a biblioteca Timer.h, que permite sequenciar ações. No nosso caso, queremos que o sensor seja lido apenas a cada 100 m.

Software

O programa a ser implementado no Arduino pode ser dividido em etapas simples: ler a medida do sensor, selecionar um estado do robô em função do valor da medida e controlar os motores de acordo com o estado selecionado.

Ler o valor do sensor de distância

Para ler o sensor de forma contínua sem perturbar o funcionamento do robô, usaremos a biblioteca Timer.h, que permite lançar uma função a intervalos fixos de tempo. O funcionamento desta biblioteca é semelhante ao exemplo BlinkWithoutDelay, que usa a função millis().

Para clareza de leitura, usamos a biblioteca HC-SR04.h. Criamos uma função de leitura que colocamos no temporizador. Para chamar a função, basta escrever sensorTimer->Update(); em vez 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; 
   }
}

Estratégia de desvio

Dependendo do estado do sensor e do robô, escolhemos o procedimento a ser seguido. Se não houver obstáculos, avançamos. Se houver, ativamos o procedimento de desvio:

recuar;
olhar para a direita (virar à direita) e salvar o valor do sensor;
depois à esquerda (virar à esquerda) e salvar o valor do sensor;
Dependendo do valor do sensor de cada lado, seguir para a esquerda ou para a direita até que um novo obstáculo seja detectado.

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;
    } 
}

Funções de deslocamento do robô

Veja aqui como conduzir um motor CC.

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();
}

Código completo

//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();
}

Resultado

Se quiser mais informações sobre este projeto, pode nos deixar um comentário ou escrever uma mensagem.

Fontes

Robot en kit Rovy pour Moteur CC TTGM

Le robot en kit Rovy est une plateforme roulante imprimable en 3D. Il peut être adapté pour différentes marques de moteur DC et est compatible avec différents microcontrôleurs.
Retrouvez les caractéristiques des moteurs ici.. Optimisé pour Moteur CC TTGM

165,00€

Acheter maintenant