Self-balancing robot - PID

Projects Programming
1

Hello, I'm working on a self-balancing robot. Now I am in the phase of PID debugging. To set the PID constants I am using a mobile app I created that communicates with esp32 via bluetooth, but I can't find the values at which the robot can balance. So I'm wondering if everything is correct in my code, design and components used.

I'm using mpu6050 to measure the tilt angle, I'm using IC L293D to control the motors, the motors are ordinary DC with gearbox and the whole thing is powered by 4 battery cells (4x1,5V).

IMG20231104150133
IMG202311041501331920×3413 603 KB

I wrote a simple library to control the motors that works, but I'll include it here just in case.

L293D.cpp

#include "L293D.h"

Motor::Motor(byte pinA, byte pinB) {
  this->pinA = pinA;
  this->pinB = pinB;
  init();
}

void Motor::init() {
  pinMode(pinA, OUTPUT);
  pinMode(pinB, OUTPUT);
}

void Motor::set(int pwm) {
  bool direction = (pwm >= 0) ? true : false;
  if(lastDirection != direction) {
    pinMode(pinA, OUTPUT);
    pinMode(pinB, OUTPUT);
  }
  if (direction) {
    analogWrite(pinA, abs(pwm));
    digitalWrite(pinB, LOW);
  } else {
    digitalWrite(pinA, LOW);
    analogWrite(pinB, abs(pwm));
  }
  lastDirection = direction;
}

void Motor::stop(){
  digitalWrite(pinA, HIGH);
  digitalWrite(pinB, HIGH);
}

L293D.h

#ifndef L293D
#define L293D
#include <Arduino.h>

class Motor {

private:
  byte pinA;
  byte pinB;
  int pwm;
  bool lastDirection;

public:

  Motor(byte pinA, byte pinB);

  void init();

  void set(int pwm);
  
  void stop();
  
};

#endif

My full code:

#include <Wire.h>
#include <L293D.h>
#include <BluetoothSerial.h>
#include <EEPROM.h>

#define motorA1 26
#define motorA2 27
#define motorB1 32
#define motorB2 33

TaskHandle_t BluetoothTask;
Motor motorA(motorA1, motorA2);
Motor motorB(motorB1, motorB2);
BluetoothSerial SerialBT;

//Other
bool tunning = false;
//Angle
const int MPU_addr = 0x68;
double xAcc, yAcc, zAcc, xGyro, yGyro, zGyro;
double angle, angleAcc, angleGyro;
double gyroMicros, deltaGyroTime;
//pid
double angleKp = 0, angleKi = 0, angleKd = 0;
double pidOutput, angleLastError, angleIntegral, angleOutputMin = -60, angleOutputMax = 60;
//time
unsigned long currentMicros;
unsigned long previousMicros;
const unsigned long interval = 4000;
//motor speed
double motorSpeed;
//functions initialization
void ReadSensors(double& angle);
double CalculatePID(double angle, double Kp, double Ki, double Kd, double& LastError, double& Integral, double OutputMin, double OutputMax);
void ControlMotors();
void Bluetooth(double& angleKp, double& angleKi, double& angleKd);

//in need of messasuring time
/*unsigned long startTime;
unsigned long endTime;
unsigned long loopDuration;
startTime = micros();
endTime = micros();
loopDuration = endTime - startTime;
Serial.print("Loop duration: ");
Serial.println(loopDuration);*/

void setup() {
  Serial.begin(115200);

  SerialBT.begin("BALANCING ROBOT");

  EEPROM.begin(512);

  xTaskCreatePinnedToCore(
    BluetoothTaskFunction, /* Funkce úkolu. */
    "BluetoothTask",       /* Název úkolu. */
    10000,                 /* Velikost zásobníku úkolu. */
    NULL,                  /* Parametr úkolu. */
    1,                     /* Priorita úkolu. */
    &BluetoothTask,        /* Ukazatel na úkol pro sledování vytvořeného úkolu. */
    1);


  Wire.begin();

  Wire.setClock(400000);

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x03);
  Wire.endTransmission(true);
}

void loop() {
  currentMicros = micros();
  if ((currentMicros - previousMicros) >= interval) {
    previousMicros = currentMicros;
    ReadSensors(angle);
    pidOutput = CalculatePID(angle, angleKp, angleKi, angleKd, angleLastError, angleIntegral, angleOutputMin, angleOutputMax);
    motorSpeed = map(pidOutput, angleOutputMin, angleOutputMax, -255, 255);
    motorSpeed = constrain(motorSpeed, -255, 255);
    motorA.set(motorSpeed);
    motorB.set(motorSpeed);
  }
}

void BluetoothTaskFunction(void* pvParameters) {
  for (;;) {
    Bluetooth(angleKp, angleKi, angleKd);
    vTaskDelay(40 / portTICK_PERIOD_MS);
  }
}
double CalculatePID(double angle, double Kp, double Ki, double Kd, double &LastError, double &Integral, double OutputMin, double OutputMax) {

  double error = -angle;

  if (Integral > OutputMax) {
    Integral = OutputMax;
  } else if (Integral < OutputMin) {
    Integral = OutputMin;
  }

  double derivative = error - LastError;
  double Output = Kp * error + Integral + Kd * derivative;

  if (Output > OutputMax) {
    if (Ki > 0) { 
      Integral -= (Output - OutputMax) / Ki;
    }
    Output = OutputMax;
  } else if (Output < OutputMin) {
    if (Ki > 0) { 
      Integral -= (Output - OutputMax) / Ki;
    }
    Output = OutputMin;
  }

  LastError = error;
  Integral += error * Ki;

  //   --- PID INFO ---

  // - Plot format -
  if (tunning) {
    Serial.print("PTerm:");
    Serial.print(Kp * error);
    Serial.print(",");

    Serial.print("ITerm:");
    Serial.print(Integral);
    Serial.print(",");

    Serial.print("DTerm:");
    Serial.print(Kd * derivative);
    Serial.print(",");

    Serial.print("Output:");
    Serial.println(Output);
  }
  return Output;
}
void ReadSensors(double& angle) {

  //Angle

  //accelometer

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 6, true);
  xAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 16384.0;
  yAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 16384.0;
  zAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 16384.0;

  //gyro
  deltaGyroTime = (double)(micros() - gyroMicros) / 1000000;
  gyroMicros = micros();

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 6, true);
  xGyro = (int16_t(Wire.read() << 8 | Wire.read())) / 131.0;
  yGyro = (int16_t(Wire.read() << 8 | Wire.read())) / 131.0;
  zGyro = (int16_t(Wire.read() << 8 | Wire.read())) / 131.0;

  angleGyro = xGyro * deltaGyroTime;
  angleAcc = atan2(-xAcc, zAcc) * RAD_TO_DEG;

  angle = 0.9 * (angle + angleGyro) + 0.1 * (angleAcc);


  //   --- IMU INFO ---

  // - Plot format -

  /*Serial.print("angle:");
  Serial.println(angle);*/

  // - Serial format -

  Serial.print("angle | ");
  Serial.println(angle);
}
static String LastConsPID;
int counterBluetooth = 0;
void Bluetooth(double& angleKp, double& angleKi, double& angleKd) {
  if (counterBluetooth != 1) counterBluetooth += 1;
  if (SerialBT.available()) {
    switch (SerialBT.read()) {
      case 'A':
        angleKp += 0.1;
        break;
      case 'B':
        angleKp -= 0.1;
        break;
      case 'C':
        EEPROM.write(0, float(angleKp * 1000));
        Serial.println("Saving");
        EEPROM.commit();
        break;
      case 'D':
        angleKi += 0.001;
        break;
      case 'E':
        angleKi -= 0.001;
        break;
      case 'F':
        EEPROM.write(4, float(angleKi * 1000));
        Serial.println("Saving");
        EEPROM.commit();
        break;
      case 'G':
        angleKd += 0.1;
        break;
      case 'H':
        angleKd -= 0.1;
        break;
      case 'I':
        EEPROM.write(8, float(angleKd * 1000));
        Serial.println("Saving");
        EEPROM.commit();
        break;
    }
  }
  String ConsPID = "(" + String(angleKp) + "|" + String(angleKi) + "|" + String(angleKd) + ")";
  if (ConsPID != LastConsPID || counterBluetooth == 1) {
    SerialBT.println(ConsPID);
  }
  LastConsPID = ConsPID;
}
2

I think that it will never stand perfectly upright and that should not be the goal. In the final system, I think the motors will have to "wobble".
This could be imperceptible but there will always be some system chaos, at the least, the inertia generated by a position correction, and the correction for that, and that, and that, etc... maybe planning for the wobble is the answer.

3

That's the goal.

I think the problem is that the robot reacts slowly to the angle even at high Kp. The PID output applied as pwm to the motors is not able to prevent the crash. Another problem I would like to point out is probably the wrong way of calculating the integral term. Whenever I increase the Ki value to only 0.001, the sum of the errors in one second is 250*error, what then leads to slow and weird behavior. I changed the limits of the integral term:

angleOutputMin = -20, angleOutputMax = 20;

4

I would use real motor drivers, L298N maybe, if you are trying to drive them with PWM.

If that isn't enough your motors might be under-rated, you may need something with higher on demand torque.

5

I was wondering, and might not the minimum input voltage of the L293D be a problem? I mean I'm using the esp32 wroom dev kit to send pwm, but it's only a 3.3V signal, so adding a pull up resistor to the motor driver control pins might solve the problem, no? Now the motors will start moving when the pwm reaches +110, until then they just make that sound.

6

Had a thought, that maybe you could try locking motors, by this I mean steppers, which won't have a free shaft after torque has been input to the system.

I would think that any kind of free motor setup is going to be a constant drain on power and have some amount of intrinsic and noticeable wobble.

Maybe what you really need are brakes?

7

I found out that the problem is actually in the motor control using the L293D driver. I did some tests with arduino uno (5V) and also with esp32 (3,3V) and in both cases the motors started spinning at +-40% pwm.

I read a post on a forum where someone had the same problem and replacing the motor driver with an L298N led to the same result.

Is there any other way than converting the pid output in the code around say 35% to a new zero?

8

Maybe you could account for it yeah, analog motors are going to behave weird with initial torque, and they also have a startup draw (even if it may be low in these projects) which might impact precision. Why not use steppers?

9

Maybe something like this completely untested snippet:

if (motorSpeed  > 0 ){
    motorSpeed = map(motorSpeed,0,255,255*0.35,255);
} else if ( motorSpeed < 0) {
    motorSpeed = -map(-motorSpeed,0,255,255*0.35,255);
} else {
    motorSpeed = 0;
}
10

I've already tried to use similar code:

    if (pidOutput >= 0) {
      motorSpeed = map(pidOutput, 0, angleOutputMax, 105, 255);
    } else {
      motorSpeed = map(pidOutput, angleOutputMin, -1, -255, -105);
    }

This solved the problem of controlling the motors. My system still seems to respond slowly even at high Kp (then also add oscillation). Any ideas?

11

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.