DCMotor.h

#ifndef __DCMOTOR_H
#define __DCMOTOR_H
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"

//Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 

// Select which 'port' M1, M2, M3 or M4. In this case, M1
Adafruit_DCMotor *myMotor = AFMS.getMotor(4);

void motor_shield_setup() {
	AFMS.begin();  // create with the default frequency 1.6KHz
	//AFMS.begin(1000);  // OR with a different frequency, say 1KHz
}

#endif //__DCMOTOR_H

horizontal.ino

#include "DCMotor.h"
#include "PIDControl.h"
#include "MPU.h"

//do you want to plot (or print otherwise)
bool serial_plot = false;

void setup() {
	Serial.begin(115200); //115200
	while (!Serial);

	i2cSetup();
	motor_shield_setup();

	Serial.println(F("Alive"));
	MPU6050Connect();
	pinMode(LED_PIN, OUTPUT); // LED Blinks when you are receiving FIFO packets from your MPU6050
}

void loop() {
	int loops = 0;
	if (mpuInterrupt) { // wait for MPU interrupt or extra packet(s) available
		GetDMP(); // Gets the MPU Data and canculates angles
		pid_control(Yaw, Pitch, Roll);
		loops++;
	}  

	static long QTimer = millis();
	if ((long)( millis() - QTimer ) >= 40) {
		QTimer = millis();

		//plot in Serial plotter
		if (serial_plot) {
			Serial.print(Yaw);
			Serial.print(' ');
			Serial.print(Pitch);
			Serial.print(' ');
			Serial.println(Roll);
		} //plot in serial terminal
		else {
			Serial.print(F("\t Yaw")); Serial.print(Yaw);
		  Serial.print(F("\t Pitch ")); Serial.print(Pitch);
		  Serial.print(F("\t Roll ")); Serial.print(Roll);
		  Serial.print(F("\t error: ")); Serial.print(angle_now);
		  Serial.print(F("\t voltage: ")); Serial.print(voltage);
		  //Serial.print(F("\t loops: ")); Serial.print(loops);
      Serial.print(F("\t Kp: ")); Serial.print(Kp);
      Serial.print(F("\t Kd: ")); Serial.print(Kd);
      Serial.print(F("\t Ki: ")); Serial.print(Ki);
		  Serial.println();
     
      if (pm_counter > 0) {
        Kp = 10.0;
        Kd = 1.0;
        pm_counter -= 1;
      } else {
        Kp = 200*(analogRead(0)/1024.0);
        Kd = 100*(analogRead(1)/1024.0);
        Ki = 10*(analogRead(2)/1024.0); 
      }
		}
		loops = 0;
	}
}

MPU.h

#ifndef __MPU_H
#define __MPU_H
#include "Arduino.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu;

// These are my MPU6050 Offset numbers: for mpu.setXGyroOffset()
// supply your own gyro offsets here, scaled for min sensitivity use MPU6050_calibration.ino <<< download to calibrate your MPU6050 put the values the probram returns below
//                       XA      YA      ZA      XG      YG      ZG
//int MPUOffsets[6] = {  -4232,  -706,   1729,    173,    -94,     37}; //MPU6050 on balanceing bot
//int MPUOffsets[6] = {  0,  0,   1729,    220,    76,     -85}; //MPU6050 on balanceing bot
int MPUOffsets[6] = {  -3742,  -1501,   1385,    56,    -5,     14}; //MPU6050 calibration
#define LED_PIN 13 // 

// ================================================================
// ===               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
float Yaw, Pitch, Roll; // in degrees

// ================================================================
// ===                      init the IMU                        ===
// ================================================================
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)..."));
  attachInterrupt(0, dmpDataReady, FALLING); //pin 2 on the Uno

  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

}

// ================================================================
// ===                      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
}

void MPUMath() {
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Yaw = (ypr[0] * 180 / M_PI);
  Pitch = (ypr[1] * 180 / M_PI);
  Roll = (ypr[2] * 180 / M_PI);
}

// ================================================================
// ===        read angle values from the DMP on the IMU         ===
// ================================================================
void GetDMP() {
  // Serial.println(F("FIFO interrupt at:"));
  // Serial.println(micros());
  mpuInterrupt = false;
  FifoAlive = 1;
  fifoCount = mpu.getFIFOCount();
  /*
  fifoCount is a 16-bit unsigned value. Indicates the number of bytes stored in the FIFO buffer.
  This number is in turn the number of bytes that can be read from the FIFO buffer and it is
  directly proportional to the number of samples available given the set of sensor data bound
  to be stored in the FIFO
  */

  // PacketSize = 42; refference in MPU6050_6Axis_MotionApps20.h Line 527
  // FIFO Buffer Size = 1024;
  uint16_t MaxPackets = 20;// 20*42=840 leaving us with  2 Packets (out of a total of 24 packets) left before we overflow.
  // If we overflow the entire FIFO buffer will be corrupt and we must discard it!

  // At this point in the code FIFO Packets should be at 1 99% of the time if not we need to look to see where we are skipping samples.
  if ((fifoCount % packetSize) || (fifoCount > (packetSize * MaxPackets)) || (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.
    Serial.println(F("Reset FIFO"));
    if (fifoCount % packetSize) Serial.print(F("\t Packet corruption")); // fifoCount / packetSize returns a remainder... Not good! This should never happen if all is well.
    Serial.print(F("\tfifoCount ")); Serial.print(fifoCount);
    Serial.print(F("\tpacketSize ")); Serial.print(packetSize);

    mpuIntStatus = mpu.getIntStatus(); // reads MPU6050_RA_INT_STATUS       0x3A
    Serial.print(F("\tMPU Int Status ")); Serial.print(mpuIntStatus , BIN);
    // MPU6050_RA_INT_STATUS       0x3A
    //
    // Bit7, Bit6, Bit5, Bit4          , Bit3       , Bit2, Bit1, Bit0
    // ----, ----, ----, FIFO_OFLOW_INT, I2C_MST_INT, ----, ----, DATA_RDY_INT

    /*
    Bit4 FIFO_OFLOW_INT: This bit automatically sets to 1 when a FIFO buffer overflow interrupt has been generated.
    Bit3 I2C_MST_INT: This bit automatically sets to 1 when an I2C Master interrupt has been generated. For a list of I2C Master interrupts, please refer to Register 54.
    Bit1 DATA_RDY_INT This bit automatically sets to 1 when a Data Ready interrupt is generated.
    */
    if (mpuIntStatus & B10000) { //FIFO_OFLOW_INT
      Serial.print(F("\tFIFO buffer overflow interrupt "));
    }
    if (mpuIntStatus & B1000) { //I2C_MST_INT
      Serial.print(F("\tSlave I2c Device Status Int "));
    }
    if (mpuIntStatus & B1) { //DATA_RDY_INT
      Serial.print(F("\tData Ready interrupt "));
    }
    Serial.println();
    //I2C_MST_STATUS
    //PASS_THROUGH, I2C_SLV4_DONE,I2C_LOST_ARB,I2C_SLV4_NACK,I2C_SLV3_NACK,I2C_SLV2_NACK,I2C_SLV1_NACK,I2C_SLV0_NACK,
    mpu.resetFIFO();// clear the buffer and start over
    mpu.getIntStatus(); // make sure status is cleared we will read it again.
  } else {
    while (fifoCount  >= packetSize) { // Get the packets until we have the latest!
      if (fifoCount < packetSize) break; // Something is left over and we don't want it!!!
      mpu.getFIFOBytes(fifoBuffer, packetSize); // lets do the magic and get the data
      fifoCount -= packetSize;
    }
    MPUMath(); // <<<<<<<<<<<<<<<<<<<<<<<<<<<< On success MPUMath() <<<<<<<<<<<<<<<<<<<
    digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the Light
    if (fifoCount > 0) mpu.resetFIFO(); // clean up any leftovers Should never happen! but lets start fresh if we need to. this should never happen.
  }
}

#endif //__MPU_H

PIDControl.h

#ifndef __PIDCONTROL_H
#define __PIDCONTROL_H
#include "DCMotor.h"

//global control variables
float angle_now = 0;
float angle_speed_now = 0;
float angle_sum = 0;
unsigned long time_now = 0;
float filter_spread = 0;
int voltage = 0;
float Kp = 0;
float Kd = 0;
float Ki = 0;
int pm_counter = 0;

//control action
void pid_control(float yaw, float pitch, float roll) {
  unsigned long time_prev = time_now;
  time_now = micros();
  
  //CALC POSITION
  float angle_prev = angle_now;
  //angle_now = 16.5 - (yaw); //yaw
  angle_now = roll+42.7;
  //filter
  angle_now = (1-filter_spread)*angle_now + filter_spread*angle_prev;
  
  //CALC SPEED
  float angle_speed_prev = angle_speed_now;
  angle_speed_now = ((1-filter_spread) * (angle_now-angle_prev) / (time_now-time_prev) * 1000000) + filter_spread * angle_speed_prev;
  
  //CALC INTEGRAL
  angle_sum += angle_now + (time_now-time_prev) * 100000; //should be 1000000 but is scaled down right now

  //check if it has gone over the zero angle
  //if ((angle_prev >= 0.0 && angle_now < 0.0) || (angle_prev < 0.0 && angle_now >= 0.0)) {
  //  pm_counter = 10;
  //  Serial.println("gone over zero");
  //}

  voltage = round(constrain(abs(angle_now*Kp + angle_speed_now*Kd + angle_sum*Ki),0,255));
  myMotor->setSpeed(voltage);
  if (angle_now > 0.0) {
    myMotor->run(BACKWARD);
  }
  else {
    myMotor->run(FORWARD);
  }
}

#endif //__PIDCONTROL_H