Self Balancing Robot With ESP8266-WiFi PID Tuning and Control

Few days back, I purchased several car chassis kits, one of those kits was mini round double-deck robotic car, this chassis come with two 65mm-dia wheels and two caster wheels that can freely move in any direction, instead of using all four wheels, I connected only two wheels and this is how my self balancing robot started!

While searching for tutorials, I came across this nice instructable (you should also check) that explains how self balancing robot works and how to interface and program MPU6050 from Arduino, you can copy MPU6050 interfacing codes from that instructable or from this gitub repo.

here are few issues that led me to add ESP8266 in my design

• Too much waste of time in tuning Kp, Ki, Kd, as need to reprogram Arduino every time we tune them
• Target angle also needs tuning, otherwise robot either moves forward or backward
• One of the two wheels move faster than other at same duty cycle PWM, this is because one motor have slightly different characteristics than other
• Need to drive robot remotely as I'm planning to make a bigger one in future and want my robot to be remote controlled!!

These are few issues that we may face while building a self balancing robot.

This was some background, now let's see how to make this thing happen.

Get these parts,

• round car kit chassis,
• MPU6050 imu sensor
• Arduino pro mini, nano whatever you like to use
• ESP8266, nodemcu, wemos, esp-01, whatever you like to use, we only need UART
• two 3.7V lipo cells
• pwm motor driver, may use L298 if you already have one, any PWM dc motor driver should work.
• jumper wires
• variable output buck converter step down module or any adjustable voltage regulator, robot will work without regulator as well, I'll explain why we need regulated power output.
• I placed a plastic box on my robot to fit different components, but that isn't necessary, you can place components without any plastic box

• Motor driver IN1,IN2 --> Arduino Nano pin 6,11
• Motor driver IN2,IN3 --> Arduino Nano pin 5,3
• Motor driver Motor-A, motor-B --> motors
• don't worry about left/right motors, just connect anyway, later can adjust in software.
• RX of Arduino to TX of ESP8266 and TX of Arduino to RX of ESP8266
• connect both lipo batteries in series, connect this supply to Arduino and motor driver, power ESP8266 from Arduino board
• MPU6050 VCC --> Arduino 5V,
• MPU6050 SCL --> Arduino A5
• MPU6050 SDA --> Arduino A4
• Do we need voltage regulator? We'll figure out later
• MPU-6050 IC marking should be on top side,

In this github repo, you'll find all codes, start with testing MPU6050, load gyro and accelerometer test codes, you'll notice angle isn't exact 0 degree even when you are holding robot straight at 0 degree, so you need to calibrate IMU, for calibration, load this code, run it, type some character in serial monitor and it will give you offset values in few seconds that you'll have to add in this self balancing robot sketch, you can find latest version of this code here

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
#define leftMotorPWMPin   6
#define leftMotorDirPin   11
#define rightMotorPWMPin  5
#define rightMotorDirPin  3

String inputString = "";         // a String to hold incoming data
boolean stringComplete = false;  // whether the string is complete

float Kp= 10.0;
float Kd= 0  ;
float Ki= 0;
float targetAngle = 0;

float motorADutyOffset=1;
float motorBDutyOffset=1;

#define sampleTime  0.005

MPU6050 mpu;

int16_t accY, accZ, gyroX;
volatile int motorPower, gyroRate;
volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0;
volatile byte count=0;


void setMotors(int leftMotorSpeed, int rightMotorSpeed) {
  if(leftMotorSpeed >= 0) {
    analogWrite(leftMotorPWMPin, leftMotorSpeed*motorADutyOffset);
    analogWrite(leftMotorDirPin, 0);
  }
  else {
    analogWrite(leftMotorPWMPin,0);
    analogWrite(leftMotorDirPin,  -leftMotorSpeed*motorADutyOffset);
  }
  if(rightMotorSpeed >= 0) {
    analogWrite(rightMotorPWMPin, rightMotorSpeed*motorBDutyOffset);
    analogWrite(rightMotorDirPin, 0);
  }
  else {
    analogWrite(rightMotorPWMPin,0);
    analogWrite(rightMotorDirPin, -rightMotorSpeed*motorBDutyOffset);
  }
}

void init_PID() {  
  // initialize Timer1
  cli();          // disable global interrupts
  TCCR1A = 0;     // set entire TCCR1A register to 0
  TCCR1B = 0;     // same for TCCR1B    
  // set compare match register to set sample time 5ms
  OCR1A = 9999;    
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS11 bit for prescaling by 8
  TCCR1B |= (1 << CS11);
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);
  sei();          // enable global interrupts
}


void setup() {
  Serial.begin(115200);
  delay(2000);
  inputString.reserve(200);
  // set the motor control and PWM pins to output mode
  pinMode(leftMotorPWMPin, OUTPUT);
  pinMode(leftMotorDirPin, OUTPUT);
  pinMode(rightMotorPWMPin, OUTPUT);
  pinMode(rightMotorDirPin, OUTPUT);
  // set the status LED to output mode 
  pinMode(13, OUTPUT);
  // initialize the MPU6050 and set offset values
  mpu.initialize();
  mpu.setXAccelOffset(-2816);
  mpu.setYAccelOffset(562);
  mpu.setZAccelOffset(1633);
  mpu.setXGyroOffset(-7);
  mpu.setYGyroOffset(33);
  mpu.setZGyroOffset(91);  
  // initialize PID sampling loop
  init_PID();
}
void loop() {
  // read acceleration and gyroscope values
  accY = mpu.getAccelerationY();
  accZ = mpu.getAccelerationZ();  
  gyroX = mpu.getRotationX();
  // set motor power after constraining it
  motorPower = constrain(motorPower, -255, 255);
  setMotors(motorPower, motorPower);
  if (stringComplete) {
    if(inputString.equals("angle:H\r\n"))
    {
     targetAngle+=0.5;
    }
    else if(inputString.equals("angle:L\r\n"))
    {
      targetAngle-=0.5;
    }
    else if(inputString.equals("i:H\r\n"))
    {
      Ki+=0.5;
    }    
    else if(inputString.equals("i:L\r\n"))
    {
      Ki-=0.5;      
    }
    else if(inputString.equals("d:H\r\n"))
    {
      Kd+=0.01;
    }
    else if(inputString.equals("d:L\r\n"))
    {
      Kd-=0.01;
    }
    else if(inputString.equals("p:H\r\n"))
    {
      Kp+=0.5;
    }    
    else if(inputString.equals("p:L\r\n"))
    {
      Kp-=0.5;
    }	
  	else if(inputString.equals("da:H\r\n"))
    {
      motorADutyOffset+=1;
    }    
    else if(inputString.equals("da:L\r\n"))
    {      
      motorADutyOffset-=1;
    }
	 else if(inputString.equals("db:H\r\n"))
    {
      motorBDutyOffset+=1;
    }    
    else if(inputString.equals("db:L\r\n"))
    {
      motorBDutyOffset-=1;
    }  	
    else if(inputString.indexOf("angle:")!=-1)
    {
      targetAngle=(inputString.substring(6)).toFloat();
    }
    else if(inputString.indexOf("p:")!=-1)
    {
      Kp=(inputString.substring(2)).toFloat();
    }
    else if(inputString.indexOf("d:")!=-1)
    {      
      Kd=(inputString.substring(2)).toFloat();
    }
    else if(inputString.indexOf("i:")!=-1)
    {      
      Ki=(inputString.substring(2)).toFloat();
    }
    else if(inputString.indexOf("dutyA:")!=-1)
    {
  	motorADutyOffset=(inputString.substring(6)).toFloat();
    }
    else if(inputString.indexOf("dutyB:")!=-1)
    {
  	motorBDutyOffset=(inputString.substring(6)).toFloat();
    }
    else if(inputString.equals("ok:1\r\n"))
    {
      Serial.print(" Kp:");
      Serial.print(Kp);
      Serial.print(" Ki:");
      Serial.print(Ki);
      Serial.print(" Kd:");
      Serial.print(Kd);
      Serial.print(" targetAngle:");
      Serial.print(targetAngle);
      Serial.print(" A offset:");
      Serial.print(motorADutyOffset);
      Serial.print(" B offset:");
      Serial.println(motorBDutyOffset);	 
    }
    inputString = "";
    stringComplete = false;
  }
}
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}
// The ISR will be called every 5 milliseconds
ISR(TIMER1_COMPA_vect)
{
  // calculate the angle of inclination
  accAngle = atan2(accY, accZ)*RAD_TO_DEG;
  gyroRate = map(gyroX, -32768, 32767, -250, 250);
  gyroAngle = (float)gyroRate*sampleTime;  
  currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);
  
  error = currentAngle - targetAngle;
  errorSum = errorSum + error;  
  errorSum = constrain(errorSum, -300, 300);
  //Serial.print("error sum:");Serial.println(errorSum);
  //calculate output from P, I and D values
  motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime;
  //Serial.print("motor power:");Serial.println(motorPower);
  prevAngle = currentAngle;
  // toggle the led on pin13 every second
  count++;
  if(count == 200)  {
    count = 0;
    digitalWrite(13, !digitalRead(13));
  }
}
<br>

This is modified version of the sketch shared in other instructable that I've already mentioned, it accepts Kp, Ki, Kd, Target angle and Left/Right motor offset parameters from UART, we send these parameters from a webpage hosted on ESP8266, once we are able to balance our robot, we can update these parameters in Arduino sketch and reprogram Arduino, motor offset is actually pwm duty cycle multiplier, don't make it zero, I'll update code with more appropriate names. These are the parameters that I found suitable for my robot.

float Kp=  45.0;
float Kd=-0.2;
float Ki=  135;
float targetAngle = 1.50;
float motorADutyOffset=1.15;
float motorBDutyOffset=1;

I have made this web page that uses zepto JS library, this is same as jQuery but of smaller size, you can find web page directory here, this web page is already inside ESP8266 sketch.

First I added only single function that was sending commands like 'p:H' or 'p:L' etc, p:H means add 0.5 to already set value of Kp, similarly p:L means decrease Kp by 0.5,

function tuneParam(paramToTune,sign){
	    var someUrl = "/Tune?"+paramToTune+"="+sign;
            $.ajax(
			{
				url: someUrl,
				dataType: "text", 
				success: function(response) 
				{
					if (!$.trim(response))
					{   
						
					}
					else
					{   
						alert("configured PID param:" + response);
					}
				}
			})
		}

Arduino was able to decode these commands and I was able to tune Kp, Ki, Kd, but what if you have to set Kp to 100 while current Kp value is 10 for example, you'll have to press increment Kp button almost 180 times, so to fix this issue, I also added text input fields where you can directly write value that you want to configure and press Update button. This new Updated button is handled by this JS function

<p>function tuneValue(paramToTune,val) <br>	{var someUrl = "/Tune?"+paramToTune+"="+val;
		$.ajax({url: someUrl,dataType: "text", 
		success: function(response) {
		if (!$.trim(response)){   
			
		}
		else{   
			alert("configured PID param:" + response);
		}
		}})}</p>

Once you are done with tuning, press Done button two times(should be one, I'll fix in future updates),

Arduino will reply back with all configured values that you will see on webpage, you can then write these values in your Arduino sketch, and at this stage, if you don't like to remotely control your robot, you can just remove ESP8266.

You can find latest version of ESP8266 Arduino sketch here, it starts access point with SSID and password set as

const char* ssid = "wifi-robot";
const char* password = "";

You can connect to wifi-robot from your PC or mobile phone and then can open http://192.168.4.1 to access web page,

I've also added OTA option, so to update ESP8266 firmware anytime, you can just access

http://192.168.4.1/update and upload new binary file.

Other than handling HTTP GET requests, there is SerialEvent function, that we call inside loop() thread,

String pidData="";
String inputString = "";         // a String to hold incoming data
boolean stringComplete = false;  // whether the string is complete
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    inputString += inChar;
     if (inChar == 0x0A) {
      stringComplete = true;
      pidData="";
      pidData+=inputString;
      inputString="";
    }
  }
}

This function receives UART data from Arduino and we use JS alert() function to display this data on web page, in future update, I'll replace alert with a text field to show configured parameters.

Ok, I tuned my robot and it was almost perfect, next day, it wasn't, I had to tune it again and it was walking again, recharged batteries and it was again dead and so had to tune again, so I figured out it was issue with variable voltage level, as voltage goes up or down, same duty cycle PWM can result different motor RPM, so I added mini buck converter and set its output to 6V, this is only for motor driver, Arduino VIN can be directly connected to batteries. Step down buck converter have only four pins, two for input and two for output, and you can adjust regulator output to same as your motors maximum voltage level.

Target angle is the angle that your robot will try to achieve, you can set it to 0 and you will notice your robot either moving forward or backward, this may be because MPU-6050 isn't exactly facing upward, or may be robot have more weight on one side as compared to other, so you can increase it or decrease it and you'll see that at some target angle, robot will be able to stay at one position instead of moving forward and backward.

How to move robot forward and backward?

Just increase or decrease the angle little bit and your robot will start walking.

This was another issue that I faced, all motors have slightly different electrical/mechanical characteristics, and so can give slightly different RPM at same voltage level. One option is to use encoder disks with tires and use some RPM sensor to make sure both tires rotate at same RPM, but this will add complexity to your design, as you may need 2nd PID controller to apply desired RPM.

One simple fix is to use a multiplier for PWM duty cycle, select multiplier value greater than 1 for the tire that move slow, this fix worked for me and I've added option in web page to tune this parameter.

I've exposed three wires, first Gnd wire(0V), 2nd from the middle of series connection(4.25V) and 3rd positive terminal(8.5V), this is how I can easily recharge batteries.

I'll try to add more improvements, stay tuned!

• So my robot is now left with single motor, and it can still balance itself.
• I've removed Arduino nano and now left with only ESP8266. Still works fine.