Try this
#include <Servo.h>
Servo myservo; // create servo object to control a servo
int pos = 0; // variable to store the servo position
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
MPU6050 mpu;
#define DEBUG
#ifdef DEBUG
//#define DPRINT(args...) Serial.print(args) //OR use the following syntax:
#define DPRINTSTIMER(t) for (static unsigned long SpamTimer; (unsigned long)(millis() - SpamTimer) >= (t); SpamTimer = millis())
#define DPRINTSFN(StrSize,Name,...) {char S[StrSize];Serial.print("\t");Serial.print(Name);Serial.print(" "); Serial.print(dtostrf((float)__VA_ARGS__ ,S));}//StringSize,Name,Variable,Spaces,Percision
#define DPRINTLN(...) Serial.println(__VA_ARGS__)
#else
#define DPRINTSTIMER(t) if(false)
#define DPRINTSFN(...) //blank line
#define DPRINTLN(...) //blank line
#endif
#define interruptPin 7
#define LED_PIN 11 //
// supply your own gyro offsets here, scaled for min sensitivity use MPU6050_calibration.ino
// XA YA ZA XG YG ZG
//int MPUOffsets[6] = { 2471, -563, 690, 66, -29, 39}; //
//int MPUOffsets[6] = {1136, -44, 1047 , 52, 5, 26};// Test Board
int MPUOffsets[6] = {-769, -427, 1955 , 93, -6, 4};// Test Board 9255
// ================================================================
// === i2c SETUP Items ===
// ================================================================
void i2cSetup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
}
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === MPU DMP SETUP ===
// ================================================================
int FifoAlive = 0; // tests if the interrupt is triggering
int IsAlive = -20; // counts interrupt start at -20 to get 20+ good values before assuming connected
// MPU control/status vars
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
byte StartUP = 100; // lets get 100 readings from the MPU before we start trusting them (Bot is not trying to balance at this point it is just starting up.)
void MPU6050Connect() {
static int MPUInitCntr = 0;
// initialize device
mpu.initialize(); // same
// load and configure the DMP
devStatus = mpu.dmpInitialize();// same
if (devStatus != 0) {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};
MPUInitCntr++;
Serial.print(F("MPU connection Try #"));
Serial.println(MPUInitCntr);
Serial.print(F("DMP Initialization failed (code "));
Serial.print(StatStr[devStatus]);
Serial.println(F(")"));
if (MPUInitCntr >= 10) return; //only try 10 times
delay(1000);
MPU6050Connect(); // Lets try again
return;
}
mpu.setXAccelOffset(MPUOffsets[0]);
mpu.setYAccelOffset(MPUOffsets[1]);
mpu.setZAccelOffset(MPUOffsets[2]);
mpu.setXGyroOffset(MPUOffsets[3]);
mpu.setYGyroOffset(MPUOffsets[4]);
mpu.setZGyroOffset(MPUOffsets[5]);
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt pin 2 on the Uno)..."));
Serial.print("mpu.getInterruptDrive= "); Serial.println(mpu.getInterruptDrive());
attachInterrupt(digitalPinToInterrupt(interruptPin), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus(); // Same
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
delay(1000); // Let it Stabalize
mpu.resetFIFO(); // Clear fifo buffer
mpu.getIntStatus();
mpuInterrupt = false; // wait for next interrupt
}
// ================================================================
// === MPU DMP Get Data ===
// ================================================================
void GetDMP() { // Best version I have made so far
// Serial.println(F("FIFO interrupt at:"));
// Serial.println(micros());
static unsigned long LastGoodPacketTime;
mpuInterrupt = false;
FifoAlive = 1;
fifoCount = mpu.getFIFOCount();
if ((!fifoCount) || (fifoCount % packetSize)) { // we have failed Reset and wait till next time!
digitalWrite(LED_PIN, LOW); // lets turn off the blinking light so we can see we are failing.
mpu.resetFIFO();// clear the buffer and start over
} else {
while (fifoCount >= packetSize) { // Get the packets until we have the latest!
mpu.getFIFOBytes(fifoBuffer, packetSize); // lets do the magic and get the data
fifoCount -= packetSize;
}
LastGoodPacketTime = millis();
MPUMath(); // <<<<<<<<<<<<<<<<<<<<<<<<<<<< On success MPUMath() <<<<<<<<<<<<<<<<<<<
digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the Light
}
}
// ================================================================
// === MPU Math ===
// ================================================================
float Yaw, Pitch, Roll;
void MPUMath() {
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Yaw = (ypr[0] * 180.0 / M_PI);
Pitch = (ypr[1] * 180.0 / M_PI);
Roll = (ypr[2] * 180.0 / M_PI);
myservo.write(map (Yaw, -180, 180,0,180));
DPRINTSTIMER(100) {
DPRINTSFN(15, " W:", q.w, -6, 4);
DPRINTSFN(15, " X:", q.x, -6, 4);
DPRINTSFN(15, " Y:", q.y, -6, 4);
DPRINTSFN(15, " Z:", q.z, -6, 4);
DPRINTSFN(15, " Yaw:", Yaw, -6, 2);
DPRINTSFN(15, " Pitch:", Pitch, -6, 2);
DPRINTSFN(15, " Roll:", Roll, -6, 2);
DPRINTSFN(15, " Yaw:", ypr[0], -6, 2);
DPRINTSFN(15, " Pitch:", ypr[1], -6, 2);
DPRINTSFN(15, " Roll:", ypr[2], -6, 2);
DPRINTLN();
}
}
// ================================================================
// === Setup ===
// ================================================================
void setup() {
Serial.begin(115200); //115200
while (!Serial);
Serial.println("i2cSetup");
i2cSetup();
Serial.println("MPU6050Connect");
MPU6050Connect();
Serial.println("Setup complete");
pinMode(LED_PIN, OUTPUT);
myservo.attach(9); // attaches the servo on pin 9 to the servo object
}
// ================================================================
// === Loop ===
// ================================================================
void loop() {
if (mpuInterrupt ) { // wait for MPU interrupt or extra packet(s) available
GetDMP();
}
}