Self Path Correcting Robot

Hello everyone. Welcome to a new project. Have you ever thought of building a robot that can self correct itself ? Well, today we will make one. The star of this project is....again MPU 6050 gyroscope module. Now the reason I make so many projects using this sensor is because there are plenty of applications where this sensor can be used and I love things that have this capability. I have few more projects in mind like a Balancing robot, PID controller, Diy Gimbal etc. So if you want to see any of these projects do leave a comment below. Also make sure you follow me. Now let's begin this project.

If you follow my projects then I assume you have a good idea how the gyroscope sensor. If not then you can google a bit about them and you will find plenty of stuff about it. In our previous projects we used to place the horizontally but here we will place is vertically. The basic idea in this project that that we will get the degrees output from the sensor and use it to monitor the robot. If the sensor deviates from a straight path then the sensor will send values and auto correct its path. Let's start building this project.

I strongly suggest you to buy the components from UTSource.net because they provide high quality products and ship them on time. They also provide high quality PCB Service at affordable rates. So do check them out.

1. Arduino Uno Board

2. MPU-6050 Gyroscope Sensor

3. L293d Motor Driver Module

4. Motor Chassis with wheels and motors

5. Breadboard

6. Connecting wires

Complete Arduino Learning Kit

Connect all the components as shown in the circuit diagram. Make sure you the mount the sensor vertically on the breadboard. Power the Arduino Board and the Motor Driver Module using 9 V batteries. You can use any other power source of your choice.

Now upload the sketch to your Arduino board.

#include <wire.h><br>//Motor A
const int motorPin1  = 5;  
const int motorPin2  = 6;  
//Motor B
const int motorPin3  = 10; 
const int motorPin4  = 9;  
//Declaring some global variables
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;
long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
float angle_pitch_output, angle_roll_output;
long loop_timer;
int temp;
void setup() 
{
  Wire.begin();                                                        
  setup_mpu_6050_registers();                                          
  for (int cal_int = 0; cal_int < 1000 ; cal_int ++)
  {                  
    read_mpu_6050_data();                                             
    gyro_x_cal += gyro_x;                                              
    gyro_y_cal += gyro_y;                                              
    gyro_z_cal += gyro_z;                                             
    delay(3);                                                          
  }
pinMode(motorPin1, OUTPUT); 
pinMode(motorPin2, OUTPUT);
pinMode(motorPin3, OUTPUT);
pinMode(motorPin4, OUTPUT);
  // divide by 1000 to get avarage offset
  gyro_x_cal /= 1000;                                                 
  gyro_y_cal /= 1000;                                                 
  gyro_z_cal /= 1000;                                                 
  Serial.begin(9600);
  loop_timer = micros();                                            
}
void loop()
{
 read_mpu_6050_data();   
 //Subtract the offset values from the raw gyro values
  gyro_x -= gyro_x_cal;                                                
  gyro_y -= gyro_y_cal;                                                
  gyro_z -= gyro_z_cal;                                                
//Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
  angle_pitch += gyro_x * 0.0000611;                                   
  angle_roll += gyro_y * 0.0000611;                                    
  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);               
  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               
  //Accelerometer angle calculations
  acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));  
  //57.296 = 1 / (3.142 / 180) The Arduino sin function is in radians
  angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296;      
  angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;     
  angle_pitch_acc -= 0.0;                                              
  angle_roll_acc -= 0.0;                                               
if(set_gyro_angles){                                               
    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;     
    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;        
  }
  else{                                                               
    angle_pitch = angle_pitch_acc;                                    
    angle_roll = angle_roll_acc;                                       
    set_gyro_angles = true;                                           
  }
  //To dampen the pitch and roll angles a complementary filter is used
  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;  
  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;      
  Serial.print(" | Angle  = "); Serial.println(angle_pitch_output);
while(micros() - loop_timer < 4000);                                  
 loop_timer = micros();//Reset the loop timer
if(angle_pitch_output<=2 && angle_pitch_output>=-2)
  {
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, HIGH);//FWD
    digitalWrite(motorPin3, HIGH);
    digitalWrite(motorPin4, LOW);
  }
  else if(angle_pitch_output>2)
  {
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, HIGH);//LFT
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, HIGH);
  }
   else if(angle_pitch_output<-2)
  {
   digitalWrite(motorPin1, HIGH);
    digitalWrite(motorPin2, LOW);//RT
    digitalWrite(motorPin3, HIGH);
    digitalWrite(motorPin4, LOW);
  }
  else
  {
    digitalWrite(motorPin1, LOW);
    digitalWrite(motorPin2, LOW);//STOP
    digitalWrite(motorPin3, LOW);
    digitalWrite(motorPin4, LOW);
  }
}
void setup_mpu_6050_registers()
{
  //Activate the MPU-6050
  Wire.beginTransmission(0x68);                                       
  Wire.write(0x6B);                                                   
  Wire.write(0x00);                                                    
  Wire.endTransmission();                                             
  //Configure the accelerometer (+/-8g)
  Wire.beginTransmission(0x68);                                       
  Wire.write(0x1C);                                                    
  Wire.write(0x10);                                                    
  Wire.endTransmission();                                             
  //Configure the gyro (500dps full scale)
  Wire.beginTransmission(0x68);                                        
  Wire.write(0x1B);                                              
  Wire.write(0x08);                                                    
  Wire.endTransmission();                                             
}
void read_mpu_6050_data()
{                                             
  Wire.beginTransmission(0x68);                                        
  Wire.write(0x3B);                                                    
  Wire.endTransmission();                                             
  Wire.requestFrom(0x68,14);                                         
  while(Wire.available() < 14);                                        
  acc_x = Wire.read()<<8|Wire.read();                                  
  acc_y = Wire.read()<<8|Wire.read();                                  
  acc_z = Wire.read()<<8|Wire.read();                                  
  temp = Wire.read()<<8|Wire.read();                                   
  gyro_x = Wire.read()<<8|Wire.read();                                 
  gyro_y = Wire.read()<<8|Wire.read();                                 
  gyro_z = Wire.read()<<8|Wire.read();                                 
}</wire.h>

You can copy it from above or download the .ino file.

Now connect the batteries. Your robot will start moving in a straight line. But if you push it tries to self correct itself.

I have attached a small video above to show its working.

If you like Arduino based projects do follow me. If you have any queries regarding this project, drop a comment below. If you like my simple diy projects then please share them with your friends. That's it for today guys. Will be back with a new project soon.