fbpixel

A robot that detects and avoids obstacles

by | 10 Feb 2020 | Projects | 0 comments

Tags: , , ,

Once your robot is mounted and the electronics are working properly, it’s time to give it its functionality. The features are “Who am I?” “And” Why am I on this Earth? Of a robot. So many existential questions that only you can answer. In the case of a mobile robot, his favorite activity is to move. And nothing better than to move independently in order to accomplish your own tasks. For the robot to be able to move independently, it must be able to detect its environment, more precisely obstacles, and be able to go around them.



Equipment

  • Robot mobile Rovy
  • 4x TTGM
  • DC Motor driver (here we use an Arduino Mega prototyping shield and 2x H-bridge SN754410)
  • battery 7,4V
  • Distance sensor HC-SR04 (or GP2Y0A21 or other)
  • Arduino Mega



Structure

In this project, we use the Rovy mobile robot but this solution can be applied to any type of mobile robot.

P1110585 A robot that detects and avoids obstacles

Hardware

  • Microcontroller

The card must have enough input / output to drive two H bridges. Here we use an Arduino Mega but a UNO would be enough.

  • Driver

To control a DC motor in speed and direction, H-bridges are often used like the SN754410.

  • HC-SR04

Ultrasonic distance sensor to detect obstacles



Assembly diagram

rovy-autonomous-sketch A robot that detects and avoids obstacles

Principle of the algorithm

On this robot, we have a fixed distance sensor on the front of the robot. The avoidance strategy will be quite simple because there is not much information apart from the presence of an obstacle or not. To add a little data, when the robot encounters an obstacle, we will make it look left and right to look at the clearest direction. This is equivalent to having the sensor mounted on a servomotor which sweeps from left to right (with, let us note, one servomotor less).

We are going to describe the movements of the robot and organize the architecture of the code into a state machine. This makes it possible to clearly describe a series of actions according to events. In our case, the robot will follow a series of states which will allow it (or not) to avoid obstacles. A simple way to code this in Arduino is to use switch..case.

Another tool that we use in this algorithm is the Timer.h library which allows you to sequence actions. In our case, we want the sensor to be read only every 100ms.



Software

The program to be implemented in the Arduino can be divided into simple steps. Read the sensor measurement, select a robot state according to the measurement value and control the motors according to the selected state.

Read the value of the distance sensor

To read the sensor continuously without disturbing the operation of the robot, we will use the Timer.h library which allows to launch a function at fixed time interval. The operation of this library is similar to the BlinkWithoutDelay example which uses the millis () function.

The HC-SR04.h library is used for clarity of reading. We create a read function which we place in the timer. To call the function, just write sensorTimer->Update(); instead of 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; 
   }
}

Avoidance strategy

Depending on the state of the sensor and the robot, the procedure to be followed is chosen. As long as there is no obstacle, we advance. If an obstacle arises, the avoidance procedure is launched:

  • we go back,
  • we look to the right (turn right) and save the value of the sensor
  • then left (turn left) and save the sensor value
  • Depending on the value of the sensor on each side, we turn left or right until the robot no longer detects an 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;
    } 
}

Robot motion functions

See how to drive a DC motorici.

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

If you would like more information on this project, don’t hesitate to leave a comment or send us a message.

Sources

Program with Arduino

Pilotez un moteur CC avec Arduino

Distance measurement with HC-SR04

Rovy

Submit a Comment

Your email address will not be published. Required fields are marked *