FIFO Overflow when controlling motors

[MereWhiplash]

So Im using the MPU6050 DMP files which have been fantastic for learning to use the module. Im trying to control a small vehicle while taking some readings from the unit but Im getting a FIFO error when I include the control code. 
I was hoping someone here might have an idea whats going on. Im fairly new to arduino programming.

 

Thanks in advance! 

Whiplash 

#include <Servo.h>
#include <WString.h>
#include <Wire.h>

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"


MPU6050 mpu;

bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;    
uint16_t fifoCount;     
uint8_t fifoBuffer[64];
  
Quaternion q;
float speedX;
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];

uint8_t packet[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

Servo cameraX;
Servo cameraY;

int CX = 10;
int CY = 11;

int cameraXPos = 90;
int cameraYPos = 90;

int E1 = 6; //M1 Speed Control
int E2 = 5; //M2 Speed Control
int M1 = 8; //M1 Direction Control
int M2 = 7; //M2 Direction Control
int led = 13; //LED Jumper

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
	mpuInterrupt = true;
}

void setup(void)
{
	//Joining I2C bus
	Wire.begin();
	TWBR = 12;

	Serial.begin(250000);//baud rate

	int i;
	for (i = 5; i <= 8; i++)
		pinMode(i, OUTPUT);

	//Setup
	mpu.initialize();
	Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
	while (Serial.available() && Serial.read()); // empty buffer

	Serial.println(F("Initializing DMP..."));
	devStatus = mpu.dmpInitialize();

	mpu.setXGyroOffset(0);
	mpu.setYGyroOffset(0);
	mpu.setZGyroOffset(0);
	mpu.setZAccelOffset(0);

	if (devStatus == 0) {
		mpu.setDMPEnabled(true);
		attachInterrupt(0, dmpDataReady, RISING);
		mpuIntStatus = mpu.getIntStatus();

		dmpReady = true;
		packetSize = mpu.dmpGetFIFOPacketSize();
	}
	else {
		Serial.print(F("DMP Initialization failed (code "));
		Serial.print(devStatus);
		Serial.println(F(")"));
	}

	cameraX.attach(CX);
	cameraX.write(cameraXPos);
	cameraY.attach(CY);
	cameraY.write(cameraYPos);

	Serial.println("Online: Autobots Roll Out");
}

void loop(void)
{
	while (Serial.available() < 1) {
	}

	char val = ' ';
	int leftspeed = 150; 
	int rightspeed = 150;

	if (val == 'e' || val == 'e') {
		cameraXPos += 1;
		if (cameraXPos > 150)
			cameraXPos = 150;
		cameraX.write(cameraXPos);
	}

	if (val == 'q' || val == 'Q') {
		cameraXPos -= 1;
		if (cameraXPos < 20)
			cameraXPos = 20;
		cameraX.write(cameraXPos);
	}

	if (val == 'i' || val == 'I') {
		cameraYPos += 1;
		if (cameraYPos > 150)
			cameraYPos = 150;
		cameraY.write(cameraYPos);
	}

	if (val == 'k' || val == 'K') {
		cameraYPos -= 1;
		if (cameraYPos < 20)
			cameraYPos = 20;
		cameraY.write(cameraYPos);
	}

	switch (val) // Perform an action depending on the command
	{
	case 'w'://Move Forward
	case 'W':
		forward(leftspeed, rightspeed);
		break;
	case 's'://Move Backwards
	case 'S':
		reverse(leftspeed, rightspeed);
		break;
	case 'a'://Turn Left
	case 'A':
		left(leftspeed, rightspeed);
		break;
	case 'd'://Turn Right
	case 'D':
		right(leftspeed, rightspeed);
		break;
	default:
		stop();
		break;
	}

	if (!dmpReady) return;//break if CHIP NOT READY
	while (!mpuInterrupt && fifoCount < packetSize) {
		// other program behavior stuff here
		
	}

	mpuInterrupt = false;
	mpuIntStatus = mpu.getIntStatus();
	fifoCount = mpu.getFIFOCount();

	if ((mpuIntStatus & 0x10 || fifoCount == 1024)) {
		mpu.resetFIFO();
		Serial.println(F("FIFO Overlfow"));
	}
	else if(mpuIntStatus & 0x02) {
		while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
		mpu.getFIFOBytes(fifoBuffer, packetSize);
		mpu.resetFIFO();
		fifoCount -= packetSize;
	}

	mpu.dmpGetQuaternion(&q, fifoBuffer);
	mpu.dmpGetAccel(&aa, fifoBuffer);
	mpu.dmpGetGravity(&gravity, &q);
	mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
	Serial.print(q.w);
	Serial.print("\t");
	Serial.print(q.x);
	Serial.print("\t");
	Serial.print(q.y);
	Serial.print("\t");
	Serial.println(q.z);
	Serial.print("areal\t");
	Serial.print(aaReal.x);
	Serial.print("\t");
	Serial.print(aaReal.y);
	Serial.print("\t");
	Serial.println(aaReal.z);
	
}

void stop(void) //Stop
{
	digitalWrite(E1, LOW);
	digitalWrite(E2, LOW);
}

void forward(char a, char 
{
	analogWrite(E1, a);
	digitalWrite(M1, LOW);
	analogWrite(E2, ;
	digitalWrite(M2, LOW);
}

void reverse(char a, char 
{
	analogWrite(E1, a);
	digitalWrite(M1, HIGH);
	analogWrite(E2, ;
	digitalWrite(M2, HIGH);
}

void left(char a, char 
{
	analogWrite(E1, a);
	digitalWrite(M1, HIGH);
	analogWrite(E2, ;
	digitalWrite(M2, LOW);
}

void right(char a, char 
{
	analogWrite(E1, a);
	digitalWrite(M1, LOW);
	analogWrite(E2, ;
	digitalWrite(M2, HIGH);
}

[Anujkumar]

How often do you get the FIFO overflow?
Try changing it to TWBR=24;