miniMe™ – DIY mini Robot Platform – Design Concepts

miniMe™ is 3D printing DIY mini robot platform

  • Balancing Robot
  • Rover
  • Line Tracer
  • Etc…

miniMe-BB: TT motor type

miniMe-BB: N20 motor type

miniMe-BB: SG90 Servo Motor type

miniMe-Rover: Servo motor type (Standard Servo)

miniMe-Rover: TT motor type

eX-Robot – ProfileBlock™ Robot Platform

[ Upload 20170424]
[ Upload design concept 20170418]

ProfileBlock – Balancing Robot – DIY Robots Platform

3D Design Tool: SketchUp Pro

ProfileBlock’s robots are built on top of an open source Arduino-based(with ESP8266, Raspberry Pi) platform.

Self Balancing Robot (eX-Robot, B-Robot, Roverbot, …


Base Plate(Parts):

6 x ProfileBlock™ DF 2020 85mmAcrylic Plate (t = 3mm)1 x Top Plate1 x Bottom Plate
2 x Step Motor Mount Plate
1 x Front Plate
1 x Rear Plate3D Printing Parts
2 x Servo Mount ProfoleBlock
12 x Knob M5 Hexaheard
2 x Wheels 100mm or Inline Wheels 70mm(with 2 x Inline Wheels Hub)
2 x Tire Mudguards
1 x Stand1 x Battery Holder

Step Motor:

2 x Nema 17 Stepper Motor bipolar 4 leads 34mm 12V 1.3A 26Ncm( 3D printer motor 42SHD0001
2 x NEMA 17 – Phase: 4, Step Angle: 1.8 Deg/Step, Holding Torque:


1 x Standard Servo or SG90 Metal Servo
1 x Standard Servo (Option) or SG90 Metal Servo

12 x M5 10mm Nylon Bolts
12 x M5 8mm Nylon Bolts
12 x M5 Nylon Nuts
8 x M3 8mm Bolts
4 x M3 10mm Bolts
4 x M3 15mm Bolts
8 x M3 Nuts


Control Board:

1 x ESP8266 Witty Cloud or WeMos (eX-Robot)

1 x HC-SR04 ultra sonic module
1 X MPU6050
2 x A4988 Step Motor Drive
1 x LM1117-5.0 5,0V 1A Regulator
1 x LM1117-3,3 3,3V 1A Regulator
6 x 100uF 25V Capacitor
4 x 0.1uF Capacitor
1 x 220KOhm Resistor
1 x 100KOhm Resistor
4 x 10KOhm Resistor
7 x 8P Female Pin Header Connector 2.54mm Pitch
3 x 4P Male Pin Header Connector 2.54mm Pitch
2 x 3P Male Pin Header Connector 2.54mm Pitch
3 x 2P Male Pin Header Connector 2.54mm Pitch
1 x 2EDGK 5.08mm 2P Plug-in terminal connectors set
1 x Right Angle SPDT 4 Pin On-On I/O Boat Rocker Switch
1 x Interface Mother boardPower Requirements:8.4VDC
12 VDCBattery:2 x 18650 Litum Ion Battery = 7.2 VDC ~ 8.4 VDCSoftware:

ESP8266 WeMos D1 mini code: Coming soon…
WiFi UDP Control TouchOSC Layout file: Coming soon…

Arduino IDE (ESP8266 ESP-12E/F)Support SoftAP and StationSupport OTA (at Local network)Support mDNS (at Local network)

The open source ProfileBlock hardware and software is free and made with love. Please show your level of support with a voluntary donation.


Arduino Self-Balancing Robot

Hello, everyone!

In this instructable, I’ll show you how to build a small self-balancing robot that can move around avoiding obstacles. This is a tiny robot measuring 4 inches wide and 4 inches tall and is based on the Arduino Pro Mini development board and the MPU6050 accelerometer-gyroscope module.

In the steps that follow, we will see how to interface the MPU6050 with Arduino, how to measure the angle of inclination of the robot, how to use PID to make the robot stay balanced. An ultrasonic rangefinder is also added to the robot which prevents it from banging into obstacles as it wanders around.

Parts List

I bought most of these parts from aliexpress but you can find them in any other electronics store as well.

1. Arduino Pro Mini

2. GY-521 module with MPU-6050

3. DRV8833 Pololu motor driver

4. 2, 5V boost converter

5. US-020 ultrasonic distance sensor

6. NCR18650 battery and holder

7. Pair of micro metal gear motors (N20, 6V, 200 rpm) and brackets

8. Pair of 42x19mm wheels

9. 3, Double-sided prototype PCB (4cm x 6cm)

10. 8, 25cm Nylon spacers and 4, nylon nuts

Apart from the above, you will need some cables, berg connectors and one on/off switch.

Step 1: A Bit of Theory

Let’s start with some fundamentals before getting our hands dirty.

The self-balancing robot is similar to an upside down pendulum. Unlike a normal pendulum which keeps on swinging once given a nudge, this inverted pendulum cannot stay balanced on its own. It will simply fall over. Then how do we balance it? Consider balancing a broomstick on our index finger which is a classic example of balancing an inverted pendulum. We move our finger in the direction in which the stick is falling. Similar is the case with a self-balancing robot, only that the robot will fall either forward or backward. Just like how we balance a stick on our finger, we balance the robot by driving its wheels in the direction in which it is falling. What we are trying to do here is to keep the center of gravity of the robot exactly above the pivot point.

To drive the motors we need some information on the state of the robot. We need to know the direction in which the robot is falling, how much the robot has tilted and the speed with which it is falling. All these information can be deduced from the readings obtained from MPU6050. We combine all these inputs and generate a signal which drives the motors and keeps the robot balanced.

Step 2: Let’s Start Building

We will first complete the circuitry and structure of the robot. The robot is built on three layers of perfboards that are spaced 25mm apart using nylon spacers. The bottom layer contains the two motors and the motor driver. The middle layer has the controller, the IMU, and the 5V boost regulator modules. The top most layer has the battery, an on/off switch and the ultrasonic distance sensor (we will install this towards the end once we get the robot to balance).

Before we begin to prototype on a perfboard we should have a clear picture about where each part should be placed. To make prototyping easy, it is always better to draw the physical layout of all the components and use this as a reference to place the components and route the jumpers on the perfboard. Once all the parts are placed and soldered, interconnect the three boards using nylon spacers.

You might have noticed that I’ve used two separate voltage regulator modules for driving the motors and the controller even though they both require a 5V source. This is very important. In my first design, I used a single 5V boost regulator to power up the controller as well as the motors. When I switched on the robot, the program freezes intermittently. This was due to the noise generated from the motor circuit acting upon the controller and the IMU. This was effectively eliminated by separating the voltage regulator to the controller and the motor and adding a 10uF capacitor at the motor power supply terminals.

Step 3: Measuring Angle of Inclination Using Accelerometer

The MPU6050 has a 3-axis accelerometer and a 3-axis gyroscope. The accelerometer measures acceleration along the three axes and the gyroscope measures angular rate about the three axes. To measure the angle of inclination of the robot we need acceleration values along y and z-axes. The atan2(y,z)function gives the angle in radians between the positive z-axis of a plane and the point given by the coordinates (z,y) on that plane, with positive sign for counter-clockwise angles (right half-plane, y > 0), and negative sign for clockwise angles (left half-plane, y < 0). We use this library written by Jeff Rowberg to read the data from MPU6050. Upload the code given below and see how the angle of inclination varies.

#include “Wire.h”
#include “I2Cdev.h” #include “MPU6050.h” #include “math.h”

MPU6050 mpu;

int16_t accY, accZ; float accAngle;

void setup() { mpu.initialize(); Serial.begin(9600); }

void loop() { accZ = mpu.getAccelerationZ(); accY = mpu.getAccelerationY(); accAngle = atan2(accY, accZ)*RAD_TO_DEG; if(isnan(accAngle)); else Serial.println(accAngle); }

Try moving the robot forward and backward while keeping it tilted at some fixed angle. You will observe that the angle shown in your serial monitor suddenly changes. This is due to the horizontal component of acceleration interfering with the acceleration values of y and z-axes.

Step 4: Measuring Angle of Inclination Using Gyroscope

The 3-axis gyroscope of MPU6050 measures angular rate (rotational velocity) along the three axes. For our self-balancing robot, the angular velocity along the x-axis alone is sufficient to measure the rate of fall of the robot.

In the code given below, we read the gyro value about the x-axis, convert it to degrees per second and then multiply it with the loop time to obtain the change in angle. We add this to the previous angle to obtain the current angle.

#include “Wire.h”
#include “I2Cdev.h” #include “MPU6050.h”

MPU6050 mpu;

int16_t gyroX, gyroRate; float gyroAngle=0; unsigned long currTime, prevTime=0, loopTime;

void setup() { mpu.initialize(); Serial.begin(9600); }

void loop() { currTime = millis(); loopTime = currTime – prevTime; prevTime = currTime; gyroX = mpu.getRotationX(); gyroRate = map(gyroX, -32768, 32767, -250, 250); gyroAngle = gyroAngle + (float)gyroRate*loopTime/1000; Serial.println(gyroAngle); }

The position of the MPU6050 when the program starts running is the zero inclination point. The angle of inclination will be measured with respect to this point.

Keep the robot steady at a fixed angle and you will observe that the angle will gradually increase or decrease. It won’t stay steady. This is due to the drift which is inherent to the gyroscope.

In the code given above, loop time is calculated using the millis() function which is built into the Arduino IDE. In later steps, we will be using timer interrupts to create precise sampling intervals. This sampling period will also be used in generating the output using a PID controller.

Step 5: Combining the Results With a Complementary Filter

Google defines complementary as “combining in such a way as to enhance or emphasize the qualities of each other or another”.

We have two measurements of the angle from two different sources. The measurement from accelerometer gets affected by sudden horizontal movements and the measurement from gyroscope gradually drifts away from actual value. In other words, the accelerometer reading gets affected by short duration signals and the gyroscope reading by long duration signals. These readings are, in a way, complementary to each other. Combine them both using a Complementary Filter and we get a stable, accurate measurement of the angle. The complementary filter is essentially a high pass filter acting on the gyroscope and a low pass filter acting on the accelerometer to filter out the drift and noise from the measurement.

 currentAngle = 0.9934 * (previousAngle + gyroAngle) + 0.0066 * (accAngle)

0.9934 and 0.0066 are filter coefficients for a filter time constant of 0.75s. The low pass filter allows any signal longer than this duration to pass through it and the high pass filter allows any signal shorter than this duration to pass through. The response of the filter can be tweaked by picking the correct time constant. Lowering the time constant will allow more horizontal acceleration to pass through.

Eliminating accelerometer and gyroscope offset errors
Download and run the code given in this page to calibrate the MPU6050’s offsets. Any error due to offset can be eliminated by defining the offset values in the setup() routine as shown below.

mpu.setZAccelOffset(963); mpu.setXGyroOffset(40);

Step 6: PID Control for Generating Output

PID stands for Proportional, Integral, and Derivative. Each of these terms provides a unique response to our self-balancing robot.

The proportional term, as its name suggests, generates a response that is proportional to the error. For our system, the error is the angle of inclination of the robot.

The integral term generates a response based on the accumulated error. This is essentially the sum of all the errors multiplied by the sampling period. This is a response based on the behavior of the system in past.

The derivative term is proportional to the derivative of the error. This is the difference between the current error and the previous error divided by the sampling period. This acts as a predictive term that responds to how the robot might behave in the next sampling loop.

Multiplying each of these terms by their corresponding constants (i.e, Kp, Ki and Kd) and summing the result, we generate the output which is then sent as command to drive the motor.

Step 7: Tuning the PID Constants

1. Set Ki and Kd to zero and gradually increase Kp so that the robot starts to oscillate about the zero position.

2. Increase Ki so that the response of the robot is faster when it is out of balance. Ki should be large enough so that the angle of inclination does not increase. The robot should come back to zero position if it is inclined.

3. Increase Kd so as to reduce the oscillations. The overshoots should also be reduced by now.

4. Repeat the above steps by fine tuning each parameter to achieve the best result.

Step 8: Adding the Distance Sensor

The ultrasonic distance sensor that I’ve used is the US-020. It has four pins namely Vcc, Trig, Echo, and Gnd. It is powered by a 5V source. The trigger and echo pins are respectively connected to digital pins 9 and 8 of Arduino. We will be using the NewPing library to get the distance value from the sensor. We will read the distance once every 100 milliseconds and if the value is between 0 and 20cm, we will command the robot to perform a rotation. This should be sufficient to steer the robot away from the obstacle.

Step 9: The Complete Code

#include "Wire.h"

#include “I2Cdev.h” #include “MPU6050.h” #include “math.h” #include <NewPing.h>

#define leftMotorPWMPin 6 #define leftMotorDirPin 7 #define rightMotorPWMPin 5 #define rightMotorDirPin 4

#define TRIGGER_PIN 9 #define ECHO_PIN 8 #define MAX_DISTANCE 75

#define Kp 40 #define Kd 0.05 #define Ki 40 #define sampleTime 0.005 #define targetAngle -2.5


int16_t accY, accZ, gyroX; volatile int motorPower, gyroRate; volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0; volatile byte count=0; int distanceCm;

void setMotors(int leftMotorSpeed, int rightMotorSpeed) { if(leftMotorSpeed >= 0) { analogWrite(leftMotorPWMPin, leftMotorSpeed); digitalWrite(leftMotorDirPin, LOW); } else { analogWrite(leftMotorPWMPin, 255 + leftMotorSpeed); digitalWrite(leftMotorDirPin, HIGH); } if(rightMotorSpeed >= 0) { analogWrite(rightMotorPWMPin, rightMotorSpeed); digitalWrite(rightMotorDirPin, LOW); } else { analogWrite(rightMotorPWMPin, 255 + rightMotorSpeed); digitalWrite(rightMotorDirPin, HIGH); } }

void init_PID() { // initialize Timer1 cli(); // disable global interrupts TCCR1A = 0; // set entire TCCR1A register to 0 TCCR1B = 0; // same for TCCR1B // set compare match register to set sample time 5ms OCR1A = 9999; // turn on CTC mode TCCR1B |= (1 << WGM12); // Set CS11 bit for prescaling by 8 TCCR1B |= (1 << CS11); // enable timer compare interrupt TIMSK1 |= (1 << OCIE1A); sei(); // enable global interrupts }

void setup() { // set the motor control and PWM pins to output mode pinMode(leftMotorPWMPin, OUTPUT); pinMode(leftMotorDirPin, OUTPUT); pinMode(rightMotorPWMPin, OUTPUT); pinMode(rightMotorDirPin, OUTPUT); // set the status LED to output mode pinMode(13, OUTPUT); // initialize the MPU6050 and set offset values mpu.initialize(); mpu.setYAccelOffset(1593); mpu.setZAccelOffset(963); mpu.setXGyroOffset(40); // initialize PID sampling loop init_PID(); }

void loop() { // read acceleration and gyroscope values accY = mpu.getAccelerationY(); accZ = mpu.getAccelerationZ(); gyroX = mpu.getRotationX(); // set motor power after constraining it motorPower = constrain(motorPower, -255, 255); setMotors(motorPower, motorPower); // measure distance every 100 milliseconds if((count%20) == 0){ distanceCm = sonar.ping_cm(); } if((distanceCm < 20) && (distanceCm != 0)) { setMotors(-motorPower, motorPower); } } // The ISR will be called every 5 milliseconds ISR(TIMER1_COMPA_vect) { // calculate the angle of inclination accAngle = atan2(accY, accZ)*RAD_TO_DEG; gyroRate = map(gyroX, -32768, 32767, -250, 250); gyroAngle = (float)gyroRate*sampleTime; currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle); error = currentAngle – targetAngle; errorSum = errorSum + error; errorSum = constrain(errorSum, -300, 300); //calculate output from P, I and D values motorPower = Kp*(error) + Ki*(errorSum)*sampleTime – Kd*(currentAngle-prevAngle)/sampleTime; prevAngle = currentAngle; // toggle the led on pin13 every second count++; if(count == 200) { count = 0; digitalWrite(13, !digitalRead(13)); } }

Step 10: Final Thoughts

Spending a bit more time on tweaking the PID constants would give us a better result. The size of our robot also limits the level of stability we can achieve. It is easier to build a full-sized balancing robot than it is to build a small one like ours. Still, I guess, our robot does a pretty decent job in balancing on various surfaces as shown in the video.

That’s it for now.

Thanks for your time. Don’t forget to leave your thoughts in the comments section.

Self Balancing Robot

First of all I want to apologize for my English, if you don’t understand something, please, ask.

I know that a self-balancing robot is not new, but when i started this project i found a lot of information, but never in the same site, i had to search a lot to join all information in a single project. Becouse of that i’m making this instrucctable, to show you all the information i get, with all detail, to make that robot.

This project is for all of you that like’s to make robots but don’t have many things, and by things i mean time, money and robotics knowledge. In this project i’m gonna show you the easiest way to do a simple, cheap and useless two wheels self-balancing robot.

I explain the materials and electronics used in the project, how and where to buy or create it and i’m gonna tell you my experience and tips along the way to create this project.

Step 1: Materials

The materials i used for this projects were the cheapest i could get, but there are even cheaper. Principally i buy from two places: DX, a Chinese online store with lots of very cheap electronic (arduino, drivers, sensors,…) and free shipping (that’s a good point); and Robot-Italy, an Italian store specialized in kits for robotics.

From Robot-Italy i get the chassis from a kit for a 3 wheeled robot and the battery, a LiPo of 1300mAh



You can buy the Arduino form both stores, in Robot-Italy have the official version (23€) and in DX have the Chinese version (10€), in not gonna open a debate but i used both and they just work fine:…

The last two thing left are the IMU sensor and the motor driver, both bought from DX:

Motor driver:…

IMU sensor:…

Making a price summary:

Arduino UNO ——————–> 10-20 €

IMU sensor ——————–> 15 €

Motor driver ——————–> 4 €

Chassis ——————–> 19 €

Battery ——————–> 10 €


TOTAL : 58-68 €

I used materials as cheap if i could but you can use whatever you have, i saw people using servo motors and stepper motors with a good result. This motor driver maybe is much bigger than the needed one, with an L293 it can work, you can make your own chassis and use other type of sensors.

Step 2: Phisics

The physics for this robot are simple, the robot stand in two points lined, the wheel, and i tends to fall and lose his verticality, the movement of the wheel in the direction of the falling rises the robot for recover the vertical position.

A Segway-type vehicle is a classic inverted pendulum control problem that is solvable in two degrees of freedom for the simplest models. The vehicle attempts to correct for an induced lean angle by moving forward or backwards, and the goal is to return itself to vertical. Or at least not fall over.

For that objective we have two things to do, in one hand we have to measure the angle of inclination (Roll) that have the vehicle, and in the other hand we have to control the motors for going forward or backwards to make that angle 0, maintaining his verticality.

Angle Measurement:

For measure the angle we have two sensors, accelerometer and gyroscope, both have his advantages and disadvantages. The accelerometer can measure the force of the gravity, and with that information we can obtain the angle of the robot, the problem of the accelerometer is that it can also measure the rest of the forces the vehicle is someted, so it has lot of error and noise. The gyroscope measure the angular velocity, so if we integrate this measure we can obtain the angle the robot is moved, the problem of this measure is that is not perfect and the integration has a deviation, that means that in short time the measure is so good, but for long time terms the angle will deviate much form the real angle.

Those problems can be resolved be the combination of both sensors, that’s called sensor fusion, and there are a lot of methods to combine it. In this project i try two of them: Kalman Filter, and complementary filter.

  • The Kalman filter is an algorithm very extended in robotics, and offers a good result with low computational cost. There is a library for arduino that implements this method, but if you want to learn more about that method or implement it by yourself look at this page.
  • The Complementary filter is a combination of two or more filters that combines the information from different sources and gets the best value you want. It can be implement in only one line of code .For more information visit this page.

angle = A * (angle + gyro * dt) + (1 – A) * accel;

where A is normally equals to 0.98.

First i tried to use Kalman filter but i don’t obtain good results, my angle was calculated with a little delay and it affect the control. The Kalman filter has three variables you can change based on the parameter of your sensor, and varying this you can obtain better result, i tried to change that values, but i don’t get better results so i decided to implement the complementary filter, so much easier and it have less computational cost. The complementary filter works fine for me.

Step 3: Chasis

For create the main structure of the robot i used the kit previously mentioned, this kit contains a simple plastic chassis with some nuts and screws, two wheels with two motors, one battery socket, one caster wheel, and even 2 little wheels for encoders. The last time i checked the price was 19€.

Tip: If you like to make your own chassis with wood, aluminium, or other materials and if you have old DC motors and wheels you can save some money.

This kit is prepared to make a 3 wheel vehicle but we gonna change the plans a little to adapt it to our project.

From the kit there are part i don’t use, like the caster wheel, two motor fastenings and some nuts and screws. I put the two motor in the lower part of the structure and closed it with the two grat plastic parts, keeping it together with the screws.

The electronics and the battery creates a tower in the upper part of the structure, i used Meccano to build the tower where the PCBs and the battery were located, but you can use other materials, like plastic, wood metal, or even only tape surrounding everywhere, like a gigant ball of tape.

And Voila! chassis done, not so difficult for the moment, let’s see next step.

Tip: When you build your robot you have to try to put the mass center the higher you can, putting heavy things in the upper part of the robot, like batteries. Remember that the more height of the center of mass the more stability the robot will have.

Step 4: Electronics

The electronics we are going to use in the project are simply three, an arduino UNO (you can use whatever arduino you have, doesn’t matter if isn’t arduino UNO), a motor driver, in this case a L298, and finally an IMU.

We use a commercial motor driver based on the chip L298, maybe much powerful that we need for these motors but i have it and it works fine. If you want to make you own DC motor driver you can use some transistors and make a H-bridge or use a L293, cheap and easy to use, there is an instructable where you can find information how to do it.

For the IMU i used the cheapest 10DOF (10 Degrees Of Freedom) i find, the chinese GY-80 with 3-axis accelerometer, 3-axis gyroscope, magnetometer, barometer and temperature sensors. We use only accelerometer and gyro so you can save money buying another IMU, like the MPU-6050, a 6DOF IMU for only 3.63€!!!!, or accelerometer and gyro for separate.

The IMU is connected to the arduino using I2C bus (If you want to lerarn more about I2C look this instructable), so we need 2 wires for communication (SDA and SCL) and 2 wires for power, it use 3.3V so we need 3.3V wire and GND.

The motor driver take power directly form the battery so don’t have to connect arduino’s power to it (i mean the 5V form the arduino), but we need 6 wires to control it, 3 for each motor, one for send the PWM signal for control the motor velocity, and for indicate the direction we want the motor to spin.

Tip: Try to position the IMU sensor (or accelerometer) in the line of the axis of the motors because if you locate the IMU far form this you can obtain much error in the accelerometer measure, remember that it measure linear acceleration, if you locate it to a distance R from the axis when the robot falls form vertical the acceleration of the accelerometer is the gravity plus R*dAngle/dt that means that it introduce an error in the measurement.

Step 5: Code

I’m not going to explain every single line of code for the project (i commented the code, if you download it i think u will have no problems to understand it), but i’m gonna show you how i organize it.

The code has 4 files: one the main code, a second one for the motors, the third is for the PID, and the last one is for the sensor code.

In the main code first i initialize the entire robot: pins, sensors, communications, … Then i calculate the error of the sensors. This part it’s very important because in this part we take the initial angle and we make it zero, it means that the sensor have an initial deviation, when we place the robot vertically the sensor don’t show that the angle is zero, instead send a deviation angle, this initial angle is used to subtract it from the posterior measurements of the sensors, to obtain the real angle. So when we initiate the robot we have to maintain it vertically until it starts to move the wheels.

The next part of code is the loop where we take the sensor values every 10 millisecond, that mean the frequency of sampling is 100Hz (you can use whatever frequency , but remember that very low and very high frequencies could not work), and we calculate the angle of the robot using, in this case, the complementary filter previously explained. We have the angle, now we can use that information to control our motors, this uses an intermediate PID, the simplest way to control things efficiently, there is an arduino library for the PID but is simple to implement it, you can code it in no more than 10-20 lines of code.

In order to use the accelerometer, in this case the ADXL345, we have to use its libraries. I used the next adafruit libraries: Adafruit_ADXL345 library and Adafruit_Sensor library.

And that’s all, simple code for simple robot, but it woks fine for me. You can implement so many more things if you want, like LCD screen, more sensors, better control, … That the magic of robots, you make one and improve it as much as you want.

Some of you have troubles using the code, i uploaded a single file with the entire project (Balacing_single_file).

Link to the google drive folder:…

Step 6: Tests

I made a lot of test in the long way for this project, first i prove the motors: direction, velocity, … Then the sensors and the sensor fusion, that was a lot of time for find the right way to use it, i made a simple processing program (included in the code file) to show the values of the sensors graphically:

That help me to understand and to get the right form to take the real angle using Kalman or complementary filter.

At the end i prove the robot itself, the first prove was no as expected but it seems like we can achieve it

After some PID adjust and some code cleaning i reach the goal, maintain the robot vertically all the time, and even recover from pushing it with a little force. As you can see the robot walks with no control, drifting around without sense, but always vertical, that’s we wanted (for now).

Step 7: Improvements

There are some much improvements to do with this robot, this is the first step of many more:

  1. The first i want to implement is the position recovery, i don’t want my robot to walk around the room like a zombie although my cat like it, not me :D. For that we need encoders for measure the movement of the wheel and use it for bring the robot back to the initial position.
  2. Control the movement of the robot, forward, backward and turning , that is easy, the only thing we have to do is change the angle we want the robots stay then the gravity will do his work and the robot will move in the direction of the angle, then we put the angle to zero again and the robot stops. For turning we have to put some offset in the motor velocity, for turning right we subtract the offset to the velocity in right wheel and sum it to the velocity of the left wheel.
  3. Adding a WiFi shield to control it via internet.
  4. Implementation with Raspberry Pi to allow the robot to use a camera.
  5. Implementation of a camera and artificial vision for the robot.
  6. Use a ball instead of wheels, so hard, but we’ll try.

Balancing Robot

The Arduino SDK has been updated since I last worked on this, and the latest version gave me this error when compiling.
This was due to where the libraries were.

libraries\MPU6050\xxxI2Cdev.cpp.o (symbol from plugin): In function `I2Cdev::I2Cdev()’:
(.text+0x0): multiple definition of `I2Cdev::I2Cdev()’

You won’t get an error if you put the header files in with the program file like so

But if like me you prefer to keep your libraries in the Arduino Libraries folder
and add them using the Include Library tab like this

because the header file MPU6050_6Axis_MotionApps20.h already includes (#include”I2Cdev.h”)

#ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_
#define _MPU6050_6AXIS_MOTIONAPPS20_H_
#include “I2Cdev.h”
#include “helper_3dmath.h”
// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board

#include “MPU6050.h”

you may see the above error. The compiler looks for I2Cdev.h in the MPU6050 folder
but its not there! So copy I2Cdev.cpp & I2Cdev.h from I2Cdev Folder to
the MPU6050 Folder then add libraries….

Shopping list of objects you will need.
1.  Compatible Nano V3.0 – ATmega328

2.  L298N DC Stepper Motor Dual H Bridge

3. MPU-6050 3 Axis Gyroscope And Accelerometer
To calibrate your MPU-6050 watch this  not a little helpful

4. SYB-170  Breadboard

5. Smart Car Robot Wheels

6.  10V 3900uf Capaictor

7. Ferrite core noise filters 25mm long 5mm core hole

You will need the I2Cdev & MPU6050  files available here
courtesy Jeff Rowberg

//   Most of this code is other peoples read below!!
// the bits I have hacked are indicated thusly//********************************************************************************************************************************************************

// i found PlyAlex  not a little helpful
// if you get the   ERROR NO LIBRARY FOUND   edit sketch book location in File-> Preferences
// default location of sketch book is not where you think it is.

//   Most of this code is other peoples read below!!
// the bits I have hacked are indicated thusly


// i found PlyAlex  not a little helpful
// if you get the   ERROR NO LIBRARY FOUND   edit sketch book location in File-> Preferences
// default location of sketch book is not where you think it is.


// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <>
// Updates should (hopefully) always be available at
// Changelog:
//      2013-05-08 – added seamless Fastwire support
//                 – added note about gyro calibration
//      2012-06-21 – added note about Arduino 1.0.1 + Leonardo compatibility error
//      2012-06-20 – improved FIFO overflow handling and simplified read process
//      2012-06-19 – completely rearranged DMP initialization code and simplification
//      2012-06-13 – pull gyro and accel data from FIFO packet instead of reading directly
//      2012-06-09 – fix broken FIFO read sequence and change interrupt detection to RISING
//      2012-06-05 – add gravity-compensated initial reference frame acceleration output
//                 – add 3D math helper file to DMP6 example sketch
//                 – add Euler output and Yaw/Pitch/Roll output formats
//      2012-06-04 – remove accel offset clearing for better results (thanks Sungon Lee)
//      2012-06-01 – fixed gyro sensitivity to be 2000 deg/sec instead of 250
//      2012-05-30 – basic DMP initialization working

/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the “Software”), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.


// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include “I2Cdev.h”

#include “MPU6050_6Axis_MotionApps20.h”
//#include “MPU6050.h” // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include “Wire.h”

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <– use for AD0 high

/* =========================================================================
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
depends on the MPU-6050’s INT pin being connected to the Arduino’s
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
digital I/O pin 2.
* ========================================================================= */

/* =========================================================================
NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
when using Serial.write(buf, len). The Teapot output uses this method.
The solution requires a modification to the Arduino USBAPI.h file, which
is fortunately simple, but annoying. This will be fixed in the next IDE
release. For more info, see these links:,109987.0.html
* ========================================================================= */

// uncomment “OUTPUT_READABLE_QUATERNION” if you want to see the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
// on a remote host such as Processing or something though)

// uncomment “OUTPUT_READABLE_EULER” if you want to see Euler angles
// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that Euler angles suffer from gimbal lock (for more info, see

// uncomment “OUTPUT_READABLE_YAWPITCHROLL” if you want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions coming
// from the FIFO. Note this also requires gravity vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info, see:

// uncomment “OUTPUT_READABLE_REALACCEL” if you want to see acceleration
// components with gravity removed. This acceleration reference frame is
// not compensated for orientation, so +X is always +X according to the
// sensor, just without the effects of gravity. If you want acceleration
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.

// uncomment “OUTPUT_READABLE_WORLDACCEL” if you want to see acceleration
// components with gravity removed and adjusted for the world frame of
// reference (yaw is relative to initial orientation, since no magnetometer
// is present in this case). Could be quite handy in some cases.

// uncomment “OUTPUT_TEAPOT” if you want output that matches the
// format used for the InvenSense teapot demo

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { ‘$’, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, ‘\r’, ‘\n’ };
//                                                 MY Variables

float OldP = 0;         // Previous value used to calculate change in P //  DELTA P
float P = 0;            //  Proportional component
float I = 0;            //  Integral        just the sum of P over time
float OldI = 0;         //  previous value of I for calculation of Delta I
float D = 0;            //  Differential       D = P – OldP
float bp = -60;         // balance point
float pwm = 0;          // value of Pulse Width Modulation  to ENA ENB
long a = 0;             // L298N to IN 1 to 4
long b = 0;             //
const int PinR1 = 5;    //  arduino  pin 5 to l298  pin IN4
const int PinR2 = 6;    //  arduino  pin 6 to l298  pin IN3
const int PinL1 = 7;    //  arduino  pin 7 to l298  pin IN1
const int PinL2 = 8;    //  arduino  pin 8 to l298  pin IN2
const int PwmR  = 9;    //  arduino  pin 9 to l298  pin ENB
const int PwmL  = 10;   //  arduino  pin 10 to l298  pin ENA

// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;

// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
// arduino to l298 pins hopefully self explanatory
// join I2C bus (I2Cdev library doesn’t do this automatically)
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
Fastwire::setup(400, true);

// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it’s
// really up to you depending on your project)
while (!Serial); // wait for Leonardo enumeration, others continue immediately

// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
// Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
// the baud timing being too misaligned with processor ticks. You must use
// 38400 or slower in these cases, or use some kind of external separate
// crystal solution for the UART timer.

// initialize device
Serial.println(F(“Initializing I2C devices…”));

// verify connection
Serial.println(F(“Testing device connections…”));
Serial.println(mpu.testConnection() ? F(“MPU6050 connection successful”) : F(“MPU6050 connection failed”));

//     Commented out so that program dose not wait for imput,but gets straight to work
// wait for ready
Serial.println(F(“\nSend any character to begin DMP programming and demo: “));
while (Serial.available() &&; // empty buffer
while (!Serial.available());                 // wait for data
while (Serial.available() &&; // empty buffer again
// load and configure the DMP
Serial.println(F(“Initializing DMP…”));
devStatus = mpu.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity
//  use calibration program to get your own values
mpu.setXAccelOffset(-1875);//(1788); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it’s ready
Serial.println(F(“Enabling DMP…”));

// enable Arduino interrupt detection
Serial.println(F(“Enabling interrupt detection (Arduino external interrupt 0)…”));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it’s okay to use it
Serial.println(F(“DMP ready! Waiting for first interrupt…”));
dmpReady = true;

// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it’s going to break, usually the code will be 1)
Serial.print(F(“DMP Initialization failed (code “));

// configure LED for output


// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
// if programming failed, don’t try to do anything
if (!dmpReady) return;

// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
// other program behavior stuff here


// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

// get current FIFO count
fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
Serial.println(F(“FIFO overflow!”));

// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);

// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;

// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

//                                                                      My Code
// PID control based on Pseudocode from
// and the balance point idea from

OldP =P;                     // save value of P
P = (ypr[2] * 1000) + bp;    // update P from MPU add bp to correct for balance
OldI = I;                    // save old I
I = I + (P * 0.05) ;
I = I + ((I – OldI)*2  );     // calulate new I
if( I >  250 ) I =  250;           // LIMIT  Stop I building up too high
if( I < -250 ) I = -250;           // or too low value
D = P – OldP;                      //  D differential   change in P
pwm = ( P * 1 ) + ( I  ) + ( D * 10 ) ; // P I D

a = 0;
b = 0;
if(pwm < 0){
a = 0;
b = 1;
bp = bp – 0.01;
digitalWrite(13, 0);
if(pwm > 0){
a = 1;
b = 0;
bp = bp + 0.01;
digitalWrite(13, 1);
// remove sign from PWM as – value has no meaning
pwm  = abs(pwm);
if ( pwm < 0) pwm = 0;
if ( pwm > 255) pwm = 255;

if(abs(ypr[2]) < abs(1.1)){
analogWrite(PwmR, pwm);
digitalWrite(PinR1, a);
digitalWrite(PinR2 ,b);

analogWrite(PwmL ,pwm);
digitalWrite(PinL1 ,a);
digitalWrite(PinL2 ,b);
analogWrite(PwmR , 0);
analogWrite(PwmL , 0);
I = 0;
bp = -98;


Self-Balancing Two-Wheel Robot Is Controlled By Android


apk download link:


-Plastic Tire Wheel with DC 3-6V Gear Motor for Robot 65*27MM   x2

-UNO R3 MEGA328P ATMEGA16U2 Development Board   x1
-Plastic Box For Robotic Body   x2

-Pololu TB6612FNG Dual Motor Driver
-MPU6050 6 Axis Acceleration and Gyro Sensor
-HC-05 Bluetooth Module
-Samsung ICR18650-22F 2200mAh(Green)Battery

Circuit  Diagram


arduino code download link:

Kod listesi

Franko – Arduino Self Balancing Robot

My latest Arduino DIY project is a self balancing robot.

Arduino Uno R3 microcontroller board is used for this project. MPU6050, an 6DOF IMU (with accelerometer and gyroscope) is used to get the angle and L298N motor controller board controls the 2 motors.

The motors are 12V 122rpm and the wheels are 8cm in diameter.

Robot size is 17x10x30 cm. Three plexiglas plates are used to hold the frame and parts. Standard PID controller is used to control the robot. To tune the PID controller parameters 3 potentiometers are used.

You may find the Arduino sketch and libs on my GitHub repo:

1 x Arduino Uno R3
1 x MPU6050 (IMU)
1 x L298N Motor Driver Controller
2 x 12V 122 RPM Gear Motors + 80mm Wheels
3 x 10k Potentiometer

Two-wheel self-balancing robot

After my first fast made self balancing robot, I want to better understand it and especially to try different control methods. For me it is necessary to simulate the whole stuff on computer (robot + control law).

The first step is therefore to model the robot with mathematical equations. Because of the complexity of the model I do make some assumption and simplification.

The model is separated into three part: the DC motor, the wheel and the inverted pendulum

The DC motor

electrical model:





equation of motion for the motor:rotor




If the motor inductance and friction torque are neglected, then


in state space form:


The inputs to the motor is the applied voltage and applied torque on the rotor.

The wheel


in horizontal (x direction):


rotation equation around the center of the wheel:


by using motor dynamic equations, one can get






Above equation is valid for both wheels.

With following equations, one can translate the angular rotate to linear motion:




After rearrangement, one has:


The inverted pendulum




The sum of forces perpendicular to the pendulum:


The sum of motions around the center of mass of pendulum:




one has:








after arrangement, one can get the following state space expression:




On my first two-wheel self-balancing robot, I didn’t use the encoder for speed measurement.
So it can basically stand still and balance its weight with small movements of its two
wheels simultaneously. I preferred that the robot would work very soon without much effort
and then I could improve it by adding speed control, RC, etc.


It’s not difficult to gather all the parts. Here is my list: fiber board
2.control board (arduino uno)
3.sensor board (MPU6050 gyroscope + accelerameter)
4.two-channel motor driver (L298N)
5.two geared DC motors (25GA370)
6.two toy wheels and fixation kit

Following is my assembly diagram:


Following is the wiring diagram:


In L298N, each motor is controlled by a full bridge circuit (or H-bridge) composed of two legs. Following figure is a simplified diagram coming from wikipedia. S1 and S2 are always controlled in complementary manner. When S1 closes, S2 opens. S1 and S2 close simultaneously is not allowed since that creates a short circuit of the power supply. Idem for S3 and S4.


When S1 and S4 close simultaneously, the motor rotates in one direction. When S3 and S2 close simultaneously, the motor rotates in the other direction. I use PWM control to set the motors’ speed. the switching frequency is about 1kHz which is the default value of Arduino module.

After the wiring, the robot looks like:


Then comes the control part. The following figure shows the control diagram. Only the inner
loop in blue is implemented this time. I use it to regulate the tilt angle of the robot. As
I want the robot stands straight, I simply set the reference angle (REF_ANGLE) to 0. The
gyro+acc sensor gives the information needed to calculate the actual tilt angle. The error
between the reference and the measurement is used by the PID controller to give the command
to the robot (the two wheel motors).


2.1 PID controller

The PID controller is a complexe topic. With google you can find tons of material about it.
Fortunately it is relatively simple to implement it in a microcontroller. With some basic
understanding and tuning techniques, one can make it work very easily.

The general mathematical form of the PID controller is (extracted from wikipedia):

Here is a simple software loop that implements a PID algorithm (extracted from wikipedia):

previous_error = 0
integral = 0 
  error = setpoint - measured_value
  integral = integral + error*dt
  derivative = (error - previous_error)/dt
  output = Kp*error + Ki*integral + Kd*derivative
  previous_error = error
  goto start

The purpose of the PID controller is to generate a suitable command to the system (the self
balancing robot in this case) so that the system is stable and reactive. There are some
creteria of both the static and dynamic performance. But they are out of the scope. The PID
controller uses three parameters to do the job, the proportional (Kp), integral (Ki),
derivative(Kd) parameter. The big Kp makes the tilt angle error small and the reaction fast. The
Ki makes the static angle error dispear. This is important since I want the robot stands
straight (tilt angle=0) all the time. I choose to put Kd=0 because Ki and Kp achieve
perfectly my desired performance. So in fact I use a PI controller. For the PI parameter
tuning, I follow Ziegler-Nichols method that you can find on wikipedia. Following table is
copied from wikipedia. It works well for me.


2.2 Tilt angle measurement

A sensor combining a gyroscope and an accelerometer is used to measure the tilt angle. The sensor don’t give the angle directly. Some calculation is needed. I use MP6050 as sensor. You can check its datasheet for more information.

The accelerometer gives the acceleration (unit g) in three dimension. If the robot is perpendicular to the earth (z axis for example), you should have only Z axis which is not zero. If you have another axis not equaling zero besides Z axis in static state, it means the robot has a none zero tilt angle. And you can get easily the tilt angle based on these two axis acceleration. However using accelerometer to get the tilt angle in this way is valid only for static state since we suppose that only gravity acts on the robot. When the two wheels act a force on the robot, the accelerometer can measure the resulting acceleration as well! In consequence, the measures are very noised. The good point is it is accurate in long period of time (or in DC after filtering).

The gyroscope measures the angle speed (unit deg/s) in three dimension. In order to get the angle, we need to do the integration of it. It is accurate dynamically (in short period). But the integration action accumulates the error of measurement, which makes the result drift in long period of time.

I use a complementary filter to combine the two sensor measurements. It is simple and needs little microcontroller calculation. It works well for me (so far). One can implement kalman filter for optimal control. But the knowledge of the system model and more calculation capability are required. It needs definitely more effort. I use following complementary filter to get the tilt angle estimation:


The sensor communicates to the microcontroller through I2C bus. By using the WIRE library of arduino, it is easy to read the data. I use analogWrite function to set PWM duty cycle for the motor driver L298N. The command (PID controller’s output) is a value between -255 and 255. Positive sign means the robot moves forward. 255 represents the max speed (100% duty cycle for the first half bridge). I use anti-windup code to prevent the command to be higher than 255 (or lower than -255) which is the limit of PWM register (8 bits) max value.

The calibration of the sensor should be done generally. I calibrated the gyroscope because I see big static offset. My accelerometer’s offset is not obvious. I use 10ms as sampling time.

The total code has only about 50 lines which I believe possible to be optimized. It’s simple, right? :)


The robot resists relatively well the disturbance from both sides. The stability and dynamic response are satisfactory. The next step is to install an encoder on each motor, to make the robot possible to move and to add a remote control. Following is a video of it standing still.


How to Build an Arduino Self-Balancing Robot

By Roland Pelayo

Ever wonder how Segways work? This tutorial will show you how to build an Arduino self-balancing robot that balances itself — just like a Segway!

How Does Balancing Work?

To keep the robot balanced, the motors must counteract the fall of the robot. This action requires a feedback and a correcting element. The feedback element is the MPU6050 gyroscope + accelerometer, which gives both acceleration and rotation in all three axis (MPU6050 I2C basics) which is used by the Arduino to know the current orientation of the robot. The correcting element is the motor and wheel combination.

Required Materials

  • Arduino (UNO or Nano can be used. I used Nano for this project.)
  • MPU6050 gyro+accelerometer breakout board.
  • L298N driver module
  • 2 x Geared DC Motor+wheels
  • Three platforms (PCB, acrylic, or thick plastic)
  • Posts to hold the platforms
  • Jumper wires
  • Headers
  • Battery Pack


Connection Diagram

Complete Fritzing diagram

Connect the MPU6050 to the Arduino first and test the connection using the codes in this IMU interfacing tutorial.  If data is now displayed on the serial monitor, you’re good to go! Proceed to connect the rest of the components as shown above.

The L298N module can provide the +5V needed by the Arduino as long as its input voltage is +7 V or greater.  However, I chose to have separate power sources for the motor and the circuit for isolation. Note that if you are planning to use a supply voltage of more than +12V for the L298N module, you need to remove the jumper just above the +12V input.

Building the Robot

Robot frame (made mostly of acrylic slab) with two geared dc motors

Main circuit board consisting of an Arduino Nano and MPU6050

L298N motor driver module

Geared DC motor with wheel


The self-balancing robot is essentially an inverted pendulum. It can be balanced better if the center of mass is higher relative to the wheel axles. A higher center of mass means a higher mass moment of inertia which corresponds to lower angular acceleration (slower fall). This is why I’ve placed the battery pack on top. The height of the robot, however, was chosen based on the availability of materials.

Completed self-balancing robot. At the top are six Ni-Cd batteries for powering the circuit board. In between the motors is a 9V battery for the motor driver.


More Self-balancing Theories

In control theory, keeping some variable (in this case, the position of the robot) steady needs a special controller called a PID: P for proportional, I for integral, and D for derivative. Each of these parameters has “gains” normally called Kp, Ki, and Kd.

PID provides correction between the desired value (or input) and the actual value (or output). The difference between the input and the output is called “error”. The PID controller reduces the error to the smallest value possible by continually adjusting the output. In our Arduino self-balancing robot, the input (which is the desired tilt, in degrees) is set by software. The MPU6050 reads the current tilt of the robot and feeds it to the PID algorithm which performs calculations to control the motor and keep the robot in the upright position.

PID requires that the gains Kp, Ki, and Kd values be “tuned” to optimal values. Engineers use software like MATLAB to compute these values automatically. Unfortunately, we can’t use MATLAB in our case because it would further complicate the project. We will tune the PID values manually instead. I’ve outlined the steps on how to do this:

    1. Make Kp, Ki, and Kd equal to zero.
    2. Adjust Kp. Too little Kp will make the robot fall over (not enough correction). Too much Kp will make the robot go back and forth wildly. A good enough Kp will make the robot slightly go back and forth (or oscillate a little).
    3. Once the Kp is set, adjust Kd. A good Kd value will lessen the oscillations until the robot is almost steady.  Also, the right amount of Kd will keep the robot standing even if pushed.
    4. Lastly, set the Ki. The robot will oscillate when turned on even if the Kp and Kd are set but will stabilize in time. The correct Ki value will shorten the time it takes for the robot to stabilize.


Arduino Self-balancing Robot Code

I needed four external libraries to make this Arduino self-balancing robot work. The PID library makes it easy to calculate the P, I, and D values. The LMotorController library is used for driving the two motors with the L298N module. The I2Cdev library and MPU6050_6_Axis_MotionApps20 library are for reading data from the MPU6050. You can download the code including the libraries in this repository.

#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

 #include "Wire.h"

#define MIN_ABS_SPEED 20

MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

double originalSetpoint = 173;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;

//adjust these values to fit your own design
double Kp = 50;   
double Kd = 1.4;
double Ki = 60;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.5;
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 8;
int IN4 = 9;
int ENB = 10;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
 mpuInterrupt = true;

void setup()
 // join I2C bus (I2Cdev library doesn't do this automatically)
 TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
 Fastwire::setup(400, true);


 devStatus = mpu.dmpInitialize();

 // supply your own gyro offsets here, scaled for min sensitivity
 mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

 // make sure it worked (returns 0 if so)
 if (devStatus == 0)
 // turn on the DMP, now that it's ready

 // enable Arduino interrupt detection
 attachInterrupt(0, dmpDataReady, RISING);
 mpuIntStatus = mpu.getIntStatus();

 // set our DMP Ready flag so the main loop() function knows it's okay to use it
 dmpReady = true;

 // get expected DMP packet size for later comparison
 packetSize = mpu.dmpGetFIFOPacketSize();
 //setup PID
 pid.SetOutputLimits(-255, 255); 
 // ERROR!
 // 1 = initial memory load failed
 // 2 = DMP configuration updates failed
 // (if it's going to break, usually the code will be 1)
 Serial.print(F("DMP Initialization failed (code "));

void loop()
 // if programming failed, don't try to do anything
 if (!dmpReady) return;

 // wait for MPU interrupt or extra packet(s) available
 while (!mpuInterrupt && fifoCount < packetSize)
 //no mpu data - performing PID calculations and output to motors 
 motorController.move(output, MIN_ABS_SPEED);

 // reset interrupt flag and get INT_STATUS byte
 mpuInterrupt = false;
 mpuIntStatus = mpu.getIntStatus();

 // get current FIFO count
 fifoCount = mpu.getFIFOCount();

 // check for overflow (this should never happen unless our code is too inefficient)
 if ((mpuIntStatus & 0x10) || fifoCount == 1024)
 // reset so we can continue cleanly
 Serial.println(F("FIFO overflow!"));

 // otherwise, check for DMP data ready interrupt (this should happen frequently)
 else if (mpuIntStatus & 0x02)
 // wait for correct available data length, should be a VERY short wait
 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

 // read a packet from FIFO
 mpu.getFIFOBytes(fifoBuffer, packetSize);
 // track FIFO count here in case there is > 1 packet available
 // (this lets us immediately read more without waiting for an interrupt)
 fifoCount -= packetSize;

 mpu.dmpGetQuaternion(&q, fifoBuffer);
 mpu.dmpGetGravity(&gravity, &q);
 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
 input = ypr[1] * 180/M_PI + 180;

My Kp, Ki, Kd values may or may not work you. If it doesn’t, then follow the steps I outlined above. Notice that the input tilt in my code is set to 173 degrees. You can change this value if you’d like but take note that this is the tilt angle to which the robot must be maintained.

Also, if your motors are too fast, you can adjust the motorSpeedFactorLeft and motorSpeedFactorRight values.


Here’s the self-balancing robot in action!

What’s Next?

An improvement for this self-balancing robot would be controlling it remotely. Unfortunately, I can’t make this robot do that unless I make it ultra stable using dc motors with an encoder. An encoder would update the current speed of the motor which I can use to make the robot steadier via a second PID loop. I might do that next time so stay tuned!

Your Arduino Balancing Robot (YABR)

Your Arduino Balancing Robot (YABR) is a self-balancing robot that you can build yourself as a school project or as a fun project with your kids. It might look simple but there is a lot that you can learn from building this self-balancing robot.

In contrast to most self-balancing robots, this one uses stepper motors instead of regular DC motors. The main reason is that stepper motors are precise and have no performance loss when the battery voltage drops. One pulse is always an exact amount of motion. Regular DC motors can have mechanical friction and electric resistance differences. This can cause performance differences. As a result the robot will not move in a straight line.

The total cost to build this robot is approximately $80 if you use the hardware list below. This includes a battery, Nunchuck, charger, stepper motors, etc.

The Arduino program that you can download for free is 100% self-written and not based on any other software. The code is well commented and clearly explained. This makes it possible to further develop the code for your own purpose.

 Click to see the full image

If you encounter any problems during the build or setup please check the Q&A page first. Most questions are already answered in detail.

Step 1 – Software

First download the complete YMFC-AL software package: (version 1.1)

Step 2 – Hardware

To build this robot you need hardware. I made the following list for convenience purpose alone. You are free to get your own hardware from different sources. But this is the hardware that I used/ordered:

If the 35mm stepper motors are out of stock. You could also use these stepper motors. Please note that these are 42mm in stead of 35mm. So you have to modify the frame.

Most of the following parts can be found in your local electronics store. But in case you don’t have an electronics store nearby I will put links here:

And finally you need an old inner tube and two sheets of plywood. I used 2.5mm and 12.5mm sheets.

Step 3 – Tools

And of course you need some simple tools like a soldering iron, screwdrivers, a fretsaw, compact drill, etc.

Step 4 – The build

Watch the YABR hardware build video and build the robot according to the video and the schematic that is included in the complete software package.

Building the YABR balancing robot.

Detailed pictures of my own YABR balancing robot can be found in the media section of this project page.

4.1 The diode and resistors

The resistor R1 on the schematic is needed for uploading a program to the Arduino. The TXD output of the transceiver is forced high or low. As a result the FTDI programmer cannot change this output anymore and you will get an upload error. By adding this resistor the FTDI programmer can change the voltage on the RX-pin of the Arduino despite the state the transceiver output and the program is uploaded without any problems.

The other two resistors (R2 and R3) form a voltage divider. Meaning that the 12.6 volt of the battery minus the 0.6 volt voltage drop over the diode is divided by 2.5. Resulting in a 4.8 volt on the analog input when the battery fully charged. In the main program this analog input will be used to protect the battery. This is because lipo batteries can be damaged when the voltage drops below 3 volt per cell.

The diode D1 protects all the electronics against reversed polarity. So when you accidentally reverse the connections of the battery the components won’t go up in smoke.

4.2 The MPU-6050 gyro/accelerometer

The only gyro / accelerometer that is supported by the YABR software is the MPU-6050. This is because the self-level feature requires an accelerometer and a gyro as I explain in these two videos:

MPU-6050 6dof IMU tutorial for auto-leveling quadcopters – Part 1
MPU-6050 6dof IMU tutorial for auto-leveling quadcopters – Part 2

The orientation of the gyro is important. Make sure to mount the gyro in the exact same orientation as shown on this picture. Otherwise the YABR software cannot calculate the correct angle and the robot will not work.

4.3 Hardware test

Please note that you are soldering the wires on the back side of the PCB. The schematic is drawn facing the components from the front. So everything is mirrored and you need to double check all the connections before you connect any power to the PCB.

With everything in place it’s time to connect the FTDI programmer to the Arduino pro mini. Don’t connect the battery yet. If the LED’s don’t lit up there is a short circuit in the wiring and you need to disconnect the FTDI programmer as soon as possible. Normally the Arduino pro mini is already programmed with the blink sketch so the LED on the Arduino should start to flash.

To check if the gyro is connected correctly and to check the balancing point of the robot you need to upload the “hardware-check” program that you can find in the software package that you downloaded earlier.

Test the balancing angle of the robot and fix it in that position on a stand as I showed in the video. Upload the “hardware-check” program, open the serial monitor and set the baud rate to 9600.

After uploading the program, open the serial monitor and set the baud rate to 9600.

The program will check if there is any I2C device is connected and if this is a MPU-6050. If everything is working as expected the program will output several raw gyro values on the screen. These are just examples and your values may be different. Note down the balance value that you see in the serial output. You will need it later in the main program.

4.4 Limit the motor current

Next thing on the to do list is to set the stepper controllers to the correct drive current. If the motor current is set to high the stepper controllers will heat up and they might get damaged.

First set the potentiometer at the same position as shown in the picture below. Now always be careful when connecting the lipo battery for the first time. A short circuit can cause high currents, heat, sparks, and burns.

A good alternative is to use a small DC fuse like the one below. This fuse will blow if the wiring on the back side of the pcb holds a short circuit.

The easiest and safest way to set the correct current and to check if the wiring is correctly connected is by measuring the current in the power supply wires with a bench power supply. The power supply is limited to 500milli amps. This is an extra safety feature. If there is a short circuit the power supply will limit the current to 500 milli amps and the wiring will be fine.

First connect one motor. With the potentiometer it’s possible to set the current between 100 and 150mA. This is more than enough to get good performance. Do the same with the other stepper controller.

If you don’t have the possibility to measure the current just set the potentiometer in this position and feel if the stepper driver doesn’t get to hot. But it’s always best to measure the current.

3.5 The remote control

If you open the Nunchuck you can note the wire colors that are connected to the various pins as you can see on the schematic. The Nunchuck works on 3.3V and you can use the 3.3V output of the Arduino Uno to power the Nunchuck.

The 5V output can be used for the transceiver. Again, connect the wires as shown on the schematic to get it to work.

To check if the Nunchuck is connected correctly you need to upload the “hardware-check” program that you can find in the software package that you downloaded earlier. After uploading the program, open the serial monitor and set the baud rate to 9600.

The program will check if there is any I2C device is connected and if this is a Nunchuck. If everything is working as expected the program will output several raw joystick values on the screen.

Step 5 – Upload the software

And finally you can upload the YABR-remote program to the Arduino Uno and the YABR-robot program to the Arduino pro mini.

The YABR-remote program does not need any modification. In the robot program you need to change the accelerometer calibration value to the value that was shown by the hardware test program when the robot was in it’s balancing position.

To start the robot you need to power up the remote. Lay the robot on it’s back and switch on the power. The LED will blink indicating that the gyro is calibrating. When the blinking stops you can slowly lift the robot and it will automatically start to balance itself.

And basically that’s it. You can now control the robot with the Nunchuck.