I Built This Autonomous Robot to Detect and Avoid Obstacles. The Code is Included.

Advertisement

This is a simple autonomous robot able to detect and avoid obstacles. I use a cheap 4WD robot platform (You can use any of these platforms), an Arduino UNO($18.59 on Amazon), and a cheap HC-SR04 sensor (2 pieces at $2.83 on Amazon).

The robot is programmed to drive forward till an obstacle is detected. Then it turns the sensor left and right. Compare the values returned by the ultrasonic sensor and take a decision.

This is the Arduino code:

/**
 * @file         Arduino UNO Autonomous Robot
 * @author       Calin Dragos for intorobotics.com
 * @version      V1.0
 * @date         13.10.2016
 * @description  This is an Arduino sketch for an autonomous robot able to detect and avoid obstacles
 */
 

#include <AFMotor.h>
#include <Servo.h> 
#include <NewPing.h>

Servo myservo;

int ENABLE_A = 6;
int PIN_A1 = 3;
int PIN_A2 = 2;

int ENABLE_B = 11;
int PIN_B1 = 5;
int PIN_B2 = 4;

int SENSOR_DISTANCE;
int LEFT_SENSOR_DISTANCE;
int RIGHT_SENSOR_DISTANCE;
int SNZ_DISTANCE_L;
int SNZ_DISTANCE_R;
int LFT_SNZ_DIS;
int RGT_SNZ_DIS;

#define TRIG_PIN 7
#define ECHO_PIN 8
#define MIN_DISTANCE 40
#define MAX_DISTANCE 200
#define INTERVAL 200


NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
  
void setup() {
  
   Serial.begin(9600);
   
  //pin mode for the DC motors
   pinMode (ENABLE_A, OUTPUT);
   pinMode (PIN_A1, OUTPUT);
   pinMode (PIN_A2, OUTPUT);

   pinMode (ENABLE_B, OUTPUT);
   pinMode (PIN_B1, OUTPUT);
   pinMode (PIN_B2, OUTPUT); 

   //pin mode for the ultrasonic sensor
   pinMode(TRIG_PIN, OUTPUT);
   pinMode(ECHO_PIN, INPUT);

   //for servo motor
   myservo.attach(9);

  //set the ultrasonic sensor to center   
   sensorCenter(); 
      
  //stop the motors
   stopMotors();   
}


void loop() {
     
    SENSOR_DISTANCE=sensorDistance();
    Serial.print("Front sensor distance is: ");
    Serial.println(SENSOR_DISTANCE);

    if(SENSOR_DISTANCE >= MIN_DISTANCE || SENSOR_DISTANCE==0)
      {
      goForward();
      Serial.println("Go forward");
        }
      else {
        
      //stop the motors
      stopMotors();

      LFT_SNZ_DIS=toTheLeft(); 
      RGT_SNZ_DIS=toTheRight();

          if(LFT_SNZ_DIS >= MIN_DISTANCE && LFT_SNZ_DIS >= RGT_SNZ_DIS) 
           {
            int NEW_SNZ_DIS_LFT;
             //try three times to escape
             for(int i=1;i<=3;i++){
              goLeft();
              Serial.println("Go left");
              delay(300);
              //stop the motors
              stopMotors();
               NEW_SNZ_DIS_LFT=sensorDistance();
               if(NEW_SNZ_DIS_LFT >=MIN_DISTANCE)
                  {
                break;
                }
              
             }
              sensorCenter();
              
          } 
            else if(RGT_SNZ_DIS >= MIN_DISTANCE && RGT_SNZ_DIS >= LFT_SNZ_DIS)
            {
             int NEW_SNZ_DIS_RGT;
             //try three times to escape  
             for(int j=1;j<=3;j++){
              goRight();
              Serial.println("Go right");
              delay(300);
              //stop the motors
              stopMotors();
              NEW_SNZ_DIS_RGT=sensorDistance();
              if(NEW_SNZ_DIS_RGT >=MIN_DISTANCE)
                  {
                break;
                }
              
             }
              sensorCenter(); 
          }
         else 
            {
              goBackward();
              Serial.println("Go backward");
              delay(500);      
              //stop the motors
              stopMotors();
              sensorCenter();
            }
      }

} 


void stopMotors(){
  analogWrite (ENABLE_A, 0);
  analogWrite (ENABLE_B, 0);
  digitalWrite (PIN_A1, LOW);
  digitalWrite (PIN_A2, LOW);
  digitalWrite (PIN_B1, LOW);
  digitalWrite (PIN_B2, LOW); 
  }
 
int sensorDistance(){
  int distance;
  int uS = sonar.ping();
  distance = uS / US_ROUNDTRIP_CM;
  if(distance !=0){
  return distance;
  delay(300);
    }
  }

void goForward(){
  analogWrite (ENABLE_A, 255);
  digitalWrite (PIN_A1, HIGH);
  digitalWrite (PIN_A2, LOW);
  analogWrite (ENABLE_B, 255);
  digitalWrite (PIN_B1, HIGH);
  digitalWrite (PIN_B2, LOW);
  }

void goBackward(){
  analogWrite (ENABLE_A, 180);
  digitalWrite (PIN_A1, LOW);
  digitalWrite (PIN_A2, HIGH);
  analogWrite (ENABLE_B, 180);
  digitalWrite (PIN_B1, LOW);
  digitalWrite (PIN_B2, HIGH);
  }


void goLeft(){
  analogWrite (ENABLE_A, 180);
  digitalWrite (PIN_A1, HIGH);
  digitalWrite (PIN_A2, LOW);
  analogWrite (ENABLE_B, 180);
  digitalWrite (PIN_B1, LOW);
  digitalWrite (PIN_B2, HIGH);
}


void goRight(){
  analogWrite (ENABLE_A, 180);
  digitalWrite (PIN_A1, LOW);
  digitalWrite (PIN_A2, HIGH);
  analogWrite (ENABLE_B, 180);
  digitalWrite (PIN_B1, HIGH);
  digitalWrite (PIN_B2, LOW);
}

void sensorCenter(){
  myservo.write(90);
  delay(500);
 }
 
void turnSensorLeft(){
  myservo.write(120);
  delay(500);
 }

void turnSensorRight(){
  myservo.write(60);
  delay(500);
 }
 
int leftSensorDistance(){
    int LEFT_SENSOR_DISTANCE;
    LEFT_SENSOR_DISTANCE=sensorDistance();
    return LEFT_SENSOR_DISTANCE;
  }

int rightSensorDistance(){
    int RIGHT_SENSOR_DISTANCE;
    RIGHT_SENSOR_DISTANCE=sensorDistance();
    return RIGHT_SENSOR_DISTANCE;
  }
    
int toTheLeft(){
    int LEFT_SENSOR_DISTANCE;
    turnSensorLeft();
    LEFT_SENSOR_DISTANCE=leftSensorDistance();
    Serial.print("Left sensor distance is: ");
    Serial.println(LEFT_SENSOR_DISTANCE);
    return LEFT_SENSOR_DISTANCE;
 }  

int toTheRight(){
    int RIGHT_SENSOR_DISTANCE;
    turnSensorRight();
    RIGHT_SENSOR_DISTANCE=rightSensorDistance();
    Serial.print("Right sensor distance is: ");
    Serial.println(RIGHT_SENSOR_DISTANCE);
    return RIGHT_SENSOR_DISTANCE;
 }  

And this is how the robot navigates autonomously in my kitchen:

Commerce Content is independent of articles and advertising, and if you buy something through our posts, I may get a small share of the sale.

8 comments » Write a comment

  1. Hello. Thank you for this. Can you tell me the component you used to move the sensor from side to side. I have found some online but they seem quite expensive?

Leave a Reply

Required fields are marked *.