Self Balancing Robot From Magicbit

This tutorial shows how to make a self balancing robot using Magicbit dev board. We are using magicbit as the development board in this project which is based on ESP32. Therefore any ESP32 development board can be used in this project.

• magicbit
• Dual H-bridge L298 motor driver
• Linear Regulator (7805)
• Lipo 7.4V 700mah battery
• Inertial Measurement Unit (IMU) (6 deg of freedom)
• gear motors 3V-6V DC

Hey guys, today in this tutorial we will learn about little bit complex thing. That is about self balancing robot using Magicbit with Arduino IDE. So lets get start.

First of all, lets look at what is self balancing robot. Self balancing robot is two wheeled robot. The special feature is that robot can balancing itself without using any external support. When the power is on the robot will stand up and then it balanced continuously by using oscillation movements. So now all you have some rough idea about self balancing robot.

To balance the robot, first we get data from some sensor to measure the robot angle to vertical plane. For that purpose we used MPU6050. After getting the data from the sensor we calculate the tilt to vertical plane. If robot at straight and balanced position, then the tilt angle is zero. If not, then tilt angle is positive or negative value. If robot is tilt to front side, then robot should move to to front direction. Also if the robot is tilt to reverse side then robot should move to reverse direction. If this tilt angle is high then the response speed should be high. Vice versa the tilt angle is low then the reaction speed is should be low. To control this process we used specific theorem called PID. PID is control system which used to control many process. PID stands for 3 processes.

• P- proportional
• I- integral
• D- derivative

Every system have input and output. In the same way this control system also have some input. In this control system that is the deviation from stable state. We called that as error. In our robot, error is the tilt angle from vertical plane. If robot is balanced then the tilt angle is zero. So the error value will be zero. Therefor the output of the PID system is zero. This system includes three separate mathematical processes.

• First one is multiply error from numeric gain. This gain is usually called as Kp.

P=error*Kp

• Second one is generate the integral of the error in the time domain and multiply it from some of gain. This gain called as Ki

I= Integral(error)*Ki

• Third one is derivative of the error in the time domain and multiply it by some amount of gain. This gain is called as Kd

D=(d(error)/dt)*kd

After adding above operations we get our final output

OUTPUT=P+I+D

Because of the P part robot can get stable position which as proportional to the deviation. I part calculates the area of error vs time graph. So it tries to get the robot to stable position always accurately. D part measures the slope in time vs error graph. If the error is increasing this value is positive. If the error is decreasing this value is is negative. Because of that, when the robot is moves to stable position then the reaction speed will be decreased and this will helpful to remove unnecessary overshoots. You can learn about more about PID theory from this link shown in below.

https://www.arrow.com/en/research-and-events/articles/pid-controller-basics-and-tutorial-pid-implementation-in-arduino

The output of the PID function is limit to 0-255 range(8 bit PWM resolution) and that will feed to motors as PWM signal.

Now this is hardware setup part. The design of the robot is depends on you. When you designed robot's body you have to consider symmetric it about vertical axis which lies in motor axis. The battery pack located below. Therefor the robot is easy to balance. In our design we fix Magicbit board vertically to the body. We used two 12V gear motors. But you can use any kind of gear motors. that is depend on your robot dimensions.

When we discuss about circuit it's powered by 7.4V Lipo battery. Magicbit used 5V for powering. Therefor we used 7805 regulator to regulate battery voltage to 5V. In later versions of Magicbit that regulator is not need. Because it powering up to 12V. We directly supply 7.4V for motor driver.

Connect all components according to below diagram.

In the code we used PID library for calculate PID output.

Go to following link to download it.

https://www.arduinolibraries.info/libraries/pid

Download the latest version of it.

To get better sensor readings we used DMP library. DMP stands for digital motion process. This is inbuilt feature of MPU6050. This chip have integrated motion process unit. So it takes reading and analysed. After it generates noiseless accurate outputs to the microcontroller (in this case Magicbit(ESP32)). But there are lot of works in microcontroller side to take that readings and calculate the angle. So to simply that we used MPU6050 DMP library. Download it by goint to following link.

https://github.com/ElectronicCats/mpu6050

To install the libraries, in the Arduino menu go to tools-> include library->add.zip library and select the library file which you downloaded.

In the code you have to change the setpoint angle correctly. The PID constant values are different from robot to robot. So in tuning that, first set Ki and Kd values zero and then increase Kp until you get better reaction speed. More Kp causes for more overshoots. Then increase Kd value. Increase it by always in very little amount. This value is generally low than other values. Now increase the Ki until you have very good stability.

Select the correct COM port and board type the. upload the code. Now you can play with your DIY robot.

#include <PID_v1.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif



MPU6050 mpu;


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

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


double originalSetpoint = 172.5;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;
int moveState=0;
double Kp = 23;//set P first
double Kd = 0.8;//this value generally small
double Ki = 300;//this value should be high for better stability
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);//pid initialize

int motL1=26;//4 pins for motor drive
int motL2=2;
int motR1=27;
int motR2=4;




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


void setup()
{
  ledcSetup(0, 20000, 8);//pwm setup
  ledcSetup(1, 20000, 8);
  ledcSetup(2, 20000, 8);
  ledcSetup(3, 20000, 8);
  ledcAttachPin(motL1, 0);//pinmode of motors
  ledcAttachPin(motL2, 1);
  ledcAttachPin(motR1, 2);
  ledcAttachPin(motR2, 3);
    // join I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
  Serial.println(F("Initializing I2C devices..."));

  pinMode(14, INPUT);
    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    Serial.begin(9600);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

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

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); // 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
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(14, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
        
        //setup PID
        
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-255, 255);  
    }
    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 "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
}


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)
   {
        
        pid.Compute();//this time period is used to load data,so you can use this for other calculations
        motorSpeed(output);
        
    }

    // 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
        mpu.resetFIFO();
        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;

        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        #if LOG_INPUT
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);//euler angles
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif
        input = ypr[1] * 180/M_PI + 180;
   }
}
void motorSpeed(int PWM){
  
float L1,L2,R1,R2;

if(PWM>=0){//forward direction
 
  L2=0;
  L1=abs(PWM);
  R2=0;
  R1=abs(PWM);
  if(L1>=255){
    L1=R1=255;
  }
}
else {//backward direction
  L1=0;
   L2=abs(PWM);
   R1=0;
  R2=abs(PWM);
  if(L2>=255){
    L2=R2=255;
  }
}

//motor drive
ledcWrite(0,  L1);
ledcWrite(1,  L2);
ledcWrite(2,  R1*0.97);//0.97 is speed factor,because right motor have high speed than left motor,so we reduce it until motor speeds are equal
ledcWrite(3,  R2*0.97);

}