@tutorial:
PID control with arduino
A proportional–integral–derivative controller (PID controller) is a control loop feedback mechanism commonly used in industrial control systems. A PID controller continuously calculates an error value e(t) as the difference between a desired setpoint and a measured process variable and applies a correction based on proportional, integral, and derivative terms (sometimes denoted P, I, and D respectively)
by: ELECTRONOOBS on 2026-09-07
In today project we will use this mechanism to control some brushless motor in order to calibrate our drone. We will put the motors on a balance and calculate the angle using the MPU6050 or the MPU9250 IMU module. So in our case the value that we will contro is the inclination angle of our drone. The e(t) error will be the difference between the raal angle of the drone and the desired one. The desired one will be 0, which means taht the drone is perfectly horizontal.
So let's say that you have already build the drone from the build your arduino drone project. But in that project we have use the multiwii platform for our flight controller. Now we want to make our own code to control the drone. But the drone won't fly steady unless we control each motor in a very precise way. That's why we have to learn how the PID control works. Each brushless motor will have a different power for the same PWM signal because motors are not perfect and they will never be the same. Fot that we have to constantly measure the angle of our drone, compare that value with the desired value and rectify the error if there is one.
So to tune our motors we will first build a balance for just 2 motors. This two motors represent just one axis, in this case will be the x-axis. As you know a drone can move in any of the 3D axis, x,y and z. All we want to do with this balance is to find our P, I and D constants. Each of this 3 constants will affect in one way or other the entire PID control, and we have to find the perfect ones. This is how we will proceed. For the balance we will need:
- 1 x 60cm (wood or metal)
- 2 x 80xm wood 2 x 22mm bearrings
- 1 x 20cm M8 threaded rod
- 4 x M8 nut
- 2 x regtangular piece of wood
- 1 x Big platform (recommended wood sheet)
- wood screws...
In the next photo we can see how to build our balance. Make a hole right in the middle of the 60cm bar and pass thge M8 rod through it. Add the bearrings and secure those with the M8 nuts, one on each side. Using two metal clamps attatch the bearrings to the two peaces of wood. Screw the entire systme to the big wood platform. Make sure that the entire system weighst more than 2Kg so the proppelers won't lift it. Add extra wood blocks to give it strenght. Also add some sponges or plastic band where the 60cm barr touches the wood in order to cushion the hit.
As you can see in the middle I've put a square sheet of wood . There is where I'll place the Arduino and the IMU module. Alos on each side, equaly distanced I've placed the two brushless motors with zip ties. The ESCs are tight in place using zip ties as well. Both ESCs share a commune 12 volts power suply with a male LiPo connector. To supply the entire circuit I've used an old PC power supply with a 12V output and 26A of current.
First of all place the brushless motors in the end of each side of the balance. Make sure that each of the motor is spinning in the direction shown in the schematic below. Also that the screw of the proppeller is geting thighten in the oposite direction as thew rotation, otherwise the propeller will fly away during the tests. If the motor is not spinning in the desired direction, just reverse teh top and bottom wires from the esc, leave the middle as it is. Make sure that the ESCs are calibrated and that both have the same scale. In this case this this ESCs both have a scale from 1000us to 2000us. If you don't know how to calibrate the esc watch this tutorial and use that codeto calibrate the ESCs. This is inportant. If the motors don't have the same scale, this system won't work.
Now we should have the schematic above. Supply 12V to the ESCs. From the ESCs BEC supply 5V to the arduino or use the USB to supply it if you want. Connect the IMU using the i2c connection pins SCL and SDA. In the schematic below you can see with better details the mpu6050 connection to the arduino. Then connect pin 3 to the right ESC and pin 5 to the left one. Aslo connect gnd between the arduino and the ESCs.
Place the MPU6050 module as centered as you can on the balance. I've use a breadboard to make all the connections. Alos I recommend you to wind each of the i2c cables around a GND cable. This will reduce noise of the i2c comunnicatin. For that get two wires of GND from the arduino to the MPU6050 and wire around each of this the SCL and SDA cables of the i2c. To equal the weight on each side I've used a magnet and place it closer or farther of teh center depending of the weight.Now that everything is ready to go we can start with the code.
The code is not that complicated. We know that we have to read some data from the IMU (inetrial movement unit) module. Using that data we calculate the real inclination angle of the axis. We will use just one axis for the balance, the x-axis. We have to comparte that angle with the desired one which is 0º because we whant the drone to be horizontal. To contol the brushless motors we have to send a PWM signal to each ESC with a pulse with between 1000us and 2000us because that is the range of the ESCs that we've calibrated in this tutorial.
We will divide the code in a few parts. I will try to explain the best I can each part. But first we have to know a little bit of mathematic. We will use some fomulas to calculate the angles. This are called Euler formulas. Go to the next page and continue with the tutorial.
Before we begin, what is an IMU? It is a device capable of measuring the force (acceleration) and speed. Generically it consists of an Accelerometer and a Gyroscope. Therefore: an IMU does not measure angles. At least not directly, it requires some calculations in order to obtain the angles. The MPU-6050 is an IMU of 6DOF (reads "6 Degrees Of Freedom"). This means that it has an accelerometer and a gyroscope, both of 3 axes (3 + 3 = 6DOF). There are 9DOF IMUs, in this case they also carry a magnetometer. Others may have 5DOF, in which case the gyroscope only measures two axes.
-The MPU-6050 operates with 3.3 volts, although some versions (like mine) have a regulator that allows to connect it to 5V.
-The MPU-6050 uses the I2C communication protocol. In this tutorial I will not go into details of how this protocol works, just make sure to connect the SCD (data) and SCL (clock) pins between the module and the microcontroller.
The accelerometer measures the acceleration. Who was going to say taht?¿. We will have the acceleration of the 3 axes: X, Y and Z, the three dimensions of our 3D space. Let's just call for example, the IMU movemnet upwards, the Z axis, forward and backward the Y axis and side to side the X axis. If you remember feom first grade (or equivalent) classes, you will remember that Earth's gravity has an acceleration of approx. 9.8 m / s², perpendicular to the ground. Therefore, the IMU also detects the acceleration of terrestrial gravity. But the values that we get from the IMU won't be in "g" units. This module works with 8 bits registers. Each acceleration value is stored in two registers, low and high bits. The sum of this registers give us 16 bits of data. This data will have a maximum of 16 bits so we will have a 2^16=65536 maximum acceleration valueincluding the positive or negative sign. If we read the datasheet of this module we will find that "1g" equals to 16384. So if we read the acceleration value and divide that value to 16384 we obtain the acceleration in "g" units.
Thanks to terrestrial gravity you can use the accelerometer readings to know the angle of inclination with respect to the X axis or Y axis.
Suppose the IMU is perfectly aligned with the floor. Then, as you can see in the picture below, the Z axis will be 1g=9.8, and the other two axes will be 0. Now suppose we turn the IMU 90 degrees. Now it is the X axis that is perpendicular to the ground, therefore it will mark the acceleration of 1g gravity.
We know that the gravity is 9.8 m / s², and we know how to measure the three axes of the accelerometer, by trigonometry it is possible to calculate the angle of inclination of the IMU. The formulas to calculate the angles are:
Since the angle is calculated from gravity, it is not possible to calculate the angle Z with this formula or with any other. To do this you need another component: the magnetometer, which is a type of digital compass. The MPU-6050 does not carry a magnetometer, and therefore can never accurately calculate the angle Z. However, for the vast majority of applications only the X and Y axes are needed.
At first, electric gyroscopes were bulky artifacts that were worth most of a state's military budget. Later, during the Second World War they were used to direct rockets and torpedoes. Luckily, thanks to the digital revolution and the miniaturization of circuits, nowadays any fan of electronics can afford one. Although not to build missiles.
The gyroscope measures the angular velocity. If you do not have very fresh your institute physics lessons I will remind you that the angular velocity is the number of degrees that is rotated in a second so our units will be degrees/second.
If we know the initial angle of the IMU, we can add the value that marks the gyroscope to know the new angle at each moment. Suppose we start the IMU at 0 °. If the gives us out a measurement every second, and marks 3 on the X axis, we will have the angle with this simple formula:
Where elapsedTime is the time that elapses each time this formula is calculated inside of the loop, PreviousAngle is the angle calculated the last time this formula was called and GyroData is the reading of the angle Y or X of the gyroscope. And the same thing happens with all the axes. Only the Z axis is usually ignored, since when you can not calculate an Z-angle with the Accelerometer, you can not apply a Supplementary Filter for the Z-axis. We already have the readings. Simple, right? Well, not really.
In an ideal world where fairies and elves sing joyous and sweet songs around bonfires, where dragons cross the skies, trees speak with mushrooms and C ++ does not cause errors at runtime, IMUs are magical artifacts that with a little Of trigonometry can give an angle with total accuracy. But you're in the real world, son. And there are two very important problems: noise and errors. Noise is all that interferes with electronic devices. The accelerometer is able to measure any angle, however its readings are noisy and have a certain margin of error. If you ever want to draw a graph of the measurements of an accelerometer as a function of time, you will see something of this style:
As you can see the raw acceleration data acts like crazy. Even with a low pass filter there would still be peacks and noise. And if we add the angle obtain from the gyro this would be even more crazy. If we obtain the angle using euler formulas and this acceleration data the angle won't be precise in eny moment. For that we should use filters and make a combination between gyro and acceleration data.
The idea is very simple: you have to eliminate the noise, the drift and get the accelerometer to not change its angle when it detects a force other than gravity. There are different algorithms, called filters, that do this task. One of the best is the famous Kálman Filter, which you may have heard of. It is used in aircraft, rockets and geostationary satellites. Kálman's filter is surprising. It is considered one of the greatest finds of the last century, and rightly so. Able to calculate the error of each measurement from the previous measurements, remove it and give the actual value of the angle. In a few words is an algorithm that learns in each iteration. It does not sound too bad, does it?
However it has two problems:
-It has a somewhat high processing cost
-It is very complicated to understand. And I emphasize the "very".
So, we have other filters at our disposal. The one we are going to use is known as Complementary Filter. It is ideal to implement with Arduino: easy to use, low cost of processing and with a very good precision. What exactly is it? The Complementary Filter is actually a union of two different filters: a High-pass Filter for the gyroscope and a Low-pass Filter for the Accelerometer. The first lets only pass the values ??above a certain limit, unlike the Low-pass filter, which only allows those below. The formula resulting from combining (complementing, hence the name) the two filters is:
It's not that complicate right? It is just 98% from the angle obtained with the gyro data and a plus of 2% of the angle obtained with the acceleration data. As you can see for the total angle X we sum the gyto data Y. That is because the inclination angle of the X axis is given by the Y axis rotation. Now we have our angles. After applying the filters we should get something like the graph below which is closer to the reality.
Now we can clearly see the yellow real representation of the angle. This is way better than the raw data. Now all we need to do is to use this formulas in our arduino code to obtain the angle. So let's start with the code.
The connections are very simple. With the USB 5V you will have plenty of power.
MPU Vcc -> Arduino 5v (or 3v3 on some models. Make sure which is yours.)
MPU Gnd -> Arduino Gnd
MPU SCL -> Arduino A5
MPU SDA Arduino A4
IMPORTANT! These connections are tested in an Arduino UNO and Arduino Nano, whose pins SDA and SCL are A4 and A5 respectively. If you use another plate may be other pins. So you have the formulas. You have the IMU. You have an Arduino board. You have the circuit. Missing? Know how to interact with IMU to get readings. The MPU-6050 will give raw values that will then have to be refined (divide by a constant) in order to obtain usable values. The official Arduino website has an example to read data from the MPU-6050. I will depart from it, although it will have to be modified considerably since it only gives the unrefined values ??of the Gyroscope and the Accelerometer.
So let's start explaining the code. In the end we will have the complete code ready to download or copy. IF you want to skip the explination just download the code from the next link:
#include <Wire.h>
#include <Servo.h>
Servo right_prop;
Servo left_prop;
/*MPU-6050 gives you 16 bits data so you have to create some 16int constants
* to store the data for accelerations and gyro*/
int16_t Acc_rawX, Acc_rawY, Acc_rawZ,Gyr_rawX, Gyr_rawY, Gyr_rawZ;
float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];
float elapsedTime, time, timePrev;
int i;
float rad_to_deg = 180/3.141592654;
float PID, pwmLeft, pwmRight, error, previous_error;
float pid_p=0;
float pid_i=0;
float pid_d=0;
/////////////////PID CONSTANTS/////////////////
double kp=3.55;//3.55
double ki=0.005;//0.003
double kd=2.05;//2.05
///////////////////////////////////////////////
double throttle=1300; //initial value of throttle to the motors
float desired_angle = 0; //This is the angle in which we whant the
//balance to stay steady
void setup() {
Wire.begin(); //begin the wire comunication
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(250000);
right_prop.attach(3); //attatch the right motor to pin 3
left_prop.attach(5); //attatch the left motor to pin 5
time = millis(); //Start counting time in milliseconds
/*In order to start up the ESCs we have to send a min value
* of PWM to them before connecting the battery. Otherwise
* the ESCs won't start up or enter in the configure mode.
* The min value is 1000us and max is 2000us, REMEMBER!*/
left_prop.writeMicroseconds(1000);
right_prop.writeMicroseconds(1000);
delay(7000); /*Give some delay, 7s, to have time to connect
*the propellers and let everything start up*/
}//end of setup void
void loop() {
/////////////////////////////I M U/////////////////////////////////////
timePrev = time; // the previous time is stored before the actual time read
time = millis(); // actual time read
elapsedTime = (time - timePrev) / 1000;
/*The tiemStep is the time that elapsed since the previous loop.
* This is the value that we will use in the formulas as "elapsedTime"
* in seconds. We work in ms so we haveto divide the value by 1000
to obtain seconds*/
/*Reed the values that the accelerometre gives.
* We know that the slave adress for this IMU is 0x68 in
* hexadecimal. For that in the RequestFrom and the
* begin functions we have to put this value.*/
Wire.beginTransmission(0x68);
Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX
Wire.endTransmission(false);
Wire.requestFrom(0x68,6,true);
/*We have asked for the 0x3B register. The IMU will send a brust of register.
* The amount of register to read is specify in the requestFrom function.
* In this case we request 6 registers. Each value of acceleration is made out of
* two 8bits registers, low values and high values. For that we request the 6 of them
* and just make then sum of each pair. For that we shift to the left the high values
* register (<<) and make an or (|) operation to add the low values.*/
Acc_rawX=Wire.read()<<8|Wire.read(); //each value needs two registres
Acc_rawY=Wire.read()<<8|Wire.read();
Acc_rawZ=Wire.read()<<8|Wire.read();
/*///This is the part where you need to calculate the angles using Euler equations///*/
/* - Now, to obtain the values of acceleration in "g" units we first have to divide the raw
* values that we have just read by 16384.0 because that is the value that the MPU6050
* datasheet gives us.*/
/* - Next we have to calculate the radian to degree value by dividing 180º by the PI number
* which is 3.141592654 and store this value in the rad_to_deg variable. In order to not have
* to calculate this value in each loop we have done that just once before the setup void.
*/
/* Now we can apply the Euler formula. The atan will calculate the arctangent. The
* pow(a,b) will elevate the a value to the b power. And finnaly sqrt function
* will calculate the rooth square.*/
/*---X---*/
Acceleration_angle[0] = atan((Acc_rawY/16384.0)/sqrt(pow((Acc_rawX/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
/*---Y---*/
Acceleration_angle[1] = atan(-1*(Acc_rawX/16384.0)/sqrt(pow((Acc_rawY/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
/*Now we read the Gyro data in the same way as the Acc data. The adress for the
* gyro data starts at 0x43. We can see this adresses if we look at the register map
* of the MPU6050. In this case we request just 4 values. W don¡t want the gyro for
* the Z axis (YAW).*/
Wire.beginTransmission(0x68);
Wire.write(0x43); //Gyro data first adress
Wire.endTransmission(false);
Wire.requestFrom(0x68,4,true); //Just 4 registers
Gyr_rawX=Wire.read()<<8|Wire.read(); //Once again we shif and sum
Gyr_rawY=Wire.read()<<8|Wire.read();
/*Now in order to obtain the gyro data in degrees/seconda we have to divide first
the raw value by 131 because that's the value that the datasheet gives us*/
/*---X---*/
Gyro_angle[0] = Gyr_rawX/131.0;
/*---Y---*/
Gyro_angle[1] = Gyr_rawY/131.0;
/*Now in order to obtain degrees we have to multiply the degree/seconds
*value by the elapsedTime.*/
/*Finnaly we can apply the final filter where we add the acceleration
*part that afects the angles and ofcourse multiply by 0.98 */
/*---X axis angle---*/
Total_angle[0] = 0.98 *(Total_angle[0] + Gyro_angle[0]*elapsedTime) + 0.02*Acceleration_angle[0];
/*---Y axis angle---*/
Total_angle[1] = 0.98 *(Total_angle[1] + Gyro_angle[1]*elapsedTime) + 0.02*Acceleration_angle[1];
/*Now we have our angles in degree and values from -10º0 to 100º aprox*/
//Serial.println(Total_angle[1]);
/*///////////////////////////P I D///////////////////////////////////*/
/*Remember that for the balance we will use just one axis. I've choose the x angle
to implement the PID with. That means that the x axis of the IMU has to be paralel to
the balance*/
/*First calculate the error between the desired angle and
*the real measured angle*/
error = Total_angle[1] - desired_angle;
/*Next the proportional value of the PID is just a proportional constant
*multiplied by the error*/
pid_p = kp*error;
/*The integral part should only act if we are close to the
desired position but we want to fine tune the error. That's
why I've made a if operation for an error between -2 and 2 degree.
To integrate we just sum the previous integral value with the
error multiplied by the integral constant. This will integrate (increase)
the value each loop till we reach the 0 point*/
if(-3 <error <3)
{
pid_i = pid_i+(ki*error);
}
/*The last part is the derivate. The derivate acts upon the speed of the error.
As we know the speed is the amount of error that produced in a certain amount of
time divided by that time. For taht we will use a variable called previous_error.
We substract that value from the actual error and divide all by the elapsed time.
Finnaly we multiply the result by the derivate constant*/
pid_d = kd*((error - previous_error)/elapsedTime);
/*The final PID values is the sum of each of this 3 parts*/
PID = pid_p + pid_i + pid_d;
/*We know taht the min value of PWM signal is 1000us and the max is 2000. So that
tells us that the PID value can/s oscilate more than -1000 and 1000 because when we
have a value of 2000us the maximum value taht we could sybstract is 1000 and when
we have a value of 1000us for the PWM sihnal, the maximum value that we could add is 1000
to reach the maximum 2000us*/
if(PID < -1000)
{
PID=-1000;
}
if(PID > 1000)
{
PID=1000;
}
/*Finnaly we calculate the PWM width. We sum the desired throttle and the PID value*/
pwmLeft = throttle + PID;
pwmRight = throttle - PID;
/*Once again we map the PWM values to be sure that we won't pass the min
and max values. Yes, we've already maped the PID values. But for example, for
throttle value of 1300, if we sum the max PID value we would have 2300us and
that will mess up the ESC.*/
//Right
if(pwmRight < 1000)
{
pwmRight= 1000;
}
if(pwmRight > 2000)
{
pwmRight=2000;
}
//Left
if(pwmLeft < 1000)
{
pwmLeft= 1000;
}
if(pwmLeft > 2000)
{
pwmLeft=2000;
}
/*Finnaly using the servo function we create the PWM pulses with the calculated
width for each pulse*/
left_prop.writeMicroseconds(pwmLeft);
right_prop.writeMicroseconds(pwmRight);
previous_error = error; //Remember to store the previous error.
}//end of loop void
First of all we include the wire and servo libraries. We need the servo.h to create the PWM signal for our ESCs. The wire library will establish a i2c communication with the MPU6050 module. Next we create our servo variables for our two motors, left and right.
MPU-6050 gives you 16 bits data so you have to create some 16int constants to store the data for accelerations and gyro. The data is divided in 2 8bits registers. We will have to sum each pair to obtain the full value.
Now we define all the global variables that we're going to use. We ussualy use float variables because we work with real values as well. In the PID constants part is where we will later change the values in order to adapt the program to our system. Each drone will be different so if this code works for me that doesn't mean that it will work with yours. Any way, we will see how to tune the PID constants later. Lets start the setup void.
As the code comments say, we begin the i2c comunication and we asign the pins 3 and 5 as our PWM signal outputs. Before all we have to measure the initial time that passed till now. We will use this varible later to calculate the elapsedTime in each loop. The ESCs won't start up if we don't send the min value to them when the LiPo battery or the power supply is connected so for that we send first a 1000us PWM signal to each, because that is the min value that we've deifned when calibrating in this tutorial.
We start the loop and the first thing that we do is calculate the elapsedTime. The elapsedTime is the time that elapsed since the previous loop. This is the value that we will use in the formulas as "elapsedTime" in seconds. We work in ms so we have to divide the value by 1000 to obtain seconds. Now in the next page we can start with the fuul code explained.
Here is the full code for the PID control for our balance one axis brushless motors. Copy it and read all the comments in order to understand each part. make the connections as we've seen in tha last pages and upload the code. Next we will start calibrating the PID constants so stay tuned for that.
#include <Wire.h>
#include <Servo.h>
Servo right_prop;
Servo left_prop;
/*MPU-6050 gives you 16 bits data so you have to create some 16int constants
* to store the data for accelerations and gyro*/
int16_t Acc_rawX, Acc_rawY, Acc_rawZ,Gyr_rawX, Gyr_rawY, Gyr_rawZ;
float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];
float elapsedTime, time, timePrev;
int i;
float rad_to_deg = 180/3.141592654;
float PID, pwmLeft, pwmRight, error, previous_error;
float pid_p=0;
float pid_i=0;
float pid_d=0;
/////////////////PID CONSTANTS/////////////////
double kp=3.55;//3.55
double ki=0.005;//0.003
double kd=2.05;//2.05
///////////////////////////////////////////////
double throttle=1300; //initial value of throttle to the motors
float desired_angle = 0; //This is the angle in which we whant the
//balance to stay steady
void setup() {
Wire.begin(); //begin the wire comunication
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(250000);
right_prop.attach(3); //attatch the right motor to pin 3
left_prop.attach(5); //attatch the left motor to pin 5
time = millis(); //Start counting time in milliseconds
/*In order to start up the ESCs we have to send a min value
* of PWM to them before connecting the battery. Otherwise
* the ESCs won't start up or enter in the configure mode.
* The min value is 1000us and max is 2000us, REMEMBER!*/
left_prop.writeMicroseconds(1000);
right_prop.writeMicroseconds(1000);
delay(7000); /*Give some delay, 7s, to have time to connect
*the propellers and let everything start up*/
}//end of setup void
void loop() {
/////////////////////////////I M U/////////////////////////////////////
timePrev = time; // the previous time is stored before the actual time read
time = millis(); // actual time read
elapsedTime = (time - timePrev) / 1000;
/*The tiemStep is the time that elapsed since the previous loop.
* This is the value that we will use in the formulas as "elapsedTime"
* in seconds. We work in ms so we haveto divide the value by 1000
to obtain seconds*/
/*Reed the values that the accelerometre gives.
* We know that the slave adress for this IMU is 0x68 in
* hexadecimal. For that in the RequestFrom and the
* begin functions we have to put this value.*/
Wire.beginTransmission(0x68);
Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX
Wire.endTransmission(false);
Wire.requestFrom(0x68,6,true);
/*We have asked for the 0x3B register. The IMU will send a brust of register.
* The amount of register to read is specify in the requestFrom function.
* In this case we request 6 registers. Each value of acceleration is made out of
* two 8bits registers, low values and high values. For that we request the 6 of them
* and just make then sum of each pair. For that we shift to the left the high values
* register (<<) and make an or (|) operation to add the low values.*/
Acc_rawX=Wire.read()<<8|Wire.read(); //each value needs two registres
Acc_rawY=Wire.read()<<8|Wire.read();
Acc_rawZ=Wire.read()<<8|Wire.read();
/*///This is the part where you need to calculate the angles using Euler equations///*/
/* - Now, to obtain the values of acceleration in "g" units we first have to divide the raw
* values that we have just read by 16384.0 because that is the value that the MPU6050
* datasheet gives us.*/
/* - Next we have to calculate the radian to degree value by dividing 180º by the PI number
* which is 3.141592654 and store this value in the rad_to_deg variable. In order to not have
* to calculate this value in each loop we have done that just once before the setup void.
*/
/* Now we can apply the Euler formula. The atan will calculate the arctangent. The
* pow(a,b) will elevate the a value to the b power. And finnaly sqrt function
* will calculate the rooth square.*/
/*---X---*/
Acceleration_angle[0] = atan((Acc_rawY/16384.0)/sqrt(pow((Acc_rawX/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
/*---Y---*/
Acceleration_angle[1] = atan(-1*(Acc_rawX/16384.0)/sqrt(pow((Acc_rawY/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
/*Now we read the Gyro data in the same way as the Acc data. The adress for the
* gyro data starts at 0x43. We can see this adresses if we look at the register map
* of the MPU6050. In this case we request just 4 values. W don¡t want the gyro for
* the Z axis (YAW).*/
Wire.beginTransmission(0x68);
Wire.write(0x43); //Gyro data first adress
Wire.endTransmission(false);
Wire.requestFrom(0x68,4,true); //Just 4 registers
Gyr_rawX=Wire.read()<<8|Wire.read(); //Once again we shif and sum
Gyr_rawY=Wire.read()<<8|Wire.read();
/*Now in order to obtain the gyro data in degrees/seconda we have to divide first
the raw value by 131 because that's the value that the datasheet gives us*/
/*---X---*/
Gyro_angle[0] = Gyr_rawX/131.0;
/*---Y---*/
Gyro_angle[1] = Gyr_rawY/131.0;
/*Now in order to obtain degrees we have to multiply the degree/seconds
*value by the elapsedTime.*/
/*Finnaly we can apply the final filter where we add the acceleration
*part that afects the angles and ofcourse multiply by 0.98 */
/*---X axis angle---*/
Total_angle[0] = 0.98 *(Total_angle[0] + Gyro_angle[0]*elapsedTime) + 0.02*Acceleration_angle[0];
/*---Y axis angle---*/
Total_angle[1] = 0.98 *(Total_angle[1] + Gyro_angle[1]*elapsedTime) + 0.02*Acceleration_angle[1];
/*Now we have our angles in degree and values from -10º0 to 100º aprox*/
//Serial.println(Total_angle[1]);
/*///////////////////////////P I D///////////////////////////////////*/
/*Remember that for the balance we will use just one axis. I've choose the x angle
to implement the PID with. That means that the x axis of the IMU has to be paralel to
the balance*/
/*First calculate the error between the desired angle and
*the real measured angle*/
error = Total_angle[1] - desired_angle;
/*Next the proportional value of the PID is just a proportional constant
*multiplied by the error*/
pid_p = kp*error;
/*The integral part should only act if we are close to the
desired position but we want to fine tune the error. That's
why I've made a if operation for an error between -3 and 3 degree.
To integrate we just sum the previous integral value with the
error multiplied by the integral constant. This will integrate (increase)
the value each loop till we reach the 0 point*/
if(-3 <error <3)
{
pid_i = pid_i+(ki*error);
}
/*The last part is the derivate. The derivate acts upon the speed of the error.
As we know the speed is the amount of error that produced in a certain amount of
time divided by that time. For taht we will use a variable called previous_error.
We substract that value from the actual error and divide all by the elapsed time.
Finnaly we multiply the result by the derivate constant*/
pid_d = kd*((error - previous_error)/elapsedTime);
/*The final PID values is the sum of each of this 3 parts*/
PID = pid_p + pid_i + pid_d;
/*We know taht the min value of PWM signal is 1000us and the max is 2000. So that
tells us that the PID value can/s oscilate more than -1000 and 1000 because when we
have a value of 2000us the maximum value taht we could sybstract is 1000 and when
we have a value of 1000us for the PWM sihnal, the maximum value that we could add is 1000
to reach the maximum 2000us*/
if(PID < -1000)
{
PID=-1000;
}
if(PID > 1000)
{
PID=1000;
}
/*Finnaly we calculate the PWM width. We sum the desired throttle and the PID value*/
pwmLeft = throttle + PID;
pwmRight = throttle - PID;
/*Once again we map the PWM values to be sure that we won't pass the min
and max values. Yes, we've already maped the PID values. But for example, for
throttle value of 1300, if we sum the max PID value we would have 2300us and
that will mess up the ESC.*/
//Right
if(pwmRight < 1000)
{
pwmRight= 1000;
}
if(pwmRight > 2000)
{
pwmRight=2000;
}
//Left
if(pwmLeft < 1000)
{
pwmLeft= 1000;
}
if(pwmLeft > 2000)
{
pwmLeft=2000;
}
/*Finnaly using the servo function we create the PWM pulses with the calculated
width for each pulse*/
left_prop.writeMicroseconds(pwmLeft);
right_prop.writeMicroseconds(pwmRight);
previous_error = error; //Remember to store the previous error.
}//end of loop voidTo calibrate the PID constants we start with Kp = 1. This will give a proportional response to the system. That means if the angle is -30º it's logical to give 30 throttle more to the left side and 30 less to the right. This will make the system oscilates from left to right and viceversa. If with Kp = 1 it dosn't oscilates we can increase the Kp. When we have a quite good result we can add the D constant. The I is not that important. There are systems that only uses PD instead of PID. The D is the constant that acts upon the speed of the error change. It reacts very past. Increase the D value till you get a good result.
Comments
Leave a comment
Please login in order to comment.