Hardi Kurnianto
Published

Self-Balancing Robot with AI-Enhanced PID Control

Self-Balancing Robot with AI-Enhanced PID Control utilizes AI-tuned PID parameters to improve stability, adaptability, and real-time balance

BeginnerShowcase (no instructions)5 hours17
Self-Balancing Robot with AI-Enhanced PID Control

Things used in this project

Hardware components

Arduino Nano R3
Arduino Nano R3
×1
SparkFun Triple Axis Accelerometer and Gyro Breakout - MPU-6050
SparkFun Triple Axis Accelerometer and Gyro Breakout - MPU-6050
×1
DC Motor, 12 V
DC Motor, 12 V
×1
Dual H-Bridge motor drivers L298
SparkFun Dual H-Bridge motor drivers L298
×1

Software apps and online services

Arduino IDE
Arduino IDE

Story

Read more

Schematics

CIRCUIT

Code

Code

Arduino
#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

// =======================
// MOTOR PINS
// =======================
#define ENA 5
#define IN1 6
#define IN2 7

#define ENB 10
#define IN3 8
#define IN4 9

// =======================
// PID PARAMETERS
// =======================
float Kp = 38.0;
float Ki = 1.5;
float Kd = 2.8;

// =======================
// KALMAN VARIABLES
// =======================
float Q_angle = 0.001;
float Q_bias = 0.003;
float R_measure = 0.03;

float angle = 0;
float bias = 0;
float rate = 0;

float P[2][2] = {{0,0},{0,0}};

// =======================
// PID VARIABLES
// =======================
float setPoint = 0;

float error = 0;
float integral = 0;
float derivative = 0;

float previousError = 0;
float filteredDerivative = 0;

float output = 0;

// =======================
// SENSOR OFFSET
// =======================
long gyroOffset = 0;

// =======================
// TIMING
// =======================
const uint32_t LOOP_TIME_US = 5000;
uint32_t lastLoop = 0;

// =======================
// FAIL SAFE
// =======================
float MAX_ANGLE = 35;

// =======================
// SETUP
// =======================
void setup()
{
  Serial.begin(115200);

  Wire.begin();

  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);

  pinMode(ENB, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  mpu.initialize();

  if (!mpu.testConnection())
  {
    Serial.println("MPU6050 FAIL");
    while(1);
  }

  calibrateMPU();

  Serial.println("READY");
}

// =======================
// LOOP
// =======================
void loop()
{
  if (micros() - lastLoop >= LOOP_TIME_US)
  {
    lastLoop += LOOP_TIME_US;

    controlLoop();
  }
}

// =======================
// CONTROL LOOP
// =======================
void controlLoop()
{
  float dt = 0.005;

  int16_t ax, ay, az;
  int16_t gx, gy, gz;

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

  float accelAngle =
      atan2((float)ay,(float)az)
      *180.0/PI;

  float gyroRate =
      ((float)(gx-gyroOffset))
      /131.0;

  angle = kalmanUpdate(
      accelAngle,
      gyroRate,
      dt);

  error = angle - setPoint;

  integral += error * dt;

  integral = constrain(
      integral,
      -50,
      50);

  derivative =
      (error - previousError)
      /dt;

  filteredDerivative =
      0.8 * filteredDerivative +
      0.2 * derivative;

  output =
      Kp * error +
      Ki * integral +
      Kd * filteredDerivative;

  previousError = error;

  if(abs(angle) > MAX_ANGLE)
  {
      stopMotor();
      integral = 0;
      return;
  }

  driveMotor(output);

  Serial.print(angle);
  Serial.print(",");
  Serial.print(output);
  Serial.print(",");
  Serial.println(error);
}

// =======================
// KALMAN FILTER
// =======================
float kalmanUpdate(
    float newAngle,
    float newRate,
    float dt)
{
  rate = newRate - bias;

  angle += dt * rate;

  P[0][0] += dt * (
      dt*P[1][1]
      - P[0][1]
      - P[1][0]
      + Q_angle);

  P[0][1] -= dt*P[1][1];
  P[1][0] -= dt*P[1][1];
  P[1][1] += Q_bias*dt;

  float S =
      P[0][0] + R_measure;

  float K0 =
      P[0][0]/S;

  float K1 =
      P[1][0]/S;

  float y =
      newAngle-angle;

  angle += K0*y;
  bias += K1*y;

  float P00 =
      P[0][0];

  float P01 =
      P[0][1];

  P[0][0] -= K0*P00;
  P[0][1] -= K0*P01;

  P[1][0] -= K1*P00;
  P[1][1] -= K1*P01;

  return angle;
}

// =======================
// MOTOR DRIVER
// =======================
void driveMotor(float pid)
{
  int pwm =
      constrain(abs(pid),
      0,
      255);

  if(pwm > 0)
      pwm += 25;

  pwm =
      constrain(
      pwm,
      0,
      255);

  if(pid > 0)
  {
      digitalWrite(IN1,HIGH);
      digitalWrite(IN2,LOW);

      digitalWrite(IN3,HIGH);
      digitalWrite(IN4,LOW);
  }
  else
  {
      digitalWrite(IN1,LOW);
      digitalWrite(IN2,HIGH);

      digitalWrite(IN3,LOW);
      digitalWrite(IN4,HIGH);
  }

  analogWrite(ENA,pwm);
  analogWrite(ENB,pwm);
}

// =======================
// STOP MOTOR
// =======================
void stopMotor()
{
  analogWrite(ENA,0);
  analogWrite(ENB,0);

  digitalWrite(IN1,LOW);
  digitalWrite(IN2,LOW);

  digitalWrite(IN3,LOW);
  digitalWrite(IN4,LOW);
}

// =======================
// CALIBRATION
// =======================
void calibrateMPU()
{
  Serial.println("CALIBRATING");

  long sum = 0;

  for(int i=0;i<2000;i++)
  {
      int16_t ax,ay,az;
      int16_t gx,gy,gz;

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

      sum += gx;

      delay(2);
  }

  gyroOffset =
      sum / 2000;

  Serial.print("GYRO OFFSET=");
  Serial.println(gyroOffset);
}

Credits

Hardi Kurnianto
19 projects • 18 followers
Master student at Intelligent Control and Systems Engineering Department

Comments

Related channels and tags
  • Arduino
  • Robotics
  • Robotics
  • SparkFun
  • TDK Corporation