Selfbalancing robot doesn't react fast enough when falling

I use an mpu6050 to get the pitch and a pid library for pid calculation. I have seen the motors that I use in other selfbalancing robots so they are powerful enough.
I think there is a problem with the code because it looks like it doesn't calculate the pwm value fast enough and falls and then spins the motors like crazy.
What is wrong with my code?

#define _USE_MATH_DEFINES
#include <cmath>
#include <math.h>
#include "Arduino.h"
#include "I2Cdev.h"
#include <AutoPID.h>
#define CUSTOM_SETTINGS
#define INCLUDE_GAMEPAD_MODULE
#include <DabbleESP32.h>
#include "MPU6050_6Axis_MotionApps20.h"
#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif
//#include "MPU6050.h" // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
//#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  #include "Wire.h"
//#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high
#define INTERRUPT_PIN 4  // use pin 2 on Arduino Uno & most boards
#define LED_PIN 2
#define SCL 22
#define SDA 21
#define OUTPUT_MIN -255
#define OUTPUT_MAX 255
double KP=5;
double KI=0;
double KD=0;
#define enA 19 //PWM output
#define enB 18 //PWM output
#define in1 5
#define in2 17
#define in3 16
#define in4 12
#define freq 5000
#define channel 0
#define resolution 8
double pwm, input;
double setPoint=0.00;
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
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

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };


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

AutoPID balance(&input, &setPoint, &pwm, OUTPUT_MIN, OUTPUT_MAX, KP, KI, KD);

void setup() {
    Dabble.begin("MyEsp32");       //set bluetooth name of your device
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin(SDA,SCL);
        Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(SDA,SCL,400, true);
    #endif
    Serial.begin(115200);
    mpu.initialize();
    pinMode(INTERRUPT_PIN, INPUT_PULLUP);
    while(GamePad.isTrianglePressed()==0){
      Dabble.processInput();
    }
    //while (Serial.available() && Serial.read()); // empty buffer
    //while (!Serial.available());                 // wait for data
    //while (Serial.available() && Serial.read()); // empty buffer again

    // load and configure the DMP
    devStatus = mpu.dmpInitialize();
     //           X Accel  Y Accel  Z Accel   X Gyro   Y Gyro   Z Gyro
      //OFFSETS    -1650,   -1233,    2850,   -1996,     -74,     -43
    // supply your own gyro offsets here, scaled for min sensitivity
    //           X Accel  Y Accel  Z Accel   X Gyro   Y Gyro   Z Gyro
    //OFFSETS    -1650,   -1331,    2860,   -2041,     -72,     -38  
    
    mpu.setXAccelOffset(-1650);
    mpu.setYAccelOffset(-1331);
    mpu.setZAccelOffset(2860);
    mpu.setXGyroOffset(-2041);
    mpu.setYGyroOffset(-72);
    mpu.setZGyroOffset(-38);
    
    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
        Serial.println(F(")..."));
        attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), 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();
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
    pinMode(enA, OUTPUT);
    pinMode(enB, OUTPUT);
    pinMode(in1, OUTPUT);
    pinMode(in2, OUTPUT);
    pinMode(in3, OUTPUT);
    pinMode(in4, OUTPUT);
    ledcSetup(channel, freq, resolution);
    ledcAttachPin(enA, channel);
    ledcAttachPin(enB, channel);
    //balance.setBangBang(0.40);
    //balance.setTimeStep(4000);
}

void loop() {
    // 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) {
        if (mpuInterrupt && fifoCount < packetSize) {
          // try to get out of the infinite loop 
          fifoCount = mpu.getFIFOCount();
        }  
       Serial.print("p\t");
       Serial.print(input);
       balance.run();
       Serial.print("pwm\t");
       Serial.println(pwm);
       if(pwm>0){
         digitalWrite(in1,HIGH);
         digitalWrite(in2,LOW);
         digitalWrite(in3,HIGH);
         digitalWrite(in4,LOW);
         ledcWrite(channel, abs(pwm));
      }else if(pwm<0){
       digitalWrite(in1,LOW);
       digitalWrite(in2,HIGH);
       digitalWrite(in3,LOW);
       digitalWrite(in4,HIGH);
       ledcWrite(channel, abs(pwm));
      }
    }

    // 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 & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        fifoCount = mpu.getFIFOCount();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
        // 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;
     mpu.dmpGetQuaternion(&q, fifoBuffer);
     mpu.dmpGetGravity(&gravity, &q);
     mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
     input=ypr[2] * 180/M_PI;

    }
}

To start with, it is not posted here

Please follow the advice given in the link below when posting code . Use code tags when posting code here to make it easier to read and copy for examination

1 Like

The PID requires proper parameters kp, ki and kd. These values depend on your specific construction. Against slow reaction it may help to increase kd (differential factor).

1 Like

I uploaded my .ino file, isn't that the code?

This post again provides a good example of my insight: Nobody reads or researches themselves anymore, others can do it much better. Why should I strain my brain? Very sad!

It's a matter of what is assumed "basic" and what is advanced. If some code is available, why should it not be used? If the Arduino IDE contains libraries for everything, why should one re-invent the wheel?

What's the motivation for a starter? I don't think that properly debouncing a switch is an attractive target. Debouncing will be required later, but how can a newbie know that? So one takes a project that somebody claims easily feasible and - usueally fails :frowning:

1 Like

You attached the .ino file. It is much more convenient for anyone offering help if you copy the code into a post here as described in the link I gave to enable the code to be copied for examination with a single click

1 Like

Ok got it, I thought that it would be more convinient to have the file I will put the code like you said then.

I put my code.

The AutoPid library only updates at a constant interval internally, which is 1000ms by default. Your pid may just not be running fast enough... change it with setTimeStep(unsigned long timeStep).

Library docs

1 Like

How fast should it be, I read on another forum that 10 ms is a good value, is it a good value?

Yeah, 10ms is pretty fast. As others have said above, however, the tuning is also important and that can help you to get a fast response too.