Self Balancing Robot Arduino

Posting this code here that I thought I had lost but was in a comment in one of my videos fortunately. It has the creep control which I cannot find anywhere now.

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
//#include <NewPing.h>



#define leftMotorPWMPin   6
#define leftMotorDirPin   7
#define rightMotorPWMPin  5
#define rightMotorDirPin  4

#define TRIGGER_PIN 9
#define ECHO_PIN 8
#define MAX_DISTANCE 75

//#define Kp  140  //was 170
//#define Kd  0.00 // was .02
//#define Ki  0 //

#define sampleTime  0.005
//#define targetAngle 7.5// -2.5
//#define adjustAngle 0

MPU6050 mpu;
//NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

int16_t accY, accZ, gyroX;
volatile int motorPower, gyroRate;
volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0;
volatile byte count=0;
volatile float Kp=150,Ki=00,Kd=.01,targetAngle=6;


int distanceCm;
int wheelOffset = 1;
float adjustAngle = 0.00;
long previousMillis = 0;  
long creepMillis = 0;
long loopTimer = 4;

void setMotors(int leftMotorSpeed, int rightMotorSpeed) {
 //  Serial.print("leftMotorSpeed= ");Serial.print(leftMotorSpeed); Serial.print("rightMotorSpeed= ");Serial.println(rightMotorSpeed);
  if(leftMotorSpeed >= 0) {
    analogWrite(leftMotorPWMPin, leftMotorSpeed);
    digitalWrite(leftMotorDirPin, LOW);
  }
  else { // if leftMotorSpeed is < 0 then set dir to reverse 
    analogWrite(leftMotorPWMPin, 255 + leftMotorSpeed);
    digitalWrite(leftMotorDirPin, HIGH);
  }
  if(rightMotorSpeed >= 0) {
    analogWrite(rightMotorPWMPin, rightMotorSpeed);
    digitalWrite(rightMotorDirPin, LOW);
  }
  else {
    analogWrite(rightMotorPWMPin, 255 + rightMotorSpeed);
    digitalWrite(rightMotorDirPin, HIGH);
  }
}


void setup() {
  //RemoteXY_Init ();  
  Serial.begin(115200);
  // set the motor control and PWM pins to output mode
  pinMode(leftMotorPWMPin, OUTPUT);
  pinMode(leftMotorDirPin, OUTPUT);
  pinMode(rightMotorPWMPin, OUTPUT);
  pinMode(rightMotorDirPin, OUTPUT);
  // set the status LED to output mode 
  pinMode(13, OUTPUT);
//  Serial.println("got here...");
  // initialize the MPU6050 and set offset values
//  Serial.print("Initialize MPU at: ");Serial.println(millis());
  
  mpu.initialize();
  
  mpu.setYAccelOffset(2349); // from calibration routine
  mpu.setZAccelOffset(1090);
  mpu.setXGyroOffset(-11);
  Serial.print("End Initialize MPU at: ");Serial.println(millis());
  // initialize PID sampling loop
 // delay(1000);
 
 // init_PID();
  // Initialize phone values
 //   RemoteXY.edit_CG= targetAngle;
 //RemoteXY.edit_CG  = targetAngle;
 //RemoteXY.edit_P   = Kp;
 //RemoteXY.edit_I   = Ki;
// RemoteXY.text_M   = Kd;

}

void loop() {
  
   //RemoteXY_Handler ();
   unsigned long currentMillis = millis();
     // update variables from and to phone
   //targetAngle = RemoteXY.edit_CG ; // set value to what you want
//   dtostrf(targetAngle     , 0, 2, RemoteXY.text_CG);
   //Kp          = RemoteXY.edit_P;
   //Ki          = RemoteXY.edit_I ;
  // Kd          = RemoteXY.edit_D;
  
   //////////////////////////////////////////////////////////////////////////////////////////////////
   //targetAngle = targetAngle + (RemoteXY.joystick_1_y/25); // set forward and backward targetAngle
   /////////////////////////////////////////////////////////////////////////////////////////////////
//   Kd          = RemoteXY.edit_D;
   if(currentMillis - previousMillis > loopTimer) {
 //    Serial.println(currentMillis);
     // read acceleration and gyroscope values
     accY = mpu.getAccelerationY();
     accZ = mpu.getAccelerationZ();  
     gyroX = mpu.getRotationX();

     previousMillis = currentMillis;   
     doPID();
   }
    //dtostrf(error     , 0, 2, RemoteXY.text_A);    // the error is increasing and it so wheels go faster
   
     Serial.print("Kp= ");Serial.println(Kp);

    if (error > .5) adjustAngle = adjustAngle - .02; // add or subtract from targetAngle to keep from drifting
    else
    if (error < -.5) adjustAngle = adjustAngle + .02;
    else
    adjustAngle = 0;
     //dtostrf(adjustAngle     , 0, 2, RemoteXY.text_M);
 
  // set motor power after constraining it

   motorPower = constrain(motorPower, -255, 255);
   
 //  Serial.print(" ");Serial.print(targetAngle);Serial.print(" ");Serial.println(motorPower);
//  Serial.print("currentAngle= ");Serial.println(currentAngle);
 // if (error < .5 && error > -.5) motorPower = 0; // create a deadband to stop robot from walking - doesnt seem to help - still drifts.

   if (abs(error) > 20) motorPower = 0; // if fall over then shut off motors
   //setMotors(motorPower + (RemoteXY.joystick_1_x/20 ), motorPower - (RemoteXY.joystick_1_x/20 ) );
setMotors(motorPower, motorPower  );
}
// The PID() will be called every 5 milliseconds
void doPID()
{
  // calculate the angle of inclination
  accAngle = atan2(accY, accZ)*RAD_TO_DEG;
  gyroRate = map(gyroX, -32768, 32767, -250, 250);
  gyroAngle = (float)gyroRate*sampleTime;  
  // complementary filter ///////////////////////////////////////////
  currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);
  //////////////////////////////////////////////////////////////////
  error = currentAngle - targetAngle + adjustAngle;
  errorSum = errorSum + error;  
  errorSum = constrain(errorSum, -300, 300);

  //calculate output from P, I and D values ///////////////////////////////////////////////////
  motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime;
  /////////////////////////////////////////////////////////////////////////////////////////////
  motorPower = constrain(motorPower, -255, 255);
  prevAngle = currentAngle;
  // toggle the led on pin13 every second
  count++;
  if(count == 200)  {
    count = 0;
    digitalWrite(13, !digitalRead(13));
  }
}