Avirup Basu
Published © GPL3+

Autonomous Navigation and 2D Mapping

Perform limited autonomous navigation and plot a 2D map of the environment.

IntermediateWork in progress2 days31,603
Autonomous Navigation and 2D Mapping

Things used in this project

Hardware components

Arduino Leonardo
Arduino Leonardo
×1
DC motor (generic)
×4
Dual DC motor driver 20A
×1
Wheel
×4
HCSR04
×3
Seeed Digital Compass Grove
×1
Jumper wires (generic)
Jumper wires (generic)
×1
Chassis
×1

Software apps and online services

Visual Studio 2015
Microsoft Visual Studio 2015

Story

Read more

Schematics

Arduino code for the bot

Connections

A word file is included. The connections are specifically mentioned in the file.

Code

Arduino code for the bot

C/C++
Since, its an on-going project, so from the code it may seem that it has lots of loose ends. Please note that you can download the code and use it according to your wishes.
#include <Wire.h>
#include <HMC5883L.h>
#include <NewPing.h>

NewPing sonar1(A0, A1, 100);
NewPing sonar2(A2, A3, 100);
NewPing sonar3(A4, A5, 100);

HMC5883L compass;


//Motor pins
int d1=8; 
int  val2=82,val=90;
int d2=9;
int brake1=4;
int brake2=12;
int op1=5;
int op2=7;
int pwm1=6;
int pwm2=11;
int td=35;

int error = 0;
String s="",s1,s2,s3;
//Final heading hard coded
/* 
 *  0   -> N
 *  90  -> E
 *  180 -> S
 *  270 -> W
 */
int fin_heading=0, curr_heading;
int m11=3,m12=4,m21=5,m22=6;
//Global variables
int t=2,e=7; //HCSR04 control pins
int d,dt; //Distance calculated
//int servoPin=9;
int trans[1000], count =0;
int dis1,dis2,dis3;
//Distance acquire
void distance()
{
  dis1=sonar1.ping_cm();
  dis2=sonar2.ping_cm();
  dis3=sonar3.ping_cm();
  
}
void heading()
{
  MagnetometerRaw raw = compass.readRawAxis();
  MagnetometerScaled scaled = compass.readScaledAxis();
  int MilliGauss_OnThe_XAxis = scaled.XAxis;
  float heading = atan2(scaled.YAxis, scaled.XAxis);
  float declinationAngle = -0.0457;
  heading += declinationAngle;
  if(heading < 0)
    heading += 2*PI;
  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
    heading -= 2*PI;
  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180/M_PI;
  curr_heading=(int)headingDegrees;
  delay(66); 
}

void setup()
{
  Serial.begin(9600);
  pinMode(d1,OUTPUT);
  pinMode(d2,OUTPUT);
  pinMode(brake1,OUTPUT);
  pinMode(brake2,OUTPUT);
  pinMode(op1,OUTPUT);
  pinMode(op2,OUTPUT);
  pinMode(pwm1,OUTPUT);
  pinMode(pwm2,OUTPUT);
  Wire.begin();
  error = compass.setScale(1.3); 
  error = compass.setMeasurementMode(MEASUREMENT_CONTINUOUS); 
}

void loop() 
{
  String sc="+";
  heading();
  distance();
  s1=String(dis1);
  s2=String(dis2);
  s3=String(dis3);
  String head=String((int)curr_heading);
  //String head="360";
  s=sc+s1+sc+s2+sc+s3+sc+head+"arg";
  Serial.print(s);
  Serial.println();
  //Algorithm
  char cd='f';
  cd= dist_empty();
  switch(cd)
  {
    
    case 'l':
    left();
    delay(420);
    stoop();
    delay(100);
    break;
    case 'r': right();
    delay(420);
    stoop();
    delay(100);
    break;
    case 'f': forward();
    delay(150);
    stoop();
    delay(100);
    break;
    default: stoop();
  }
  Serial.println(cd);
  delay(100);
}
char dist_empty()
{
  //Follow RHR
  td=18;
  if(dis1<td&&dis1!=0)
  {
    if(dis2>td||dis2==0)
    return 'l';
    else if(dis3>td||dis3==0)
    return 'r';
    else
    return 's';
  }
  else
  {
    /*if(dis3<=10)
    return 'l';//a=l
    else if(dis2<=10)
    return 'r';//b=r
    else*/
    return 'f';
  }
}
void left()
{
  digitalWrite(brake2,LOW);
      analogWrite(pwm2,val);
      digitalWrite(d1,HIGH);
      analogWrite(pwm1,val);
      digitalWrite(d2,LOW);
      digitalWrite(brake1,LOW);
      digitalWrite(op1,LOW);
      digitalWrite(op2,LOW);
}
void right()
{
 digitalWrite(brake2,LOW);
      analogWrite(pwm2,val);
      digitalWrite(d1,LOW);
      analogWrite(pwm1,val);
      digitalWrite(d2,HIGH);
      digitalWrite(brake1,LOW);
      digitalWrite(op1,LOW);
      digitalWrite(op2,LOW);
}
void forward()
{
  digitalWrite(d1,LOW);
      analogWrite(pwm1,val);
      digitalWrite(brake2,LOW);
      analogWrite(pwm2,val2);
      digitalWrite(d2,LOW);
      digitalWrite(brake1,LOW);
      digitalWrite(op1,LOW);
      digitalWrite(op2,LOW);  
}
void stoop()
{
   digitalWrite(brake2,HIGH);
      digitalWrite(d1,LOW);
      digitalWrite(d2,LOW);
      digitalWrite(brake1,HIGH);
      digitalWrite(op1,LOW);
      digitalWrite(op2,LOW);
      analogWrite(pwm1, 0);
      analogWrite(pwm2, 0);
}

WPF app file

Credits

Avirup Basu

Avirup Basu

2 projects • 10 followers

Comments

Add projectSign up / Login