In this article, I would like to show you how to build a self balancing two-wheel robot (that is so-called segway). In this project, you will learn how to access 6-axis motion sensor, motor control, and basic automatic control theorem.
Step 1: Materials
- Raspberry Pi, AC-to-DC 5V power adapter, plastic enclosure
I am using Raspberry Pi 2, with Raspbian installed.
- USB WiFi adaptor,
I’m using Edimax Wifi adaptor.
- DC motors x2, wheels x2, car frame, acrylic sheet x2, motor bracket x2, brass stud x8, shaft coupling x2, screw xn,
I purchased them from Here
The gear ratio of the motors I got is 1:34. I think the one of 1:21 should be better. You can use any other DC motors as long as the speed and the torgue is large enough for your robot.
- AC-to-DC 12V power adapter
- L293D IC,
I purchased it from Here.
- MPU6050 module,
I purchased it from Here.
- Breadboard: 830 tie-points
- Jumper wires,
Male-to-Male wires xN,
Male-to-Female wires xN
- Cable Tie x8
- Total cost about $123.0
Step 2: Frame Installation
Please follow the installation Instructions to build the car frame.
Step 3: Circuit Installation
Please follow the circuit diagram to install components to the breadboard and insert connection to you RPi.
Let me have a short explaination of the circuit.
The interface of the MPU6050 module is i2C. There are four pins have to be connected to Raspberry Pi GPIO as below.
- MPU6050 VCC --> RPi GPIO header pin#1 (3.3V),
- MPU6050 SDA --> RPi GPIO header pin#3 (SDA)
- MPU6050 SCL --> RPI GPIO header pin#5 (SCL)
- MPU6050 GND --> RPi GPIO header pin#6 (GND)
To drive DC motors, I am using the popular L293D IC.
- RPi GPIO header pins (pin#11, pin#13, and pin#15) are for the left wheel.
They have to be connected to L293D (pin#2, pin#7 and pin#1)
- RPi GPIO header pins (pin#16, pin#18, and pin#22) are for the right wheel.
They have to be connected to L293D (pin#15, pin#10 and pin#9)
- L293D pin#3 & pin#6 --> left motor M+ and M-.
- L293D pin#14 & pin#11 --> right motor M- and M+.
To drive L293D, we should have extra power with enough voltage. What I'm using is a 110V AC-to-DC 12V adaptor and then connect to L293D.
- L293D Vs --> 12V DC
- L293D Vss --> 5V DC
- L293D pin#4, #5, #13, #12 --> GND
Step 4: Tight Them Up
Please use the cable ties to tight all the parts up.
Step 5: Software Installation
I’m assuming that you already installed the Raspbian with WiFi adapter and you know how to use ssh to login to your RPi. We will start from installing i2C kernel module and wiringPi library.
The i2C kernel module is to help us to access MPU6050 and the WiringPi library is to help us to access GPIO.
Here are the instructions:
To install i2c kernel module,
$ sudo apt-get install libi2c-dev
To setup i2c kernel module,
$ sudo vi /etc/modules
Add following lines into the file.
We also have to check a blacklist file.
$ sudo vi /etc/modprobe.d/raspi-blacklist.conf
Make sure the following two lines are commented, then save.
$ sudo raspi-config
In Advanced Options -> I2C, please enabled it.
Then, reboot your RPi
$ sudo bash; sync;sync;reboot
When it goes back, check if the i2c driver kernel module is loaded automatically.
$ lsmod |grep i2c
i2c_dev 6027 0
i2c_bcm2708 4990 0
Then, let's install the wiringPi library.
Install git first.
$ sudo apt-get install git-core
Download and install wiringPi
$ git clone git://git.drogon.net/wiringPi
$ cd wiringPi
$ sudo ./build
Step 6: Testing MPU6050 Sensor
Now, we will make sure the MPU6050 actually works.
Let's install the i2c testing tool.
$ sudo apt-get install i2c-tools
Execute it to check if it saw the MPU6050 on i2c bus.
$ sudo i2cdetect -y 0 (for RPi Revision 1 board) <br> or <br>$ sudo i2cdetect -y 1 (for RPi Revision 2, Pi B, Pi B+, or Pi 2 boards)
If you saw the output as below, it saw the MPU6050. It means the mpu6050 device is on i2c bus and its i2c address is “0x68”
0 1 2 3 4 5 6 7 8 9 a b c d e f <br>00: -- -- -- -- -- -- -- -- -- -- -- -- -- <br>10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- <br>20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- <br>30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- <br>40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- <br>50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- <br>60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- -- <br>70: -- -- -- -- -- -- -- --
Let's try some simple tools to get those acceleration and gyroscope information.
Download and compile following tool
$ cd <br>$ git clone <a href="https://github.com/wennycooper/mpu6050_in_c.git" rel="nofollow"> https://github.com/wennycooper/mpu6050_in_c.git <br></a>$ cd mpu6050_in_c $ cat README.md<br>$ gcc -o mpu6050_accl ./mpu6050_accl.c -lwiringPi -lpthread -lm $ gcc -o mpu6050_gyro ./mpu6050_gyro.c -lwiringPi -lpthread -lm
To get acceleration information, please execute
$ sudo ./mpu6050_accl
You should see something like:
My acclX_scaled: 0.140625
My acclY_scaled: -0.031006
My acclZ_scaled: 0.994141
My X rotation: -1.768799
My Y rotation: -8.047429
The above acclX, acclY & acclZ are the measured acceleration in g(the g=9.8m/s^2) in X, Y and Z axis.
In fact, the accelerometer only tells us acceleration information. With some basic trigonometric functions calculation, we can obtain the orientation angles. They are “My X & Y rotation” and the unit is in degree.
Please try put the car frame a little bit forward or leanback, and watch the changes of the measurement. You will understand it more.
Let's also check gyroscope measurement. Please execute:
$ sudo ./mpu6050_gyro
It will tell you the velocity (in degree/s) of angle in each axis, like this.
My gyroX_scaled: -4.312977 <br>My gyroY_scaled: 0.458015 <br>My gyroZ_scaled: 0.366412 <br>My gyroX_scaled: -4.053435 <br>My gyroY_scaled: 0.427481 <br>My gyroZ_scaled: -0.160305
Please try rotate the car frame and watch the changes by time. Then you will understand the gyroscope measurement more.
If we only use acceleration information to calculate the orientation angle, the angle will be not stable and not reliable. So, somebody invent the method to use the velocity of angle information to calculate the angle, and complement with the angle from acceleration. Then it will give us more stable and reliable angle.
It's so-called "Complementary filter". You will see the complementary filter in our code later. Check for more detail explanation in reference documents.
Step 7: Testing DC Motors
Let's download and compile the code to make the DC motors rotating.
$ git clone https://github.com/wennycooper/dcMotor.git
$ cd dcMotor
$ cat README.md
$ gcc -o dcMotor0 dcMotor0.c -lwiringPi -lpthread
Now we are ready to make the motors go. Please make sure all the circuit components and 12V DC power are ready and connected. Bring your robot off the ground and execute:
$ sudo ./dcMotor0
How is that? If everthing works, the two motors should rotate in the same direction together. They should rotate for 5 seconds in forward direction and then for 5 seconds in backward direction, then stopped.
If anything went wrong, please go back to the previous steps to check. If both motors rotated in opposite direction, please swap the M+ & M- connections in one motor.
One more thing to mention is: Is the rotation speed fast enough??
If they rotated slowly, say two revolutions per second, it is too slow to balance the robot. The problem could be in the circuits to L293D. Please go back to check it.
Step 8: Put All Together
If you can go to here withour any problem. Congratulation, you are almost there!!
Let’s put all the our efforts together. Please download and compile the auto balancing control program
$ git clone https://github.com/wennycooper/mySegway.git
$ cd mySegway
$ less README.md
$ gcc -o mySegway ./mySegway.c ./motors.c -lwiringPi -lpthread -lm
This program will try it's best to balance the robot. It measured the orientation infomation and drive the motors in 100 times per second. If the angle of orientation is too large (>60 degree) to be balanced, it will stop the motors for safety.
You can see the PID control in code. It is a very important method in automatic control theory. Check for more detail in the reference documents..
Step 9: Go!!
Because of the torgue and rotation speed limitation, the control program will fail to keep balancing if the error angle is too large. So, before running the program, please use your hand to keep the robot balanced as possible as you can.
My approach is having my left hand to balance the robot and having my right hand on keyboard to press the Enter key.
Due to each robot has different center of mass. It causes the initial balanced angle varies. That's why I have a "angle_offset" variable in code. Please modify it to fit your robot.
Here are some videos for demostration.
If the robot failed to keep balancing, the problem may be:
- The initial angle_offset is not good enough, please modify it and try more.
- The direction of driving motor and the direction of sensor measurement are the the same. You may modify it in the motors.c
- If motors go to the correct direction, but it still falled. It may be your motors not powerful enough or the angle_offset is not good.
- When start it, the initial angle is too large to be balanced. You may try it more.
- Unknown problems
- If it is balanced but not graceful, you may try to modify PID parameters.
Hope you all enjoy it!!
Step 10: References