Self Balancing Robot

1,691

22

10

Posted

Introduction: Self Balancing Robot

Are self-balancing robots old? No! not at all for all the enthusiast available out there.It is amazing to see these bot struggling to balance. Actually, I have already made this bot once but that was without PID. This version has a PID embedded in it with less coding complexity.

Part List

1. DC Motors or better be servo motors or better be stepper motors

2. L293D

3. 0.1uF X 4 Ceramic Capacitors

4. Bluetooth Module HC05

5. 0.1 uF Ceramic Capacitor

6. Hot Glue

7. Arduino (Any version of your hobby box)

8. GY-521 Gyro+Accelerometer

Step 1: What Is This Actually ?

I 'll be straight. You have a stick. You are holding it vertically on your palm. It starts falling on one side and you move your hand to that direction to maintain its balance. This is done to maintain the center of gravity within the vertical line.The top and bottom of the stick are the limit points. If COG lies outside the line joining this line then it will lose balance. To maintain its balance it what we are gonna do.

Step 2: Building

I have used a breadboard to assemble the circuitry. You are free to choose yours. I am using ceramic capacitors to filter out the noise from DC Motors. You must attach two ceramic capacitors across the 5v and 36V terminals of L293D for more noise filter. This is necessary as the noise may turn off your MPU6050 or HC05 or may even reset your Arduino.

I have used HC05 to upload my sketches wirelessly. Serial Monitor can also be accessed wirelessly.

If you are using servos having low RPM then use larger wheels. If you are using DC motors with high RPM then you can use smaller wheels.

Step 3: Make Your Robot Frame

Apply Glue to bot motors. Then use the Acrylic sheet to join them. I have painted them white.

Step 4: Add All the Electronics.

Mount the weight on top if possible. Mount your Gyro along the axis to motors. This is done because linear acceleration is least at that point.

Step 5: PID Tuning.

1. Set P = 0, I = 0, D = 0.

2. The increase P until your bot starts oscillating. It may oscillate wildly or even swiftly. It should be between these two.

3. Increase D to reduce the oscillations.

4. Increase I to increase its response. Higher is the I value, quicker is the response time when it's out of balance. Larger the I value, less is the inclination limit.

Step 6: Source Code

Working of the Bot

Do follow my blog guys Visit it here www.hellocodings.com

It appears that Github script and Instructable.com are rivals. Some spaces are missing in the code. Please take care of it.

The only reason why my bot is 98% stable and not 99% is because of backlash error. 100% is not possible since not balance bot can stand still. For a better stability use gripped rubber wheels. Mine had plane surface rubber wheels. Gripped with cut type wheel will increase your stability since stability depends not only on an algorithm or MPU readings but also on better physical characteristics of the robot.

Here is the link if you want for formatted code Code

Self Balancing Robot

/* Developed By: www.hellocodings.com----------------------------
---Shashi Suman--------------------------------------------------
---Modify as you want--------------------------------------------
---------------------------------------------------------------*/
#include<Wire.h>
constint MPU_addr=0x68;
double AccelX,AccelY,AccelZ,Tmp,GyroX,GyroY,GyroZ; //These will be the raw data from the MPU6050.
uint32_t timer; //it's a timer, saved as a big-ass unsigned int. We use it to save times from the "micros()" command and subtract the present time in microseconds from the time stored in timer to calculate the time for each loop.
double Angle_X, Angle_Y; //These are the angles in the complementary filter
#definedegconvert57.29577951//there are like 57 degrees in a radian.
#definem1_left12
#definem1_right10
#definem2_left3
#definem2_right4
#defineKp110
#defineKd0.09
#defineKi90
#definesampleTime0.005
#definetargetAngle0
volatileint output = 0;
volatilefloat currentAngle, prevAngle=0, error, prevError=0, errorSum=0;
voidsetup() {
pinMode(m1_left, OUTPUT);
pinMode(m1_right, OUTPUT);
pinMode(m2_left, OUTPUT);
pinMode(m2_right, OUTPUT);
Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Serial.begin(57600);
delay(100);
//setup starting angle
//1) collect the data
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AccelX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AccelY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AccelZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyroX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyroY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyroZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
double roll = atan2(AccelY, AccelZ)*degconvert;
double pitch = atan2(-AccelX, AccelZ)*degconvert;
double gyroXangle = roll;
double gyroYangle = pitch;
double Angle_X = roll;
double Angle_Y = pitch;
//start a timer
timer = micros();
}
voidcommand(int L_M_Speed, int R_M_Speed) {
if(L_M_Speed >= 0) {
analogWrite(m2_left, L_M_Speed);
digitalWrite(m2_right, LOW);
}
else {
analogWrite(m2_left, 255 + L_M_Speed);
digitalWrite(m2_right, HIGH);
}
if(R_M_Speed >= 0) {
analogWrite(m1_right, R_M_Speed);
digitalWrite(m1_left, LOW);
}
else {
analogWrite(m1_right, 255 + R_M_Speed);
digitalWrite(m1_left, HIGH);
}
}
voidemergency(){
digitalWrite(m2_right, LOW);
digitalWrite(m1_right, LOW);
digitalWrite(m2_left, LOW);
digitalWrite(m1_left, LOW);
}
voidloop(){
//Now begins the main loop.
//Collect raw data from the sensor.
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AccelX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AccelY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AccelZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyroX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyroY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyroZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
double dt = (double)(micros() - timer) / 1000000;
timer = micros();
double roll = atan2(AccelY, AccelZ)*degconvert;
double pitch = atan2(-AccelX, AccelZ)*degconvert;
double gyroXrate = GyroX/131.0;
double gyroYrate = GyroY/131.0;
Angle_X = 0.99 * (Angle_X + gyroXrate * dt) + 0.01 * roll;
Angle_Y = 0.99 * (Angle_Y + gyroYrate * dt) + 0.01 * pitch;
currentAngle = Angle_Y;
if(currentAngle >50 && currentAngle < -50){
emergency();
}
elseif(currentAngle >-0.3 && currentAngle <0.3){
emergency();
delay(50);
}
else{
error = currentAngle - targetAngle;
errorSum = errorSum + error;
errorSum = constrain(errorSum, -300, 300);
//calculate output from P, I and D values
output = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime;
output = constrain(output, -255, 255);
command(output, output);
prevAngle = currentAngle;
}
}
view rawPo.ino hosted with ❤ by GitHub

Self Balancing Robot Arduino

#include<PID_v1.h>
#include<Wire.h>
constint MPU_addr=0x68;
double AccelX,AccelY,AccelZ,Tmp,GyroX,GyroY,GyroZ; //These will be the raw data from the MPU6050.
uint32_t timer; //it's a timer, saved as a big-ass unsigned int. We use it to save times from the "micros()" command and subtract the present time in microseconds from the time stored in timer to calculate the time for each loop.
double Angle_X, Angle_Y; //These are the angles in the complementary filter
#definedegconvert57.29577951//there are like 57 degrees in a radian.
#definem1_left12
#definem1_right10
#definem1_en11
#definem2_left3
#definem2_right4
#definem2_en9
bool f;
double offset = 0.3;
double f_set = 0;
double b_set = 0;
double Kp = 145, Kpx = 225;
double Kd = 120, Kdx = 170;
double Ki = 5, Kix = 20;
double targetAngle = 0;
double input, output;
volatileint control = 0;
volatilefloat currentAngle, prevAngle=0, error, prevError=0, errorSum=0;
PID pid(&input, &output, &targetAngle, Kp, Ki, Kd, DIRECT);
voidsetup() {
pinMode(m1_left, OUTPUT);
pinMode(m1_right, OUTPUT);
pinMode(m2_left, OUTPUT);
pinMode(m2_right, OUTPUT);
Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Serial.begin(57600);
delay(100);
//setup starting angle
//1) collect the data
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AccelX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AccelY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AccelZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyroX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyroY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyroZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
//2) calculate pitch and roll
double roll = atan2(AccelY, AccelZ)*degconvert;
double pitch = atan2(-AccelX, AccelZ)*degconvert;
//3) set the starting angle to this pitch and roll
double gyroXangle = roll;
double gyroYangle = pitch;
double Angle_X = roll;
double Angle_Y = pitch;
input = Angle_X + 3;
//start a timer
timer = micros();
pid.SetMode(AUTOMATIC);
pid.SetSampleTime(5);
pid.SetOutputLimits(-255, 255);
}
voidloop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AccelX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AccelY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AccelZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyroX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyroY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyroZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
double dt = (double)(micros() - timer) / 1000000; //This line does three things: 1) stops the timer, 2)converts the timer's output to seconds from microseconds, 3)casts the value as a double saved to "dt".
timer = micros(); //start the timer again so that we can calculate the next dt.
double roll = atan2(AccelY, AccelZ)*degconvert;
double pitch = atan2(-AccelX, AccelZ)*degconvert;
double gyroXrate = GyroX/131.0;
double gyroYrate = GyroY/131.0;
Angle_X = 0.99 * (Angle_X + gyroXrate * dt) + 0.01 * roll; // Calculate the angle using a Complimentary filter
// Angle_Y = 0.99 * (Angle_Y + gyroYrate * dt) + 0.01 * pitch;
currentAngle = Angle_X+3;
input = currentAngle;
//Serial.println(currentAngle);
if (currentAngle >40.0 || currentAngle < -40.0)
stop();
elseif(currentAngle > -0.3 && currentAngle < +0.3)
stop();
else {
error = currentAngle - targetAngle;
if (abs(error) < 3){
pid.SetTunings(Kp, Ki, Kd);
}
else
pid.SetTunings(Kpx, Kix, Kdx);
pid.Compute();
if(currentAngle > targetAngle && currentAngle < 40){
Serial.println(output);
command(false, output);
}
elseif(currentAngle < targetAngle && currentAngle > -40)
command(true, output);
else
stop();
}
}
voidcommand(bool flag, int pwm) {
//Serial.println(pwm);
if(flag == true) {
analogWrite(m2_en, pwm);
digitalWrite(m2_left, LOW);
digitalWrite(m2_right, HIGH);
analogWrite(m1_en, pwm);
digitalWrite(m1_right, LOW);
digitalWrite(m1_left, HIGH);
}
else {
analogWrite(m2_en, abs(pwm));
digitalWrite(m2_left, HIGH);
digitalWrite(m2_right, LOW);
analogWrite(m1_en, abs(pwm));
digitalWrite(m1_right, HIGH);
digitalWrite(m1_left, LOW);
}
}
voidstop(){
digitalWrite(m2_right, LOW);
digitalWrite(m1_right, LOW);
digitalWrite(m2_left, LOW);
digitalWrite(m1_left, LOW);
}

Share

    Recommendations

    • Science of Cooking

      Science of Cooking
    • Pocket-Sized Contest

      Pocket-Sized Contest
    • Spotless Contest

      Spotless Contest
    user

    We have a be nice policy.
    Please be positive and constructive.

    Tips

    3 Questions

    where is the wiring diagram?

    0

    Hello first off awesome project; I was wondering if it is at all possible to adjust your angles from completely horizontal all the way up to vertical so a full 90 degrees via a rotary dial. My idea is to use it in an RC car so that I can do a wheelie at the turn of a dial and remain in that position until I turn it back down, sorry about the complicated question but thanks for any help I may get

    0

    Hello Suman,
    Greetings!

    Firstly Congratulations on this wonderful project, and I wish to make one myself.
    I want to know, can Arduino Nano be used for this? If so, need any changes be made to the coding?

    Also, what's the use of the Bluetooth Module? I went through your blogpost too, and noticed that perhaps you hadn't included Bluetooth Module while listing down for "self balancing bot without PID".

    Lastly, from your write ups i believe you are using the Capacitors as Filters. Where & how to use them?

    Thank You

    5 Comments

    Hello Suman,
    Greetings!

    Firstly Congratulations on this wonderful project, and I wish to make one myself.
    I want to know, can Arduino Nano be used for this? If so, need any changes be made to the coding?

    Also, what's the use of the Bluetooth Module? I went through your blogpost too, and noticed that perhaps you hadn't included Bluetooth Module while listing down for "self balancing bot without PID".

    Lastly, from your write ups i believe you are using the Capacitors as Filters. Where & how to use them?

    Thank You

    Hello,
    Indeed you can use Arduino Nano. However, a small change is required. Nano outputs 5v whereas Bluetooth Module uses 3.3v logic level. So TX of Nano would go to a voltage divider. The Voltage divider will divide the voltage and provide 3.3v logic for RX of Bluetooth. TX of Bluetooth can straight go RX of Nano as TX of Bluetooth will be at 3.3v level which is acceptable for Nano.

    The Bluetooth I used is HC 05.
    If you can get a 5V logic level Bluetooth, no divider is required. This is the reason I used Pro Mini.

    I have used Bluetooth module to wirelessly upload the code from Arduino IDE (PC or Mac) to my bot without messing with wires. I can even use it as a wireless serial monitor.

    Capacitors are used here to provide a delay output. Whenever HC05 is connected to a device, it's state pin will go high otherwise low. So when my PC is connected to HC05 to upload code, the pin goes high which in turn charges the capacitor. This capacitor then discharges to reset the Arduino.
    Reset is required here to upload the code to Arduino,

    State pin of HC05 will go to one end of Capacitor. The other end of the capacitor will go to the reset pin of Arduino.

    Regards

    I like the instructable and was going to try it but the link to the source code took me to another blog but no source code. Could you make the source available It looks like you are very successful with this bot.

    Well, I have checked the link it has the source code.

    Awesome thanks so much.