I am a beginner.. I am trying to control RC servo using Arduino based of data from MPU6050. I am using library I2Cdev to communicate with gyro and to get yaw, pitch and roll. I am using example script which I have modified to control servo based on received data.

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

Serial.print(ypr[0] * 180 / M_PI);
Serial.print(ypr[1] * 180 / M_PI);
Serial.println(ypr[2] * 180 / M_PI);

pitch_angle = (ypr[1] * 180 / M_PI) + 90;
roll_angle = (ypr[2] * 180 / M_PI) + 90;   



Without servo being connected everything works well. But after I plug in servo after few second being still (it seems to even follow movement of gyro) it starts to shake and then probably whole Arduino stops working (serial transmission of data stops) and Arduino has to be restarted.

100 95
ypr 12.21   10.51   5.49
100 95
ypr 12.23   10.31   5.48
100 95
ypr 12.25   10.15   5.45
100 95
ypr 12.27   10.02   5.45
100 95
ypr -13.31  30.81   55.76
120 145
ypr 20.53   -21.36  -52.59
68  37
ypr 64.96   35.38   10.25
125 100

I tried to plug two 100 nF capacitors across power and ground of the servo and I tried to connect servo in to different power source than Arduino.

EDIT: Servo is not fixed with MPU6050 so there shouldn't be any resonance at least on the mechanical base. Wiring is very simple:

enter image description here

Any idea? Thanks

  • Is the mpu6050 mounted on the servos or in a fixed position? Commented Jun 30, 2015 at 2:56
  • Please show your wiring. If it stops when the servo is connected it sounds like it is not getting enough power. If you tried a different power source, did you connect the grounds together?
    – Nick Gammon
    Commented Jun 30, 2015 at 3:12
  • I have just edit my question.. Thanks for effort.
    – Jakub Jon
    Commented Jun 30, 2015 at 6:04
  • As far as I can see from the photo, if you have the different power source connected (out of photo) it is also connected to the Arduino power (it is hard to tell). The external power should go to the servo only, not the Arduino. However the grounds need to be connected.
    – Nick Gammon
    Commented Jun 30, 2015 at 6:34
  • sorry for the photo - there is no external power source .. breadboard is connected straightly to the arduino
    – Jakub Jon
    Commented Jun 30, 2015 at 12:58

2 Answers 2


At the end two pull-up resistors and one capacitor solved the problem.

I followed instruction here:


Now it works well.. Thank you all.

  • Just FYI that particular mod is unique to what clone 521 board you have. Some clone makers (all that I have received) chose better pull up resistor values to begin with.
    – user6569
    Commented Jul 8, 2015 at 22:32

So a couple of ideas.

1) you need to be real careful with the MPU and overflows. You do not show all your code, but you must check for overflows, and ignore the data if one has occurred. I see in your print a stray negative value, that tells me you are not skipping bad data.

2) I dont know how 'tight' your loop is. You need to service the MPU a lot. In fact the sample you used, uses an interrupt to do so. I think the default update rate is about 100 times per second. That may be too fast to make large changes in servo positions. You will need to think about moving slower from point A to B if the difference is more than the servo can handle at that speed.

3) You are adding 90. With the numbers you have shown that is probably ok, but in your final code you need to be careful you do not exceed a servos range, 0 to 180. the map() function is probably something you will want to use, instead of just adding 90.

4) My guess is that since it works for a second or 2, it is indeed over flowing data, and then goes to heck. Especially since you do not skip errors.

5) it takes the 6050 30 seconds or so to settle - so you will see large gyro drift (including PitchRollYaw) until it does. Probably not what your code is expecting.

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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