I'm new to gyroscope and accelerometer and am working on my project to control servo with Arduino Uno R3 based on the reading from InvenSense MPU6050 6DOF gyroscope and accelerometer. I've found the a same sheet of code from different websites.
Website 1_http://www.projehocam.com/wp-content/uploads/mpu6050-2-servo.txt
Website 2_http://www.instructables.com/files/orig/FLY/GZAL/IDIN48PC/FLYGZALIDIN48PC.ino
And from this part of the code, I couldn't understand the servo.write part...
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
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);
myservoY.write(int(ypr[1] * -180/M_PI)+90); // Rotation around Y
Serial.print("\t");
Serial.println(ypr[2] * 180/M_PI);
myservoX.write(int(ypr[2] * 180/M_PI)+90); // Rotation around X
#endif
Why is the operation (int(ypr[1] * -180/M_PI)+90) ??
1 Answer 1
ypr
contains the Yaw, Pitch and Roll. It is provided in radians.
Servo.write takes degrees.
Multiplying radians by 180/PI converts it to degrees. Adding 90 centralises it around the 90 degree point (half way) of the servo.