Self-Balancing Robot -- Arduino PID Autotune Library

Hi friends!
I am making a Self-Balancing Robot. I have got very good results, but I want to improve the code. Recently, I have discovered de possibility to use an auto-tune (Arduino PID Autotune Library), to tune in the constants kp, ki, kd of PID. However, I am using my own PID (that works very well), so I do not want to use the libraries of PID-Arduino.
After to read the description of this library, I think that I only need the Input, output, and noise band (in my case 1º, approximately). I am getting the Input from sensor MPU6050, the output (U) is the signal to control the motors, and using the serial plotter, I think that my noise band is one degree.
I have added the code of my robot. But I can’t find the way to calculate and use automatically the constants kp, ki, kd. Please, can you help me?

#include <PID_AutoTune_v0.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
MPU6050 mpu;
bool blinkState = false;
// 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
// packet structure for InvenSense teapot demo
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
int IN1 = 4; //motors
int IN2 = 5; //motors
int ENA = 3; //motors PWM
int IN3 = 8; //motors
int IN4 = 9; //motors
int ENB = 10; //motors PWM
// Variables PID
unsigned long lastTime;
double  U, Setpoint = 15.10, Ui, Ud, Up, Input; // U = Output, Input = the angle from kalman filter
double  ITerm, lastInput;
double  kp, ki, kd;
int  SampleTime = 5; // 
double outMin = -128, outMax = 128;  
double error;
PID_ATune aTune(&Input, &U);
void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
        Fastwire::setup(400, true);
    devStatus = mpu.dmpInitialize();
    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setZAccelOffset(1105); // 1688 factory default for my test chip
    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        // enable Arduino interrupt detection
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();
        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        dmpReady = true;
        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
    pinMode(IN1, OUTPUT); //Electric Motors
    pinMode(IN2, OUTPUT); //Electric Motors
    pinMode(ENA,OUTPUT); //Electric Motors
    pinMode(IN3, OUTPUT); //Electric Motors
    pinMode(IN4, OUTPUT); //Electric Motors
    pinMode(ENB,OUTPUT); //Electric Motors

    pinMode(A0, INPUT); //Electric Motors
    pinMode(A1, INPUT); //Electric Motors
    pinMode(A2, INPUT); //Electric Motors

    // Autotune
void loop() {
void Compute(){
    if (!dmpReady) return;
    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
        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);
      kp = aTune.GetKp();
      ki = aTune.GetKi();
      kd = aTune.GetKd();
  unsigned long now = millis(); //Current time. Indicates the time lapsed since the program began to execute the program.
  int timeChange = (now - lastTime); // It will indicate the time elapsed since the last time the PID was executed
  if (timeChange >= SampleTime){ // It is use to know when to execute the PID or tetornar of the function.
      Input = ypr[2] * 180/M_PI;
      error = Setpoint - Input; //error
      ITerm += (ki*error); // Integral del area
      if (ITerm> outMax) ITerm= outMax;
      else if (ITerm< outMin) ITerm= outMin;
          double dInput = (Input - lastInput);
  Up = kp*error; //proporcional signal
  Ui = ITerm; // integral signal
  Ud = kd*dInput; // derivative signal
  U = Up + Ui - Ud; // output signal
  if (U > outMax) U = outMax;
  else if (U < outMin) U = outMin;
  lastInput=Input; //time update
  lastTime = now; //error update
  // motors
  if (U <= 0){
   digitalWrite(IN1, LOW);
   digitalWrite(IN2, HIGH);
   analogWrite(ENA, -U);
   digitalWrite(IN3, LOW);
   digitalWrite(IN4, HIGH);
   analogWrite(ENB, -U);
  if (U > 0){
   digitalWrite(IN1, HIGH);
   analogWrite(ENA, U);
   digitalWrite(IN3, HIGH);
   digitalWrite(IN4, LOW);
   analogWrite(ENB, U);
void SetTunings(double Kp,double Ki,double Kd){
  double SampleTimeInSec = ((double)SampleTime)/1000;
  kp = Kp;
  ki = Ki * SampleTimeInSec;
  kd = Kd / SampleTimeInSec;
void SetSampleTime(int NewSampleTime)
  if (NewSampleTime > 0){
    /* if the user decides to change the sampling time during operation, Ki and Kd will have to adjust to reflect this change. */
    double ratio  = (double)NewSampleTime / (double)SampleTime;
    ki *= ratio;
    kd /= ratio;
    SampleTime = (unsigned long)NewSampleTime; 
void SetOutputLimits(double Min, double Max)
  if (Min > Max) return;
    outMin = Min;
    outMax = Max;
  if (U > outMax) U = outMax;
  else if (U < outMin) U = outMin;
    if (ITerm> outMax) ITerm= outMax;
      else if (ITerm< outMin) ITerm= outMin;
void motores(){