Self Balancing Robot From Magicbit

196

Introduction: 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.

Supplies

• 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

Step 1: Story

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.

Step 2: Theory and Methodology

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.

Step 3: Hardware Setup

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.

Step 4: Software Setup

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

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

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

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.

Step 6: Code

#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; // FIFO storage buffer

Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr;           // [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
{
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)..."));
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..."));

// 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

// 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 * 180/M_PI);//euler angles
Serial.print("\t");
Serial.print(ypr * 180/M_PI);
Serial.print("\t");
Serial.println(ypr * 180/M_PI);
#endif
input = ypr * 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);
}