Self Balancing Robot

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "MPU6050.h"
#include "Wire.h"


// ==========================================================================================
//         The following code was written by Jeff Rowberg to make the MPU6050 usable.
// ==========================================================================================


MPU6050 mpu;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
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
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

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


// ==========================================================================================
//                                 Jeff Rowberg's code ends here.
// ==========================================================================================


// Arduino pin attatchments
const int mpuInt = 2; 
const int motorADir2 = 3;
const int motorBDir1 = 4;
const int motorBDir2 = 5;
const int motorADir1 = 6;
const int motorAPWM = 9;
const int motorBPWM = 10;
const int greenLED = 11;
const int redLED = 12;
const int mpuSDA = A4;
const int mpuSCL = A5;

// PID variables
const int kp = 15; // Proportional: angle from balance point
const int ki = 2;  // Integral: past trends of values
const int kd = 20; // Derivative: angular velocity of robot
int error = 0;     // Deviation from desired upright point based on kp, ki, and kd

// Other variables
float angle = 0, lastAngle = 0; // Current and previous angle values
float angularVelocity = 0;      // Angular velocity of robot
float balancePoint = 5.5;       // An offset to couteract the misaligned center of gravity
short signed int integral = 0;  // The "integral" of the plotted angles

void setup() {
  // mpuSetup was written by Jeff Rowberg to initialize the MPU6050:
  mpuSetup();

  // Set the LEDs as outputs
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);
}

void loop() {
  // mpuLoop was written by Jeff Rowberg to obtain values from the MPU6050:
  mpuLoop();

  // Converting the angle into usable degrees:
  angle = (ypr[2] * 180/M_PI) - balancePoint;
  Serial.print("Angle:\t");
  Serial.print(angle);
  Serial.print("\t");

  // Feeding the angle values into an array and summing them caused
  // errors, so I will use this cheap integral shortcut:
  if(abs(integral) > 5){
    // This limits the magnitude of the integral to +/-5:
    if(integral > 0){
      integral--;
    } else {
      integral++;
    }
  } else if(angle < 0){
    // Subtract one from the integral if it is negative:
    integral--;
  } else {
    // Add one to the integral if it is positive:
    integral++;
  }

  // Determining the rate of angle change:
  angularVelocity = angle - lastAngle;

  // PID error calculation:
  error = (kp * angle) + (ki * integral) + (kd * angularVelocity);
  Serial.print("Error:\t");
  Serial.println(error);

  // Save the current angle so it can be used in the next error calculation:
  lastAngle = angle;
 
  // Turn on the green LED when the robot can move, red if it can't:
  if(abs(angle) > 30){
    digitalWrite(redLED, HIGH);
    digitalWrite(greenLED, LOW);      
  } else {
    digitalWrite(greenLED, HIGH);
    digitalWrite(redLED, LOW);
  }

  // Setting the speed and direction of the motors:
  if(abs(error) < 10 || abs(angle) > 40){
    // Turn off the motors if tilting too far, or if well-balanced:
    digitalWrite(motorADir1, LOW);
    digitalWrite(motorADir2, LOW);
    digitalWrite(motorBDir1, LOW);
    digitalWrite(motorBDir2, LOW);
  } else if(error < 0){
    // Go backwards:
    digitalWrite(motorADir1, HIGH);
    digitalWrite(motorADir2, LOW);
    digitalWrite(motorBDir1, HIGH);
    digitalWrite(motorBDir2, LOW);

    // Setting the motor speeds. PWM outputs go from 0 to 255, but
    // below 100 the motors just hum and don't turn.
    analogWrite(motorAPWM, map(abs(error), 0, 320, 100, 255));
    analogWrite(motorBPWM, map(abs(error), 0, 320, 100, 255));
  } else {
    // Go forwards:
    digitalWrite(motorADir1, LOW);
    digitalWrite(motorADir2, HIGH);
    digitalWrite(motorBDir1, LOW);
    digitalWrite(motorBDir2, HIGH);

    // Setting the motor speeds:
    analogWrite(motorAPWM, map(abs(error), 0, 320, 100, 255));
    analogWrite(motorBPWM, map(abs(error), 0, 320, 100, 255));
  }
}


// ==========================================================================================
//         The following code was written by Jeff Rowberg to make the MPU6050 usable.
// ==========================================================================================


void mpuSetup(){
  // 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

  // initialize serial communication
  Serial.begin(115200);

  // initialize device
  Serial.println(F("Initializing I2C devices..."));
  mpu.initialize();

  // verify connection
  Serial.println(F("Testing device connections..."));
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788);

  // turn on the DMP, now that it's ready
  Serial.println(F("Enabling DMP..."));
  mpu.setDMPEnabled(true);

  // enable Arduino interrupt detection
  Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
  attachInterrupt(0, dmpDataReady, RISING);
  mpuIntStatus = mpu.getIntStatus();

  // set our DMP Ready flag so the main loop() function knows it's okay to use it
  Serial.println(F("DMP ready! Waiting for first interrupt..."));
  dmpReady = true;

  // get expected DMP packet size for later comparison
  packetSize = mpu.dmpGetFIFOPacketSize();
}

void mpuLoop(){
  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize) {}

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // 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!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);
        
    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount -= packetSize;
    
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  }      
}