Balancing robot help

Help!! I can’t seem to balance my robot. I’ve used several codes and several PID’s, but nothing works.
Using arduino, mpu6050, drv8833, 6v motor, 6v power supply.
I need a tutor that can help me with my project. My bot seems to fall to one side, so not sure if it’s too heavy and I need to change design.
DM for $

Perhaps your (not geared?) motor speed is too high for balancing?

I tried 255, I tried 100, 150, but my ‘box’ that contains the components might be too big and all of the components do not refresh as fast to keep it balanced. I’ve tried a Kp of like 10-6000 and nothing seems to work.

I’ve tried codes with Kalman filters, codes without it.
I’ve calibrated the mpu to get the acc (x,y,z) and gyro(x,y,z) numbers. The setpoint I set it to about 187 which seemed to be the top spot. Besides that, not sure what all else I could do

What’s that?

Balancing requires proper Kd and Ki.

You better become an expert in MPU usage before you start balancing.

Also show your device and code.

I meant “255, 100, 150” for motor speed.

Well, for Kp, Ki, and Kd you need to find Kp first, then the rest. I couldn’t get a proper Kp where it would be ‘stable’ and not falling all over the place. That’s why I started with Kp.
I did calibrate the MPU with the MPU test. My arduino crashed just now, so I don’t have the actual numbers in this code below. I did the test where I left it on top of it horizontally to get the acc(x,y,z) and gyro(x,y,z)

This is one of the codes I really liked and the last one I tried. I’ve tried at least 10-15 different codes:

#include <Wire.h>
#include <I2Cdev.h>
#include <MPU6050.h>
/* Code edited from http://www.instructables.com/id/Self-Balancing-Robot/
*/

MPU6050 accelgyro;

#define runEvery(t) for (static long _lasttime;\
                         (uint16_t)((uint16_t)millis() - _lasttime) >= (t);\
                         _lasttime += (t))


int16_t ax, ay, az;
float accBiasX, accBiasY, accBiasZ;
float accAngleX, accAngleY;
double accPitch, accRoll;

int16_t gx, gy, gz;
float gyroBiasX, gyroBiasY, gyroBiasZ;
float gyroRateX, gyroRateY, gyroRateZ;
float gyroBias_oldX, gyroBias_oldY, gyroBias_oldZ;
float gyroPitch = 180;
float gyroRoll = -180;
float gyroYaw = 0;

uint32_t timer;

// input
double InputPitch, InputRoll;

// initial values
double InitialRoll;

// Motors
int enablea = 9;
int enableb = 10;
int a1 = 4;
int a2 = 5;
int b1 = 12;
int b2 = 8;

void setup() {

  Wire.begin();

  Serial.begin(9600);

  Serial.println("Initializing I2C devices...");
  accelgyro.initialize();

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

  delay(1500);

  // Motor
  pinMode(enablea, OUTPUT);
  pinMode(enableb, OUTPUT);
  pinMode(a1, OUTPUT);
  pinMode(a2, OUTPUT);
  pinMode(b1, OUTPUT);
  pinMode(b2, OUTPUT);

  digitalWrite(a1, HIGH);
  digitalWrite(a2, HIGH);
  digitalWrite(b1, HIGH);
  digitalWrite(b2, HIGH);
  
 //TODO: Better calibration 
  accelgyro.setXAccelOffset(-250);
  accelgyro.setYAccelOffset(-1929);
  accelgyro.setZAccelOffset(1077);
  accelgyro.setXGyroOffset(98);
  accelgyro.setYGyroOffset(-145);
  accelgyro.setZGyroOffset(-22);

  gyroBiasX = 0;
  gyroBiasY = 0;
  gyroBiasZ = 0;

  accBiasX = 4;
  accBiasY = -4;
  accBiasZ = 16378;

  //Get Starting Pitch and Roll
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  accPitch = (atan2(-ax, -az) + PI) * RAD_TO_DEG;
  accRoll = (atan2(ay, -az) + PI) * RAD_TO_DEG;

  if (accPitch <= 360 & accPitch >= 180) {
    accPitch = accPitch - 360;
  }

  if (accRoll <= 360 & accRoll >= 180) {
    accRoll = accRoll - 360;
  }

  gyroPitch = accPitch;
  gyroRoll = accRoll;

  timer = micros();
  delay(1000);
  initializeValues();

}

double Setpoint;

void MotorControl(double out) {
  if (out > 0) {
    digitalWrite(a1, LOW);
    digitalWrite(a2, HIGH);
    digitalWrite(b1, HIGH);
    digitalWrite(b2, LOW);
  } else {
    digitalWrite(a1, HIGH);
    digitalWrite(a2, LOW);
    digitalWrite(b1, LOW);
    digitalWrite(b2, HIGH);
  }

  byte vel = abs(out);
  if (vel < 0)
    vel = 0;
  if (vel > 255)
    vel = 255;

  analogWrite(enablea, vel);
  analogWrite(enableb, vel);
}

void initializeValues() {

  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  //////////////////////
  //  Accelerometer   //
  //////////////////////
  accPitch = (atan2(-ax/182.0, -az/182.0) + PI) * RAD_TO_DEG;
  accRoll = (atan2(ay/182.0, -az/182.0) + PI) * RAD_TO_DEG;

  if (accRoll <= 360 & accRoll >= 180) {
    accRoll = accRoll - 360;
  }

  //////////////////////
  //      GYRO        //
  //////////////////////

  gyroRateX = ((int)gx - gyroBiasX) * 131; 

  gyroPitch += gyroRateY * ((double)(micros() - timer) / 1000000);

  
  timer = micros();
  InitialRoll = accRoll;

  Setpoint = InitialRoll;
}

double filtered = 0;
void loop() {

  runEvery(10) {
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    //////////////////////
    //  Accelerometer   //
    //////////////////////

    accRoll = (atan2(ay/182.0, -az/182.0) + PI) * RAD_TO_DEG;

    if (accRoll <= 360 & accRoll >= 180) {
      accRoll = accRoll - 360;
    }


    //////////////////////
    //      GYRO        //
    //////////////////////

    gyroRateX = -((int)gx - gyroBiasX) / 131; 


    double gyroVal = gyroRateX * ((double)(micros() - timer) / 1000000);

    timer = micros();

    //Complementary filter
    filtered = 0.98 * (filtered + gyroVal) + 0.02 * (accRoll);

    MotorControl(Compute(filtered - InitialRoll));

  }

}

int outMax = 255;
int outMin = -255;
float lastInput = 0;
double ITerm = 0;
double kp = 100;
double ki = 10;
double kd = 2;

double Compute(double input)
{

  double error = Setpoint - input;

  ITerm += (ki * error);

  if (ITerm > outMax) ITerm = outMax;
  else if (ITerm < outMin) ITerm = outMin;
  double dInput = (input - lastInput);


  /*Compute PID Output*/
  double output = kp * error + ITerm + kd * dInput;

  if (output > outMax) output = outMax;
  else if (output < outMin) output = outMin;

  /*Remember some variables for next time*/
  lastInput = input;
  return output;
}
}

botarduinoforum

I will post wiring in my next comment. I can’t post 2 pictures just yet

I’m using a breadboard, so the wiring is a little bit different (power sources)

For the mpu I’m using, I have to use the VDD to 5v and the VIO to 3.3V btw.

Let me know if the pics make sense

I also like this one: https://github.com/lukagabric/Franko
I will try the potentiometers tomorrow to tune it as it goes

Motor speed can not be controlled so easily, instead the true rpm has to be measured and taken into account.

I really suggest that you master the basic tasks first, i.e. MPU calibration and movements, motor speed measured and controlled by PID, yaw angle determination and PID.

It’s not a good idea to truncate a double value (calculated motor speed) to a byte without checking how far away the double value is from the byte value (0…255). A too big difference will make the entire regulator ineffective and uncontrollable.

how do I master the speed measured and controlled by PID?

Add a rotary encoder and related code to your project.

I do use the encoders already though. If you see the code,
// Motors
int enablea = 9;
int enableb = 10;
int a1 = 4;
int a2 = 5;
int b1 = 12;
int b2 = 8;

In your program all these are outputs to the motor driver.

the EnableA and B are the encoders from the motors

moving the center of gravity worked, though. Thanks!