2 Wheeled Self Balancing Robot

2,281

10

14

In my humble opinion you are not a real Maker, unless you don't build your own 2 wheeled self balacing robot. :-)

So, here it is... and, most important thing, it works!!!

This project looks very simple. Instead, it requires a good level of knowledge of physics (inverse pendulum), maths (Kalman Filter) and mechanics (PID).

The are people who chose this project as a dissertation to obtain the Degree in engineering, therefore do not underestimate it. Once everything is assembled, you must do a bit of tuning learning about how the PID (Proportional, Integral and Derivative) control works.

I've assembled this bot in 3 different configurations, before achieving a good stability with the robot untethered.

When I saw on Internet the robot carrying the glass of water, I was immediately fascinated and I've decided to give it a go.

WARNING!!!

Please be aware that Lipo Battery are dangerous.

If you attempt this "stunt" waterproof all the electronics using some CorrosionX.

If you spill the water on the robot, you will destroy it, not mentioning the fact the short circuit can ignite the Lipo Battery.
For the frame I used some Oxford look-alike-Lego-parts.

Teacher Notes

Teachers! Did you use this instructable in your classroom?
Add a Teacher Note to share how you incorporated it into your lesson.

Step 2: Build the Frame Using Some Lego (compatible) Parts

This is a very easy task. I believe that if Lego would start to make kits (like this), mixing bricks with electronics up-to-date, they will do much better in terms of sales (they are currently doing).

Anyway, please note that I moved the Lego platform for the Arduino Uno to a higher position to improve the inverse pendulum effect.

To attach the motors, make 4 holes in the base, passing a zip tie (for each motor) in it.

I've added a bit of glue to be sure the motors don't move.

Step 3: Wiring Diagram & Coding

Following the schematic above, wire to the Arduino Uno clone, the motor shield L298N, the MPU-6050 and the battery.

Regarding the coding, you can easily find on Internet the code for this project, that thanks to Kalman Filter and to the PID (Proportional, Integral and Derivative) control, improves the stability of your bot.

If you don't find it, please subscribe to my Youtube channel and I'll send to you straight away.

Step 4: Enjoy It!

Congratulations, you have built your 2 wheeled self balancing robot!

Participated in the
Microcontroller Contest

Recommendations

• Internet of Things Class

22,849 Enrolled

14 Discussions

The reminder for the Youtube subscription is for those are looking for this code and "land" on this Instructables page. :-)

Here the code comes...

Install the MPU6050 library

#include <Wire.h>

#include <I2Cdev.h>

#include <MPU6050.h>

MPU6050 accelgyro;

#define runEvery(t) for (static long _lasttime;\

(uint16_t)((uint16_t)millis() – _lasttime) >= (t);\

_lasttime += (t))

int16_t ax, ay, az;

float accBiasX, accBiasY, accBiasZ;

float accAngleX, accAngleY;

double accPitch, accRoll;

int16_t gx, gy, gz;

float gyroBiasX, gyroBiasY, gyroBiasZ;

float gyroRateX, gyroRateY, gyroRateZ;

float gyroBias_oldX, gyroBias_oldY, gyroBias_oldZ;

float gyroPitch = 180;

float gyroRoll = -180;

float gyroYaw = 0;

uint32_t timer;

// input

double InputPitch, InputRoll;

// initial values

double InitialRoll;

// Motors

int enablea = 11;

int enableb = 10;

int a1 = 5;

int a2 = 3;

int b1 = 7;

int b2 = 6;

void setup() {

Wire.begin();

Serial.begin(9600);

Serial.println(“Initializing I2C devices…”);

accelgyro.initialize();

// verify connection

Serial.println(“Testing device connections…”);

Serial.println(accelgyro.testConnection() ? “MPU6050 connection successful” : “MPU6050 connection failed”);

delay(1500);

// Motor

pinMode(enablea, OUTPUT);

pinMode(enableb, OUTPUT);

pinMode(a1, OUTPUT);

pinMode(a2, OUTPUT);

pinMode(b1, OUTPUT);

pinMode(b2, OUTPUT);

digitalWrite(a1, HIGH);

digitalWrite(a2, HIGH);

digitalWrite(b1, HIGH);

digitalWrite(b2, HIGH);

//TODO: Better calibration

accelgyro.setXAccelOffset(-721);

accelgyro.setYAccelOffset(-3352);

accelgyro.setZAccelOffset(1526);

accelgyro.setXGyroOffset(52);

accelgyro.setYGyroOffset(-26);

accelgyro.setZGyroOffset(0);

gyroBiasX = 0;

gyroBiasY = 0;

gyroBiasZ = 0;

accBiasX = 4;

accBiasY = -4;

accBiasZ = 16378;

//Get Starting Pitch and Roll

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

accPitch = (atan2(-ax, -az) + PI) * RAD_TO_DEG;

accRoll = (atan2(ay, -az) + PI) * RAD_TO_DEG;

if (accPitch <= 360 & accPitch >= 180) {

accPitch = accPitch – 360;

}

if (accRoll <= 360 & accRoll >= 180) {

accRoll = accRoll – 360;

}

gyroPitch = accPitch;

gyroRoll = accRoll;

timer = micros();

delay(1000);

initializeValues();

}

double Setpoint;

void MotorControl(double out) {

if (out > 0) {

digitalWrite(a1, LOW);

digitalWrite(a2, HIGH);

digitalWrite(b1, HIGH);

digitalWrite(b2, LOW);

} else {

digitalWrite(a1, HIGH);

digitalWrite(a2, LOW);

digitalWrite(b1, LOW);

digitalWrite(b2, HIGH);

}

byte vel = abs(out);

if (vel < 0)

vel = 0;

if (vel > 255)

vel = 255;

analogWrite(enablea, vel);

analogWrite(enableb, vel);

}

void initializeValues() {

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

//////////////////////

// Accelerometer //

//////////////////////

accPitch = (atan2(-ax/182.0, -az/182.0) + PI) * RAD_TO_DEG;

accRoll = (atan2(ay/182.0, -az/182.0) + PI) * RAD_TO_DEG;

if (accRoll <= 360 & accRoll >= 180) {

accRoll = accRoll – 360;

}

//////////////////////

// GYRO //

//////////////////////

gyroRateX = ((int)gx – gyroBiasX) * 131;

gyroPitch += gyroRateY * ((double)(micros() – timer) / 1000000);

timer = micros();

InitialRoll = accRoll;

Setpoint = InitialRoll;

}

double filtered = 0;

void loop() {

runEvery(10) {

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

//////////////////////

// Accelerometer //

//////////////////////

accRoll = (atan2(ay/182.0, -az/182.0) + PI) * RAD_TO_DEG;

if (accRoll <= 360 & accRoll >= 180) {

accRoll = accRoll – 360;

}

//////////////////////

// GYRO //

//////////////////////

gyroRateX = -((int)gx – gyroBiasX) / 131;

double gyroVal = gyroRateX * ((double)(micros() – timer) / 1000000);

timer = micros();

//Complementary filter

filtered = 0.98 * (filtered + gyroVal) + 0.02 * (accRoll);

MotorControl(Compute(filtered – InitialRoll));

}

}

int outMax = 255;

int outMin = -255;

float lastInput = 0;

double ITerm = 0;

double kp = 100; //100

double ki = 8; //10

double kd = 2; //2

double Compute(double input)

{

double error = Setpoint – input;

ITerm += (ki * error);

if (ITerm > outMax) ITerm = outMax;

else if (ITerm < outMin) ITerm = outMin;

double dInput = (input – lastInput);

/*Compute PID Output*/

double output = kp * error + ITerm + kd * dInput;

if (output > outMax) output = outMax;

else if (output < outMin) output = outMin;

/*Remember some variables for next time*/

lastInput = input;

return output;

}

Remember to calibrate the MPU-6050 to calculate the offsets (accellerometer & gyro), using this code written by Jeff Rowberg.

http://42bots.com/tutorials/arduino-script-for-mpu-6050-auto-calibration/

Once you these data you can put them in the code written for the self balancing bot.