RC servo misbehave using arduino and MPU6050

Question

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\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

Answer 1

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.

Answer 2

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.