// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include <Servo.h>
Servo firstESC, secondESC,thirdESC,fourthESC; //Create as much as Servoobject you want. You 
const int MPU=0x68;  // I2C address of the MPU-6050
int speed1=2000,speed2=0,speed3=0,speed4;
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float integ=0,der=0,pidx=0,kp = .5,ki=0.00005 ,kd=.01,prerror,dt=100;
void setup(){
  firstESC.attach(3);    // attached to pin 9 I just do this with 1 Servo 
  secondESC.attach(5);    // attached to pin 9 I just do this with 1 Servo 
  thirdESC.attach(6);    // attached to pin 9 I just do this with 1 Servo 
  fourthESC.attach(9);    // attached to pin 9 I just do this with 1 Servo 
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.write(0x1c);  // PWR_MGMT_1 register
  Wire.write(0<<3);     // set to zero (wakes up the MPU-6050)
void loop(){
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.requestFrom(MPU,4,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) 
    //speed1 = Serial.parseInt(); 
  //Serial.print("AcX = "); Serial.print(AcX);
  //Serial.print(" | AcY = "); Serial.print(AcY); 

void PIdD()
  float error;
  error = (atan2(AcY,AcZ)*180/3.14);
  now = millis(); 
  dt = now-ptime;
  else error = -(180+error); 
  integ = integ+(error*dt) ;
  der = (error - prerror)/dt ;
  pidx = (kp*error);
  ptime = now; 

The above is my program for my quadcopter, but now I have to tune the PID values, that is kp, ki, and kd. My accelome is at 2g. Please point to me what is wrong with the program? Is the error signal not appropriate? Please also give me or help me choose correct PID tuning. My limitation is I always have to connect my Arduino to pc and change kp ki or kd values, that is I have no remote control available currently.

  • $\begingroup$ Without a mathematical description of the system that you are trying to control, you cannot predict the optimal controller parameters, hence you need to find them empirically, which is indeed an iterative process. A quick way to modify the parameters is your actual problem, not finding the parameters. $\endgroup$ Sep 11 '15 at 14:51
  • $\begingroup$ and what about the error signal, is that correct for quadcopter case, $\endgroup$
    – Bilal Ayub
    Sep 11 '15 at 15:02
  • 1
    $\begingroup$ (1) Per the N1570 C standard, code from a compliant C compiler can evaluate the operands of | in either order. See eg §6.5.14. Thus, AcX=Wire.read()<<8|Wire.read(); is not well-defined. (2) Re atan(az/ay) mentioned in a comment, instead use atan2(az, ay) $\endgroup$ Sep 11 '15 at 18:46
  • $\begingroup$ What does this sentence mean: "my accelorem is 2g"? Have you already searched through stab exchange? There are numerous tutorials and example codes doing what you are trying to achieve, 1) control a system, 2) define/find optimum p I d constants . Good luck. $\endgroup$ Sep 12 '15 at 19:21
  • 1
    $\begingroup$ possible duplicate of What PID values should i keep $\endgroup$
    – Ian
    Sep 23 '15 at 20:05

Some things that are wrong in your program are:

You do not calculate your angular displacement properly, leading to a wrong error for the PID controller
You are only using one PID controller, for stabilization you need at least two
You just power two motors, why not all four?
You do not use a sampletime (the delay is commented). Your PID is likely to fail, regardless of the parameters.

you are trying to implement a digital PID controller. These type of controllers work best, when there is a specific time between the calculation of the controller. This time is usually called sampletime. The better you can keep this time constant the better the controller is able to perform. Usually this sampletime has to be short enought, that the controller is able to "see" distrubances. For your example the controller must be fast enought to see if the quadcopter has been tilted by the wind or similar things.

So basically for quadcopters you choose a control frequency of 50 Hz. For two reasons: First 20ms is fast enought to see disturbances and react on them. And the other point (a more technical one) ESC cannot be controlled faster then 50Hz, because they need a 20ms control signal (You create them with ESCxxx.WriteMicrosenconds())

The mistake in your implementation is that you do not have this timing factor(it is commented) so the controller is calculating new setpoints as fast as possible. As far as I know Arduinos it should be around 1-10kHz. This frequency is too high for your hardware and it is hard to tell if the timeing stays constant (at least I cannot). The I2C do not have to be constant in time. So your sampling frequency can change from iteration to iteration. This is usually the dead of a PID implementation. We (the team I'm working in in my company) invest a lot of work to keep samplingtime in controller implementation constant. You totally ignored this.

The parameters do not have to be multiplied or divided by any value, because they are not tuned at all. At the end: If you get your parameters tuned and you want to change the samplingtime, then you have to adjust those values. But you do not need to do this, in the untuned case.


Without using a sampletime your system can be "non-controllable" at all. This depends on lot of different factors like used hardware and quality of the electric connections and the ICs.
Use the sampletime of 20ms and tune your PID Parameter, those values depend on your hardware setup. There are no: Take these values and it's fine. You have to tune them and you there are tons of different methods. I recommend you to use an experimental iterative method (it's the best one, if you got no mathematical model or deeper controls knowledge). So guess some parameters and see how the system behaves. If is doing more or less nothing, then increase P. If it seem not controlled and bouncing up and down, then decrease the I. You will find lot of different (and well written) instructions online, which tell you how to tune PID controllers.

  • $\begingroup$ i am now using error signal as the atan(az/ay); that is the angle which finds the correct angle. so the 1st of your rocomendations is corrected i think. i am using one PID for debugging on axis only, other will be the same then. AND if i use delay of 100ms in pid function, then i will have to multiply integ with 0.01? i am not clear about sample time please help $\endgroup$
    – Bilal Ayub
    Sep 11 '15 at 15:38
  • $\begingroup$ mpu6050 gives the accelerometer values of constant sample time, that by burst reading the same time values are given $\endgroup$
    – Bilal Ayub
    Sep 11 '15 at 17:28
  • $\begingroup$ and the integral term in your controller this accumulate the error and therefore adjusting the output to the plant $\endgroup$
    – TobiasK
    Sep 11 '15 at 17:45
  • $\begingroup$ so what should i do. the error values are taken at same instant. so now what i do. $\endgroup$
    – Bilal Ayub
    Sep 11 '15 at 17:58
  • $\begingroup$ if i use no sample time, how will the system effect, $\endgroup$
    – Bilal Ayub
    Sep 11 '15 at 17:59

Not the answer you're looking for? Browse other questions tagged or ask your own question.