This guide was first posted at the Arduino forum, and can be found here: http://arduino.cc/forum/index.php/topic,58048.0.html
I just reposted it, so even more will get use of it. The code can be found here:
https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter
Hallo everybody
I recently bought this analog 6DOF (six degrees of freedom) IMU board (http://www.sparkfun.com/products/10010) from watterott.com. It uses three gyros and three accelerometers to calculate angles in three dimensions.
I looked a while for some code online and how to connect with them. After many hours of research I succeeded of making af precise measurement of angles in two directions. I decided to write a short guide for fellow electronic enthusiasts.
The main purpose of this guide is to teach others how to get some useful data from their IMU or just a gyro or accelerometer. The code for Arduino can be found at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter. It should be pretty easy to implement my code to your own sensor. I will not describe all the details about the theory behind, instead you can look at the sources for more info.
Before you begin you have to connect the IMU as follows:
Acc_Gyro Arduino
3.3V <—> 3.3V
GND <—> GND
Gx4 X <—> AN0
Gx4 Y <—> AN1
Gx4 Z <—> AN2
Acc X <—> AN3
Acc Y <—> AN4
Acc Z <—> AN5
Also connect 3.3V to the AREF pin on the Arduino for more accuracy.
It is VERY important that you do not connect the sensor to 5V - this will destroy the sensor.
Now your are ready for reading some data from the sensor.
To communicate with the sensor is straightforward:
The gyro measures degrees per second while the accelerometer measures acceleration (g's) in three dimensions. Both outputs the measurements as a analog signal.
To get these translated into degrees you have to do some coding:
The gyro
First you have to translate quids (a number from 0-1023) into something useful (this is for a ADC with a 10 bit resolution, for example this should be 4095 (2^12-1=4095) for 12 bit ADC). To do this I just use this simple equation:
gyroRate = (gyroAdc-gyroZero)/sensitivity - where gyroAdc are the readed value from our sensor, gyroZero is the value when it is stationary (this is done in the code - look in the "Setup" section) while sensitivity is the sensitivity found in the datasheet, but translated into quids.
If you look in the two gyros datasheets (http://www.sparkfun.com/datasheets/Sensors/IMU/lpr530al.pdf and http://www.sparkfun.com/datasheets/Sensors/IMU/LY530ALH.pdf) you will see that the sensitivity is 3.33mV/deg/s for the 4xOUT. To translate these into quids is pretty easy: sensitivity/3.3*1023.
So in this example I get:
0.00333/3.3*1023=1.0323.
NB: to translate mV to V simple just divide by one thousand.
The final equation will look like this:
gyroRate = (gyroAdc-gryoZero)/1.0323
The result will come out as degrees per second. To translate this into degrees you have to know the exact time since the last loop. Fortunately, the Arduino got a simple command to do so: millis(). By using that, one can calculate the time difference (delta time) and thereby calculate the angle of the gyro. The final equation will look like this:
gyroAngle += gyroRate*dtime/1000
Unfortunately, the gyro drifts over time. That means it can not be trusted for a longer timespan, but it is very precise for a short time. This is when the accelerometer comes in handy. It does not have any drift, but it is too unstable for shorter timespan. I will describe how to combine these measurements in a while, but first I will describe how to translate the readings from the accelerometer into something useful.
The accelerometer
The accelerometer measures the acceleration (g's) in three dimensions. To translate the analog readings into degrees you simply need to read the axis and to subtract the zero offset like so:
accVal = accAdc-accZero
Where accAdc is the analog reading and accZero is the value when it reads 0g - this is calculated in the start of the code, look in the "Setup" section. The zero value can also be found in the datasheet: http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf. You will see that the zero voltage at 0g is approximately 1.5V, to translate this into quids, you again have to use this equation: zeroVoltage/3.3*1023.
So in this example I get:
1.5/3.3*1023=465.
You can then calculate the pitch and roll using the following equations:
pitch = atan2(accYval, accZval)+PI
roll = atan2(accXval, accZval)+PI
Atan2 has a output range from -π to π (see http://en.wikipedia.org/wiki/Atan2), I simply add π, so the range it converted to 0 to 2π.
To convert it from radians to degrees we simply multiply the result by 57.295779513082320876798154814105 - this is predefined in the Arduino IDE as RAD_TO_DEG.
Kalman filter
As I explained earlier the gyro is very precise, but tend to drift. The accelerometer is a bit unstable, but does not drift. You can calculate the precise angle by using something called a Kalman filter. A detailed guide on how it's implemented can be found at my blog: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/.
If you want to use something a bit more simple, you can use what's called a Complementary Filter. It is pretty easy to understand and the math is much simpler, because it only works in one step.
For example the equation could look like this:
angle = 0.98 *(angle+gyro*dt) + 0.02*acc - you can fine tune the numbers to whatever you like. Just remember that the sum must be 1.
For me the result from the Complementary Filter was very close (or almost the same) as the one calculated by the Kalman filter.
You have now learned (hopefully) how to get analog data from IMU and translate it to something useful. I have attached my own code for my 6DOF IMU (http://www.sparkfun.com/products/10010), but with some slightly modification, I am pretty sure that it is possible to use it with any analog gyro/accelerometer.
If you have any question, fell free to post a comment below.
Sources:
http://www.instructables.com/id/Accelerometer-Gyro-Tutorial/
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418
http://www.x-firm.com/?page_id=148
http://web.mit.edu/first/segway/
Update
I have just finished a Processing code which prints out data from the Arduino on a nice graph. As you can see in the video below the filtering is quit effective. The light blue line is the accelerometer, the purple line is the gyro, the black line is the angle calculated by the Complementary Filter, and the red line is the angle calculated by the Kalman filter. As you might see the Kalman filter is just a bit more precise (i know it is difficult to see in the video) than the Complementary Filter, especially when I shake it.
I have attached my code, both the updated code for the Arduino and the Processing code. It is also possible to see the data from the y-axis. Just uncomment drawAxisY(); in the code.
Newest firmware
I decided to put all the source code on github, as it is much easier to maintain.
The newest code can now be found at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter
My Balancing robot
Below is a video of my balancing robot. It uses the same IMU and algorithm as described in the post above.
I just reposted it, so even more will get use of it. The code can be found here:
https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter
Hallo everybody
I recently bought this analog 6DOF (six degrees of freedom) IMU board (http://www.sparkfun.com/products/10010) from watterott.com. It uses three gyros and three accelerometers to calculate angles in three dimensions.
I looked a while for some code online and how to connect with them. After many hours of research I succeeded of making af precise measurement of angles in two directions. I decided to write a short guide for fellow electronic enthusiasts.
The main purpose of this guide is to teach others how to get some useful data from their IMU or just a gyro or accelerometer. The code for Arduino can be found at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter. It should be pretty easy to implement my code to your own sensor. I will not describe all the details about the theory behind, instead you can look at the sources for more info.
Before you begin you have to connect the IMU as follows:
Acc_Gyro Arduino
3.3V <—> 3.3V
GND <—> GND
Gx4 X <—> AN0
Gx4 Y <—> AN1
Gx4 Z <—> AN2
Acc X <—> AN3
Acc Y <—> AN4
Acc Z <—> AN5
Also connect 3.3V to the AREF pin on the Arduino for more accuracy.
It is VERY important that you do not connect the sensor to 5V - this will destroy the sensor.
Now your are ready for reading some data from the sensor.
To communicate with the sensor is straightforward:
The gyro measures degrees per second while the accelerometer measures acceleration (g's) in three dimensions. Both outputs the measurements as a analog signal.
To get these translated into degrees you have to do some coding:
The gyro
First you have to translate quids (a number from 0-1023) into something useful (this is for a ADC with a 10 bit resolution, for example this should be 4095 (2^12-1=4095) for 12 bit ADC). To do this I just use this simple equation:
gyroRate = (gyroAdc-gyroZero)/sensitivity - where gyroAdc are the readed value from our sensor, gyroZero is the value when it is stationary (this is done in the code - look in the "Setup" section) while sensitivity is the sensitivity found in the datasheet, but translated into quids.
If you look in the two gyros datasheets (http://www.sparkfun.com/datasheets/Sensors/IMU/lpr530al.pdf and http://www.sparkfun.com/datasheets/Sensors/IMU/LY530ALH.pdf) you will see that the sensitivity is 3.33mV/deg/s for the 4xOUT. To translate these into quids is pretty easy: sensitivity/3.3*1023.
So in this example I get:
0.00333/3.3*1023=1.0323.
NB: to translate mV to V simple just divide by one thousand.
The final equation will look like this:
gyroRate = (gyroAdc-gryoZero)/1.0323
The result will come out as degrees per second. To translate this into degrees you have to know the exact time since the last loop. Fortunately, the Arduino got a simple command to do so: millis(). By using that, one can calculate the time difference (delta time) and thereby calculate the angle of the gyro. The final equation will look like this:
gyroAngle += gyroRate*dtime/1000
Unfortunately, the gyro drifts over time. That means it can not be trusted for a longer timespan, but it is very precise for a short time. This is when the accelerometer comes in handy. It does not have any drift, but it is too unstable for shorter timespan. I will describe how to combine these measurements in a while, but first I will describe how to translate the readings from the accelerometer into something useful.
The accelerometer
The accelerometer measures the acceleration (g's) in three dimensions. To translate the analog readings into degrees you simply need to read the axis and to subtract the zero offset like so:
accVal = accAdc-accZero
Where accAdc is the analog reading and accZero is the value when it reads 0g - this is calculated in the start of the code, look in the "Setup" section. The zero value can also be found in the datasheet: http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf. You will see that the zero voltage at 0g is approximately 1.5V, to translate this into quids, you again have to use this equation: zeroVoltage/3.3*1023.
So in this example I get:
1.5/3.3*1023=465.
You can then calculate the pitch and roll using the following equations:
pitch = atan2(accYval, accZval)+PI
roll = atan2(accXval, accZval)+PI
Atan2 has a output range from -π to π (see http://en.wikipedia.org/wiki/Atan2), I simply add π, so the range it converted to 0 to 2π.
To convert it from radians to degrees we simply multiply the result by 57.295779513082320876798154814105 - this is predefined in the Arduino IDE as RAD_TO_DEG.
Kalman filter
As I explained earlier the gyro is very precise, but tend to drift. The accelerometer is a bit unstable, but does not drift. You can calculate the precise angle by using something called a Kalman filter. A detailed guide on how it's implemented can be found at my blog: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/.
If you want to use something a bit more simple, you can use what's called a Complementary Filter. It is pretty easy to understand and the math is much simpler, because it only works in one step.
For example the equation could look like this:
angle = 0.98 *(angle+gyro*dt) + 0.02*acc - you can fine tune the numbers to whatever you like. Just remember that the sum must be 1.
For me the result from the Complementary Filter was very close (or almost the same) as the one calculated by the Kalman filter.
You have now learned (hopefully) how to get analog data from IMU and translate it to something useful. I have attached my own code for my 6DOF IMU (http://www.sparkfun.com/products/10010), but with some slightly modification, I am pretty sure that it is possible to use it with any analog gyro/accelerometer.
If you have any question, fell free to post a comment below.
Sources:
http://www.instructables.com/id/Accelerometer-Gyro-Tutorial/
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418
http://www.x-firm.com/?page_id=148
http://web.mit.edu/first/segway/
Update
I have just finished a Processing code which prints out data from the Arduino on a nice graph. As you can see in the video below the filtering is quit effective. The light blue line is the accelerometer, the purple line is the gyro, the black line is the angle calculated by the Complementary Filter, and the red line is the angle calculated by the Kalman filter. As you might see the Kalman filter is just a bit more precise (i know it is difficult to see in the video) than the Complementary Filter, especially when I shake it.
I have attached my code, both the updated code for the Arduino and the Processing code. It is also possible to see the data from the y-axis. Just uncomment drawAxisY(); in the code.
Newest firmware
I decided to put all the source code on github, as it is much easier to maintain.
The newest code can now be found at github: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter
My Balancing robot
Below is a video of my balancing robot. It uses the same IMU and algorithm as described in the post above.
Visit Our Store »
Go Pro Today »
Are these the steps of PID Tuning?
dv=kp*angle+ki*integral+kd*diff;
integral+=angle;
diff=angle-prev_angle;
prev_angle=angle;
oc0=(int)dv;
You will need to find the Kp, Ki and Kd values.
once again! i'm here with you.
As i told you in my previous conversation, i was trying to make the self balancing robot using AVR platform but some of the problem.My robot was not working properly so again i need your help.
I got the PID result on my MATLAB Software but unfortunately,i'm not able to tune the graph of PID so I need your help to tune the parameter of Kp,Kd,Ki of PID to balance the robot.
if u know any software to direct tune PID using graph then it will be really helpful for me.
Also check out my Kickstarter campaign: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit. It's a complete kit with all you need to build a balancing robot.
The only difference between the MPU-6000 and the MPU-6050 is that the MPU-6000 supports SPI as well. So you can use my code just fine.
Your suggestions really helped!
You can see how to implement the integral term here: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L178.
Your suggestions really helped!
Your suggestions really helped!
Now that I can monitor the angle along the Y axis, am I ready to go for the balancing process? Almost all tutorials are using PID controller (I haven't used it before) and in my case I'm not using wheel encoders. Will the PID algorithms still applicable for my L293D motor driver?
Yes you are ready to build the robot. Yes you can still use a PID loop. I recommend getting some encoders for better performance.
the robot.I don't know why i'm feeling like with out gyroscope,it will not balance it self.
right now i'm taking the value of kd=12,ki=0.2 and kp=5.5.
as well as well i was increasing the value of ki then robot were becoming more stiff.
by Ziegler-Nochols method i tried many times but my robot is not working as a balance robot.
I've one doubt. Is it possible with out the wheel Encoder,robot will be balance?
I'm sorry. :( I've no idea of arduino as i told u before. but with the help of ur link, i got an idea about complementary filter :).The platform which i'm using to implement this code is CVAVR compiler and Atmega16 microcontroller.
now my problems are:
PROBLEM 1-how to get perfect value of kp,ki,kd according to my requirement?
Soln: as I was thinking with the help of complementary filter result,i got an angle.
now few steps of PID that i'm implementing:
angle=theta2; // theta2 which i got from complementary filter.
dv=kp*angle+ki*integral+kd*diff;
integral+=angle;
diff=-angle+prev_angle;
prev_angle=angle;
now the value of dv with the help of microcontroller pin.,I will display on CRO then i can check the perfect PID value by hit and trail method.
is it a good way?
Problem 2: After getting perfect value of PID,In which way should I implement the Pwm?
Your diff calculation is not correct, it should be:
diff=(angle-prev_angle).
Take a look at my PID routine: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L175-L181.
2. The output from your PID loop should be the input to the pwm routine. You can't either just use analogWrite or write directly to the registers as I did: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L389-L437.
Also take a look at this reply at my blog where I give a basic description of all the code: http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/comment-page-1/#comment-20112
so I'm here only using accelerometer because it is giving me precise reading. I need to require lit bit code info pls help me out.i'm using my code like this:
theta=(float)(atan((float)((float)(accz-accz0)/(float)(accx-accz0))))*180.0/(22.0/7.0); /* for measuring accelerometer angle*/
angle=theta; /*now i got angle*/
dv=kp*angle+ki*integral+kd*diff; /* here I'm taking PID kp=1.5,Kd=6.0,ki=0.000*/
integral+=angle;
diff=-angle+prev_angle;
prev_angle=angle;
oc0=(int)dv;
if(oc0>=0)
{
if(oc0>255){oc0=255;} // this will try to control motor rotation.
OCR0=(int)oc0;
PORTD.0=0;PORTD.1=1;
PORTD.2=0;PORTD.3=1;
}
else
{
if(oc0<-255){oc0=-255;}
OCR0=(int)(-oc0);
PORTD.0=1;PORTD.1=0;
PORTD.2=0;PORTD.3=1;
}
but my code is not working efficiently a lot jerk occurs and i'm confuse how to Set PID kp,kd,ki value exactly to my requirement?will it make help to balance my robot?
Btw I have made some modifications to the original guide, you can find the newest one at the Arduino forum: http://arduino.cc/forum/index.php/topic,58048.0.html.
Again take a look at my Balancing robot code: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino and read the comments at my blog: http://blog.tkjelectronics.dk/2012/03/the-balancing-robot/comment-page-1/#comments where you will find an answer to all your questions.
Gyroscope: http://www.e-gizmo.com/KIT/GYROSCOPE%20MODULE.html
Accelerometer :http://www.e-gizmo.com/KIT/3%20axis%20digital%20accelerometer.html
I'm not so sure about these items(especially the gyroscope). . .could you please take a look and help me decide. This is for my balancing robot. Thanks!
I've no idea about Arduino Board and it's coding so i'm confused how to combine the reading of Accelerometer and gyroscope? in various arduino codes i've seen the command millis() which measures the degrees per second (0/s). To use this command in my code i need to require Arduino board.so i'm confuse with out arduino board.it's not possible.now Do u know any other way by which i could make it with out using arduino board?
I was thinking in this way if i convert the sensors adc value in the form of angle by the coordinate system equations after that with the help of Complementary filter i will combine the both the angle in the form of single result then according to the leaning of the Robot,Pwm will work and that will try to balance it self but i need your help to implement it with out using arduino board.
Thank u for ur Support. :)
You could make a analog balancing robot. There is many out there. Just try to google it.
The code is available here: https://github.com/TKJElectronics/BalancingRobotArduino.
I have seen your balancing robot video below and I am also trying to build one but in my case a line follower. I have additional questions about this balancing robot:
1.) I'm currently in doubt now if 6V battery and 6V geared motor is sufficient enough for this project. In most cases 12V are used.
2.) The 'wheel encoder' thing, is it really necessary or just for the robot to go back in place when pushed at steady state?
3.) I already have two L293D motor drivers, can I still use those(what I'm trying to point is the performance) for as long as the current requirement of the motor is <= to the driver's current output?
Your response is really a great help for us beginners. Thanks for the reply above!
2) I really recommend getting some encoders as well. It improves the performance a lot.
3) It only has a peak output of 2A per channel (see the datasheet: http://www.ti.com/lit/ds/symlink/l293d.pdf), so make sure that your motors peak current doesn't exceed this. If they don't, then it will work just fine.
I am trying to build a self-balancing robot and I have several questions:
1.) I have a separate board of gyroscope and a digital accelerometer. In almost all guides that I have read, the VCC & other supply ports of the sensors are need to be connected in the microcontroller supply ports. Now that I have the two, how am I going to do that? I am using ATMEGA328. Would it be a different story if I'll connect either of the two sensors to an external supply? Will it affect the performance?
2.) I have this I2C output digital accelerometer (MMA8541 Capacitive Accelerometer,14-bit & 8 bit digital output). I'm familiar with the ADC but not with this 12C. How am I going to fetch the data in this form?
Hope you can help...Thanks!
You can see example on how to read from a digital IMU at the following link: https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF