Simple Arduino code for a two wheel self balancing robot https://youtu.be/aJHFEo97TYA

self_balancing_robot.ino

#include "Wire.h"
#include "Kalman.h"

const int MPU_ADDR = 0x68;
const double MICRO_SECOND = 1000000.00;
const double INTERVAL = 1000.0;

double dt, timer = 0.0;

double gyro_y_offset, accl_x_offset, accl_z_offset;
double pitch;
int16_t accl_x, accl_z, gyro_y;
Kalman kalman;

double error_old, integral, kp, ki, kd;

int throttle = 0;

void setupMotors() {
  pinMode(DD5, OUTPUT);
  pinMode(DD6, OUTPUT);
  analogWrite(DD6, throttle);
  analogWrite(DD5, throttle);
}

void setupIMU() {
  Wire.begin();
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  int period = 5;
  int samples = period * 10;
  for (int i = 0; i < samples; i++)
  {
    readIMU();
    gyro_y_offset += gyro_y;
    accl_x_offset += accl_x;
    accl_z_offset += accl_z;
    delay(100);
  }

  gyro_y_offset = gyro_y_offset / samples;
  accl_x_offset = accl_x_offset / samples;
  accl_z_offset = accl_z_offset / samples;
  
  gyro_y_offset = (0 - gyro_y_offset);
  accl_x_offset = (0 - accl_x_offset);
  accl_z_offset = (16384 - accl_z_offset);
}

void setupValues() {
  pitch = 0;
  kp = 15;
  ki = 175;
  kd = 0.3;

  kalman.setAngle(pitch);
  timer = micros();
}

void setup() {
  setupMotors();
  setupIMU();
  setupValues();
  //Serial.begin(9600);
}

void loop() {  
  dt = (micros() - timer) / MICRO_SECOND;
  timer = micros();

  work(); // Main working method.

  while ((micros() - timer) < INTERVAL)
  {
    //NOOP
  }
}

void work() {
  readIMU();
  calculatePitch();
  computePID();
  driveMotors();
}

void readIMU() {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 7 * 2, true);

  accl_x = Wire.read() << 8 | Wire.read();
  Wire.read() << 8 | Wire.read();
  accl_z = Wire.read() << 8 | Wire.read();
  Wire.read() << 8 | Wire.read();
  Wire.read() << 8 | Wire.read();
  gyro_y = Wire.read() << 8 | Wire.read();
  Wire.read() << 8 | Wire.read();
}

void calculatePitch() {
  double gyroY = (gyro_y + gyro_y_offset ) / 65.5;
  double accX = ((accl_x + accl_x_offset ) / 16348.00)*9.81;
  double accZ = ((accl_z + accl_z_offset ) / 16384.00)*9.81;
  
  pitch = atan2(-accX, accZ) * RAD_TO_DEG;

  pitch = kalman.getAngle(pitch, gyroY, dt);
}

void computePID() {
  double error;
  double derivative;

  if(pitch > 0){
    error = (0.1 - pitch);
  } else {
  error = (0.1 - pitch);
  }
    
  integral += error * dt;
  derivative = (error - error_old) / dt;
  error_old = error;

  //Serial.print("kp:");
  //Serial.print((kp * error));
  //Serial.print(",ki:");
  //Serial.print((ki * integral));
  //Serial.print(",kd:");
  //Serial.println((kd * derivative));
  
  throttle = -1*((kp * error) + (ki * integral) + (kd * derivative));

  if (throttle > 125) {
    throttle = 125;
  }
  if (throttle < -125) {
    throttle = -125;
  }
}

void driveMotors() {
  if(throttle < 0) {
    analogWrite(DD5, abs(throttle));
    analogWrite(DD6, 0);
  } else {
    analogWrite(DD5, 0);
    analogWrite(DD6, abs(throttle));
  }
}