FIFO Overload

I’m currently working on a self balancing robot. The code consists mainly of IMU sensor readings, PID control, PWM to control the motors and loop timing. I’ve noticed that when I run the sensor code by itself there’s no FIFO overload. I’m using an adaptation of this code:
https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino and using the output aaReal.zz which is shown on line 319 on that link. When i added this code to the bottom:
int pos = aaReal.z/500;
updatePid(0,pos);
motors(spd);
lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME) delay(STD_LOOP_TIME-lastLoopUsefulTime);
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();
}
void motors(int spd)
{
if (spd > 0)
{
analogWrite(9, spd);
analogWrite(10, spd);
digitalWrite(4, LOW);
digitalWrite(5, HIGH);
digitalWrite(7, LOW);
digitalWrite(6, HIGH);
}
else
{
analogWrite(9, -spd);
analogWrite(10, -spd);
digitalWrite(7, HIGH);
digitalWrite(6, LOW);
digitalWrite(4, HIGH);
digitalWrite(5, LOW);
}
}
void updatePid(int targetPosition, int currentPosition) {
int error = targetPosition - currentPosition;
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error);
last_error = error;
spd= constrain((pTerm + iTerm + dTerm), -255, 255);

The sensor values starts increasing in magnitude and alternating even in the same position.

From the code:
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F(“FIFO overflow!”));
I’m wondering what I did to make the code too inefficient.

anything using float values? whats the control loop period?

I'm curious about this code:

    analogWrite(9, spd);
    analogWrite(10, spd);
    digitalWrite(4, LOW);
    digitalWrite(5, HIGH);
    digitalWrite(7, LOW);
    digitalWrite(6, HIGH);

Set the speed, first, then the direction. Do you do that in your car?

I think that I’m getting closer to the solution. I think that the period of the loop is affecting the FIFO overload. I uploaded a program that just displays the positions, but added a delay after the readings. When the delay was 1 second the FIFO overload error was more common, when the delay was one hundredth of a second the FIFO overload wasn’t as common. I think that I should maybe lower the sample rate of the IMU 6050 or average the values coming out of the sensor.

I think that it's about 95% solved now. I added the line: mpu.resetFIFO();

after every reading. When the delay is 100 milliseconds the readings will come out fine, 98% of the time. The other 2% of the time the value will come out ten times as much.