Self balancing bot
I bought most of the parts to built a self balancing bot many months ago. This time I didn’t use cheap Chinese stuff. I actually bought Pololu parts from a local shop. Pololu has a nice range of motors and motor controllers. I wanted to keep the bot small so I used the micro metal gear motors with 2 matching 60mm wheels. I did go for the most powerful version of the micro metal gear motors. To control the motors I got a DRV8833 break out board.
Pololu motors and wheels
For the electronics I used one of my own PCB creations, a Bajduino of course 🙂 It’s just a small (50x50mm) break out board for an ATmega328. I’m running the ATmega @ 16MHz and 3.3V. It’s out of spec according to the datasheet but it works… I also needed an IMU of course. I found a MP6050 sensor in my parts box. The MPU6050 combines a 3 DOF gyro and 3 DOF accelerometer in a small package, ideal for a self balancing bot.
The chassis is made from a sheet of polystyrene, I made 3 small plates out of a big sheet. Polystyrene is very easy to cut up with a sharp knife. Very handy to quickly make a custom robot chassis. I then drilled holes in the plates to fit the motors and electronics. To connect the 3 chassis plates I used brass spacers. I mounted the MPU6050 sensor to the bottom of the lowest chassis plate.
Self balancing bot
I did have some trouble with the little motors. During my initial tests I noticed that the MPU6050 sensor would stop working when driving the motors. I hooked up my oscilloscope to the 3.3V that powers the sensor and saw that the regulator on my board was oscillating.
LM1117 oscillating
Brushed DC motors are noisy things so I soldered the biggest (0,22µF) ceramic capacitors I had on hand to each motor. This did the trick and the regulator now gives a nice stable 3.3V.
The hard part in building a self balancing bot is coding it. Luckily there are some smart people that have shared their code and ideas about self balancers. So I just borrowed a lot of code I found online and turned it in to something that makes my bot balance.
First thing I needed was code to get the data from the MPU6050 sensor. You need some kind of filter to get good results. There are different ways of doing this. The guys from TKJElectronics have shared the necessary code to get the data from the MPU6050 sensor. They have also made a library for a Kalman filter. Their code can be downloaded on Github.
Next step was coding a PID algorithm. I have no experience with this and tried lots of different things. Including using the Arduino PID library. I could never get decent results with it so I gave up pretty fast. I then got some nice PID code from someone on letsmakerobots (thanks Protowrxs). This code worked a lot better but I could not find the right PID parameters. I then had a good look at this excellent guide to make a self balancing bot and rewrote my sketch. The following video shows my latest code:
This is the sketch I’m using (you need the library from TKJElectronics to compile this code):
// http://www.bajdi.com
// Self balancing bot:
// µController = ATmega328
// 2 Pololu micro metal gear motors with 60mm wheels + DRV8833 motor controller
// 6DOF MPU6050 sensor
#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter
Kalman kalmanX; // Create the Kalman instance
/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
float accXangle;//, accYangle; // Angle calculate using the accelerometer
float gyroXangle;//, gyroYangle; // Angle calculate using the gyro
float kalAngleX;//, kalAngleY; // Calculate the angle using a Kalman filter
unsigned long timer;
uint8_t i2cData[14]; // Buffer for I2C data
float CurrentAngle;
// Motor controller pins
const int AIN1 = 3; // (pwm) pin 3 connected to pin AIN1
const int AIN2 = 9; // (pwm) pin 9 connected to pin AIN2
const int BIN1 = 10; // (pwm) pin 10 connected to pin BIN1
const int BIN2 = 11; // (pwm) pin 11 connected to pin BIN2
int speed;
// PID
const float Kp = 4;
const float Ki = 1;
const float Kd = 1;
float pTerm, iTerm, dTerm, integrated_error, last_error, error;
const float K = 1.9*1.12;
#define GUARD_GAIN 10.0
#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))
void setup() {
pinMode(AIN1, OUTPUT); // set pins to output
pinMode(AIN2, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
Serial.begin(57600);
Wire.begin();
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode
while(i2cRead(0x75,i2cData,1));
if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while(1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while(i2cRead(0x3B,i2cData,6));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
kalmanX.setAngle(accXangle); // Set starting angle
gyroXangle = accXangle;
timer = micros();
}
void loop() {
runEvery(25) // run code @ 40 Hz
{
dof();
if (CurrentAngle <= 180.2 && CurrentAngle >= 179.8)
{
stop();
}
else{
if (CurrentAngle < 230 && CurrentAngle > 130)
{
Pid();
Motors();
}
else
{
stop();
}
}
}
}
void Motors(){
if (speed > 0)
{
//forward
analogWrite(AIN1, speed);
analogWrite(AIN2, 0);
analogWrite(BIN1, speed);
analogWrite(BIN2, 0);
}
else
{
// backward
speed = map(speed,0,-255,0,255);
analogWrite(AIN1, 0);
analogWrite(AIN2, speed);
analogWrite(BIN1, 0);
analogWrite(BIN2, speed);
}
}
void stop()
{
analogWrite(AIN1, 0);
analogWrite(AIN2, 0);
analogWrite(BIN1, 0);
analogWrite(BIN2, 0);
}
void Pid(){
error = 180 - CurrentAngle; // 180 = level
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error);
last_error = error;
speed = constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}
void dof()
{
while(i2cRead(0x3B,i2cData,14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = ((i2cData[6] << 8) | i2cData[7]);
gyroX = ((i2cData[8] << 8) | i2cData[9]);
gyroY = ((i2cData[10] << 8) | i2cData[11]);
gyroZ = ((i2cData[12] << 8) | i2cData[13]);
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
double gyroXrate = (double)gyroX/131.0;
CurrentAngle = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000);
timer = micros();
}
I spent hours trying to find the right PID parameters but the bot still does not balance like it should… I tried lots of things, to no avail. I changed the height of the bot. Moved the battery from the top to the bottom… I’m giving up for the moment. I might take another shot at it at a later date.
I was wondering. Wouldn’t it be better to put you sensor, not at the tilt position but rather at the center pivot point? In your case, where the micro-controler is.
The problem is that by the time your robot detect that it’s upright, it’s momentum from the top of your robot, which include the weight of your micro-controller board, will already be on it’s way to “fall” to the opposite side. This will cause the robot to always over-correct itself and eventually fall over.
Well I first mounted the sensor next to the micro controller. But then you don’t get an exact angle reading of the accelerometer. I got a lot of comments that for best results the sensor should be mounted on the tilt axis. Most balancing bots I’ve seen use this setup. But at the moment I’m willing to try anything to get it to balance 😀
What engines did you use? What are the characteristics? What brand are they? They are equipped with reduction gear?
thanks
Drake
Have you actually read my post???
Vimeo says Sorry There was a problem loading this video 🙁
Try again… still works for me.
muy buen proyecto, me atrevo a decir que el problema es por las velocidades de las ruedas, de seguro son diferentes. seria bueno colocar un encoder a cada una y así de alguna manera mediante software comparar dichas velocidades y hacer que giren a la misma velocidad saludos desde Venezuela
very good project, I dare say that the problem is the wheel speeds, insurance are different. Good serious post each encoder and so somehow compare these speeds with software and spinning at the same speed Greetings from Venezuela
i have an large polystirene sheet (2metersX1,22meters)
i wanna know some questions:
what type of screws is indicated if i use an polystirene sheet?
where should i put the screws in order to get better perfomance?
(milimeter of the screw,torque etc)
is there some kind of way to open the hole for the screws?
something i need to know?
the drill that you used it has to be smaller or bigger than the hole for the screws?
i dont understand about carpenter/joiner so it is why i am asking you.
if you know some good tutorial or book about make chassi/case for robots with polystirene sheet i will really appreciate!
thanks!
Polystyrene is very easy to work with. You don’t need any special type of screws or tools. To mount the spacers (3mm thread) I used a 3mm drill. Have a look on letsmakerobots.com, there are a couple of robots on there made from polystyrene: http://letsmakerobots.com/node/37634
Tilt Robots works on the basis of an inverted pendulum. You will need most of your weight at the top of your robot. This will increase your momentum at the top and make your robot more stable.
Put your battery on-top and add more weight to it if needed.
You tilt sensor should be about the middle, at your center of balance.
Try balancing a plastic ruler, upright, on your finger. Try adding a lump of clay to the top end and try again. You’ll see that the ruler is much easier to balance if there are weight at the top.
I’m sure that will make your robot more stable.
Originally my bot was about 50% higher and had the battery on top. But I could not get it to balance. So I started making lots of code changes and also made it shorter. I will give it another try and put the battery back on top and put some more spacers between the 3 plates. My ultimate goal is to have it remotely controlled. Thanks for the comments 🙂
I’m having a problem to compile the code,
the error saying: ‘i2cWrite’ was not declared in this scope.
What can it be?
I copied your code exactly as in your post, but I can not make it work, I appreciate the help!
I tried again, and got a new error:
‘Kalman’ does not name a type
Do you have any tips of what can be?
Have you installed the Kalman library in the right place? https://github.com/TKJElectronics/KalmanFilter
Also do not expect this code to work… As you can see in the video the PID settings need more tuning to make it balance. I have recently ordered new motors from Pololu, with encoders. And I will change the mechanical design of the robot in the hope it will balance better.
I looked at your code and can make some suggestions:
1) You need to calibrate your sensors. IS everything really square, Does Y remain constant when X changes? If not the sensor is not aligned. You can fix it with some trig
2) I don’t see where you are correcting out a zero error. Are the sensors saying “zero” at zero or is there a bias.
3) If using integer math don’t convert to degrees. You have 15 bits use then ALL or at least more of them “hundredths of a degree” fits but better to make up your own units like “1/32nd counts” or whatever so you can do the scaling with a shift.
In any case one degree per count is so large a unit for integer math. One a larger computers I use double (64-bit floats) in radians for all angles.
4) you motor gearbox likely has “backlash” so when you reverse the direction the motor has to spin a little before the wheel turns. You might be able to see this if you make it go really slow. You may have to compensate in software.
Once you get the motor encoders then you PID sends wheel speed, not voltage and your wheel driver can ignore the first N counts after a direction change because they do’t move the wheel.
Yo are actually ahead of me. I’m waiting on on parts, so I’m writing hexapod code, not balance bot code.
Hi nice Work
i tried to get it work but always i got the error I2CWrite not declared in this scope
I got the right kalman library but i thing its a problem of the wire library or the arduino version i use 1.0.5
I want to use the code on a Real size Segway
Hope you can help with my Problem
Thx
nice project.
but i got always same error ” I2CWrite not declared in this scope”
i downloaded kalman library but now only have that error.
[…] http://www.bajdi.com/building-a-self-balancing-bot/ […]
why do you calculate the X acceleration angle from the accY and accZ values, should it not be accX and accZ?
your code: accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
I’m having a problem to compile the code,
the error saying: ‘i2cWrite’ was not declared in this scope.
What can it be?
I copied your code exactly as in your post, but I can not make it work, I appreciate the help!
[…] di controllo: PID (spiegazione QUI, QUI, QUI); LQR; Kalman filter per utilizzare accelerometro e giroscopio, si trova su github: […]
May i know the inputs from your sensors to the Arduino(Plzz specify the pin names on the arduino along with the pin on the mpu6050).Thank you
The mpu6050 uses the I2C bus to communicate with a micro controller. I2C pins (SDA/SCL) on an Arduino with an ATmega328 (Uno, Nano, …) are A4 and A5.
I’m having a problem to compile the code,
the error saying: ‘i2cRead’ was not declared in this scope.
What can it be?
I copied your code exactly as in your post, but I can not make it work, I appreciate the help!
Sagar Shah,
You will need to install the KalmanFilter library. In the examples for this library you will see a folder called MPU6050, inside you will see a file called I2C.ino. Copy this file into the folder where you have downloaded the code. Stop and restart the Arduino IDE and it should now compile.
just sharing for ‘i2cRead’ was not declared in this scope
in your active sketch (coppied sketch from admin) add file I2C.ino, you can find it as John Crawford said, add file by clicking sketch-add file-choose i2c.ino then you get new tab i2c.ino beside your sketch ,verify your your sketch than error will gone:)
Is it really necessary to use motors with encoders or can we use ordinary 12V dc motors WITHOUT ENCODERS?
I’ve seen balancing bots that work perfectly fine with and without encoders. Depends how much control you want.
hi, It is very good, Could you tell me What is Funtion constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN)??
Code is explained here: http://www.x-firm.com/?page_id=145
Hi, Thank you so much, please tell me PID parameters can depend too much on weight and height of the robot????,my english is not good. Thank you!!!
can you give circuit diagram
hello mate, thanks for sharing this tutorial, but I have to try and get an error
sketch_feb09a.ino: In function ‘void setup ()’:
sketch_feb09a.ino: 55: 38: error: ‘i2cWrite’ was not declared in this scope
sketch_feb09a.ino: 56: 32: error: ‘i2cWrite’ was not declared in this scope
sketch_feb09a.ino: 58: 33: error: ‘i2cRead’ was not declared in this scope
sketch_feb09a.ino: 67: 31: error: ‘i2cRead’ was not declared in this scope
sketch_feb09a.ino: In function ‘void dof ()’:
sketch_feb09a.ino: 140: 32: error: ‘i2cRead’ was not declared in this scope
Error compiling.
how do I fix it? and can you share the wiring diagram?
oh sorry, I have to fix the problem and succeed … and now I want to ask how to connect mpu6050 to arduino?
SCL and SDA pin on mpu6050 in arduino pin plug to the Which?
Hey how you fix it ?
I have a problem about ‘i2cRead’ was not declared in this scope when I complie this code. Please help me, I tried to solve but i can’t do it.
Hi QuanNB,
You could have solved your problem had you just read this forum. Please see my post of 17/09/2014 and that of edissty on 21/09/2014 above.
Regards
John
Do you have gears? Makes a big difference. I had a reduction gearbox but it wasn’t enough. You need about 30:1
hey, i m interesting your self-balancing bot, but i wanna question. would you tell me your self balancing bot’s components in detail?
Everything is just perfect but i couldnt find the library in the code.
could you pls share the link to the codes…especially the mpu6050 code
Jeff Rowberg’s I2CDev library is also needed. Google for ‘github’ jeff rowberg’s i2cdev’
I have been working on a self balancing robot (third attempt) and have added some debugging print statements inside the computation loop. Noticed that the angle computed by the kalman filter lags the raw angle from the accelerator by about 6 state changes. This results the the computation loop thinking that it is tilting one way when it has already passed the center point.
I understand that the kalman filter is not your code but wondered if anyone else has observed this behavior. I have played with the q_angle & q_bias. Making the 0.1 and 0.3 seems to decrease the lag at the expense of stability. I also changed the loop iteration time to 2 miliseconds.
Also wondered if using the embedded code in the 6050 would help.
Note – I am on a different CPU and did port the kalman filter code to C so there is a real probability that I am the culprit.
Hi
I could figure out why, but if you display all the values on the serial monitor, then the iTerm goes up to a value: -10 and i will not change. I changed the following in the program: “float pTerm, iTerm, dTerm, last_error, error; // deleted integrated_error” and in the part Void Pid(){ I put in the beginning ” integrated_error = 0; ” and this solved the problem for me, I have now changing values for the iTerm. I don’t know if this is the right thing to do, but can you check on this, or maybe I am wrong.
Thanks for putting your time in this project.
Thank you for letting me know. I should have another look at this project and make a new attempt to make it balance 🙂
Hello
Can it balance now ?
hello i have interfaced mpu6050 with atmega32.it is successfully reading the mpu6050.but when i read the value of accelerometer and gyro it gives zero.Even i wake it from sleep mode.still it is giving zero
so can u tell me what can be the problem .and give me appropriate solution
Sorry my crystal ball is broken.
Hi, I just watched the tutorial from EEEnthusiast (Arduino Accelerometer MPU-6050 Tutorial) and was wondering if you could help me with it. I’m working on a quadcopter and wanted to use the MPU to balance the unit. Do I need to use both accelerometer and gyroscope data to do that? Do you have an example I could use for it?
Thanks,
John
Hello! What RPM is your engine?
Hello !
My bot is working only backward direction
it doesn’t working forward side
please help me
Arduino: 1.6.1 (Windows 8.1), Board: “Arduino NG or older, ATmega8”
sketch_apr17a.ino: In function ‘void setup()’:
sketch_apr17a.ino:55:38: error: ‘i2cWrite’ was not declared in this scope
sketch_apr17a.ino:56:32: error: ‘i2cWrite’ was not declared in this scope
sketch_apr17a.ino:58:33: error: ‘i2cRead’ was not declared in this scope
sketch_apr17a.ino:67:31: error: ‘i2cRead’ was not declared in this scope
sketch_apr17a.ino: In function ‘void dof()’:
sketch_apr17a.ino:140:32: error: ‘i2cRead’ was not declared in this scope
Error compiling.
This report would have more information with
“Show verbose output during compilation”
enabled in File > Preferences.
Saudações do Brasil
Constrain() is the reason why you had to give up. Use map() instead and see the magic!!
can you please show the circuit diagram or show the connections of MPU6050 with Arduino.