RC servo misbehave using arduino and MPU6050 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(&amp;q, fifoBuffer);
mpu.dmpGetGravity(&amp;gravity, &amp;q);
mpu.dmpGetYawPitchRoll(ypr, &amp;q, &amp;gravity);

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

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

Serial.print(pitch_angle);
Serial.print("\t");
Serial.println(roll_angle);

pitchServo.write((int)(pitch_angle));
rollServo.write((int)(roll_angle));


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:



Any idea? Thanks
At the end two pull-up resistors and one capacitor solved the problem.

I followed instruction here:

http://www.instructables.com/id/Brushless-Gimbal-with-Arduino/step3/Using-the-Accelerometer-and-Gyro/

Now it works well.. Thank you all.