Un robot qui détecte et évite les obstacles

Xukyo

il y a 6 ans

Une fois que votre robot est monté et que l’électronique fonctionne correctement, il est temps de lui donner ses fonctionnalités. Les fonctionnalités sont les « Qui suis-je? » et les « Pourquoi suis-je sur cette Terre? » d’un robot.
Autant de questions existentielles dont vous seul pouvez apporter les réponses. Dans le cas d’un robot mobile, son activité favorite est de se déplacer. Et rien de mieux que de se déplacer de manière autonome afin d’accomplir ses propres tâches. Pour que le robot puisse se déplacer de manière autonome, il doit être capable de détecter son environnement, plus précisément les obstacles, et pouvoir les contourner.

Matériel

Robot mobile Rovy
4x TTGM
DC Motor driver (ici nous utilisons un Arduino Mega prototyping shield et 2x pont en H SN754410)
Batterie 7,4V
Capteur de distance HC-SR04 (ou GP2Y0A21 ou autre)
Arduino Mega

Structure

Dans ce projet, nous utilisons le robot mobile Rovy mais cette solution peut être appliquée à tout type de robot mobile.

Hardware

Microcontrôleur

La carte doit avoir suffisamment d’entrée/sortie pour piloter deux ponts en H. Ici nous utilisons un Arduino Mega mais un UNO suffirait.

Driver

Pour contrôler un moteur CC en vitesse et en direction, les ponts en H sont souvent utilisés comme le SN754410.

HC-SR04

Capteur de distance à ultrasons pour détecter les obstacles

Schéma de montage

Principe de l’algorithme

Sur ce robot, nous disposons d’un capteur de distance fixe sur l’avant. La stratégie d’évitement va être assez simple car il n’y a pas beaucoup d’information à part la présence d’un obstacle ou non.Pour rajouter un peu de données, lorsque le robot rencontre un obstacle, nous allons lui faire regarder à gauche et à droite pour regarder la direction la plus dégagée. Ceci équivaut à avoir le capteur monté sur un servomoteur qui balaierai de gauche à droite (avec, notons-le, un servomoteur en moins).

Nous allons créer, pour décrire les déplacements du robot et organiser l’architecture du code, une machine d’état. Ceci permet de décrire clairement une suite d’action en fonction d’évènements. Dans notre cas, le robot va suivre une suite d’états qui va lui permettre (ou pas) d’éviter les obstacles. Une manière simple de coder cela en Arduino est d’utiliser switch..case.

Un autre outil que nous utilisons dans cet algorithme est la librairie Timer.h qui permet de séquencer des actions. Dans notre cas, nous voulons que le capteur ne soit lu que toutes les 100ms.

Software

Le programme à implémenter dans l’Arduino peut être divisé en étapes simples. Lire la mesure du capteur, sélectionner un état du robot en fonction de la valeur de la mesure et contrôler les moteurs en fonction de l’état sélectionné.

Lire la valeur du capteur de distance

Pour lire le capteur de façon continue sans perturber le fonctionnement du robot, nous allons utiliser la librairie Timer.h qui permet de lancer une fonction à intervalle de temps fixe. Le fonctionnement de cette librairie se rapproche de l’exemple BlinkWithoutDelay qui utilise la fonction millis().

On utilise pour la clarté de lecture la librairie HC-SR04.h. On crée une fonction de lecture que l’on place dans le timer. Pour appeler la fonction, il suffit d’écrire sensorTimer->Update(); à la place 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; 
   }
}

Stratégie d’évitement

En fonction de l’état du capteur et du robot, on choisit la procédure à suivre. Tant qu’il n’y a pas d’obstacle on avance. Si un obstacle se présente, on lance la procédure d’évitement:

on recule,
on regarde à droite (tourne à droite) et on sauvegarde la valeur du capteur
puis à gauche (tourne à gauche) et on sauvegarde la valeur du capteur
En fonction de la valeur du capteur de chaque côté, on tourne à gauche ou à droite jusqu’à ce que le robot ne détecte plus d’obstacle.

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

Fonctions de mouvement du robot

Regardez comment piloter un moteur CC ici.

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

Code Complet

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

Résultat

Si vous désirez plus d’information sur ce projet n’hésitez pas à laisser un commentaire ou à nous envoyer un message.

Sources

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

137,50€

Acheter maintenant