I am using Arduino Uno, with PID library and Jeff Rowberg library for MPU6050 i.e I2Cdev.h
, MPU6050_6Axis_MotionApps20.h
.
There is one PID for each axis i.e Pitch and Roll, I am getting output that wobbles and is unstable. I don't know whether my code is wrong or something else.
Errors which I feel are:
- I am using
Esc.write()
, I know Microseconds should be used but it feels very light, even it doesn't wobble. - Kalman Filter not used, but I am getting the pitch, roll, yaw.
- PID library (By Brett Beauregard) not suitable for Quadcopter.
This statement in loop making loop slow, it's written in its comment and maybe it making pid compute upsetelse if (mpuIntStatus & 0x02) // wait for correc available data length, should be a VERY short wait while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
Please look at code.
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
yaw = ypr[0] * 180/M_PI;
pitch = ypr[1] * 180/M_PI;
roll = ypr[2] * 180/M_PI;
Input = pitch;
RInput = roll;
myPID.SetTunings(Kp,Ki,Kd);
myPIDR.SetTunings(RKp,RKi,RKd);
//Sample Time is 10 for both PID.
myPID.Compute();
myPIDR.Compute();
if(armed == 1) {
int m1 = throttle.toInt() + Output;
int m2 = throttle.toInt() + ROutput;
int m3 = throttle.toInt() - Output;
int m4 = throttle.toInt() - ROutput;
int angle1 = map(m1, 0, 100, 0, 180);
ESC1.write(angle1); //FRONT PITCH+
int angle2 = map(m2, 0, 100, 0, 180);
ESC2.write(angle2); //LEFT ROLL+
int angle3 = map(m3, 0, 100, 0, 180);
ESC3.write(angle3); //BACK PITCH-
int angle4 = map(m4, 0, 100, 0, 180);
ESC4.write(angle4); //RIGHT ROLL-
}
#endif
Kp = 0.25, Ki = 0.04, Kd = 0.05 | RKp = 0.25, RKi = 0.04, RKd = 0.05
From testing: if I shake my Quadcopter fast by hand it behaves like PID is activated and works as wanted, but suddenly after a couple of seconds again feel loose, may be PID compute become upset