2 Wheel Self Balancing Robot From Broken Toy Car : 17 Steps (with Pictures)

As usual, first of all, I would say, English is not my native language; so please excuse any errors on my part!!

Today, I'd like to share with you how to make a 2 wheel self-balancing robot with Arduino from my boy's broken toy car. Of course, two motors and two wheels are still good. laugh. Balancing itself on two wheels is similar to balancing a stick on our finger. I think everyone have ever tried it before!? To balance, we have to move our fingers quickly or slowly following inclination's direction and inclination's speed of stick. Let's start and discover how Arduino can adjust itself.

Step 1: Preperation

We need to take our times to study the following information:

P.I.D Closed Loop Control:

https://en.wikipedia.org/wiki/PID_controller

PID_v1 library from Brett Beauregard.

MPU-6050 with I2C Protocol:

https://www.i2cdevlib.com/devices/mpu6050#source

Inverted pendulum:

https://en.wikipedia.org/wiki/Inverted_pendulum

Step 2: MPU-6050

The MPU-6050™ from InvenSense, are the world’s first MotionTracking devices designed for the low power, low cost, and high-performance. It is integrated a 3-axis gyroscope and a 3-axis accelerometer
- Tri-Axis angular rate sensor (gyro) with a sensitivity up to 131 LSBs/dps and a full-scale range of ±250, ±500, ±1000, and ±2000dps.
- Tri-Axis accelerometer with a programmable full scale range of ±2g, ±4g, ±8g and ±16g.
When learning about the MPU-6050, we will encounter the terms “YAW”, “PITCH”, and “ROLL”. And to me, this is the simplest and most straightforward explanation:

Article below is good reference for yaw, pitch, raw motion.

How many types of motion that an aircraft can do. What is the name of the axis about which the motion occurs?

An aircraft can do three types of motion. It can pitch, roll and yaw.

Pitching motion is the motion in which the aircraft’s nose goes up or down. Pitching motion occurs about the lateral axis. (See Fig 1 [a] - [b] - [c]).
Rolling motion is the motion in which one of the aircraft’s wings drops while the other rises. For instance, if the aircraft is rolling to the left, the left wing is dropping while the right wing is rising. Rolling motion occurs about the longitudinal axis. (See Fig 2 [a] - [b] - [c]).
Yawing motion is the motion in which the aircraft's nose moves to the right or to the left. Yawing motion occurs about the normal axis [vertical axis]. (See Fig 3 [a] - [b] - [c]).

Stability and control are much more complex for an airplane, which can move freely in three dimensions, than for cars or boats, which only move in two. A change in any one of the three types of motion affects the other two. But understanding the movements of the aircraft above will help us to easily visualize how the self-balancing robot works!

You can see all motion simulations on video above or link:

Take note that gyroscope measures the angular velocity. It is the number of degrees that is rotated in a second, so called degrees/second.
In addition, you need to calibrate the offsets for MPU-6050, check this topic:

http://www.i2cdevlib.com/forums/topic/91-how-to-de...

Step 3: Sensor Fusion

"Sensor fusion" is the art of combining multiple physical sensors, performs some complex calculations to minimize the errors in each sensor, even though each sensor might be unreliable on its own. Accelerometers and gyros have different inherent limitations, when used on their own.

The accelerometer has no accumulated error but has the gravity component problem.
The gyro has accumulated error but is not sensitive to gravity.

-->Sensor fusion: combining 2 sensors together to compensate each other’s weakness.

Check picture above for MPU-6050 connection diagram.

Step 4: P.I.D Closed Loop Control

Here are my P.I.D tests for DC servo motor with SPEED and POSITION control through potentiometer. Let take a look through diagram above & videos below, it may be useful for you before coming to next part.

SPEED CONTROL

POSITION CONTROL

Step 5: B.O.M

I reused 2 DC motors and 2 wheels from broken toy car for 1st version. The 2nd version, I bought new motors and wheels.

Wheel diameter 65mm.
DC Gear-Box Reducer Electric Motor GA25 620RPM.

Step 6: SCHEMATIC

This is circuit diagram for self balancing robot, include 3 PID tuning potentiometers. In my real design, I add one more potentiometer for options, like adjust motor speed or adjust set point...., & it looks symmetrical.

Step 7: Modify Broken Car (1st Version)

Let see pictures showing how to do a robot from broken toy car.

Step 8: Better Self Balancing Robot (2nd Version)

Because my son loved a version of self balancing robot from his toy. So I had to buy another 2 dc motors and wheels as describing on B.O.M.

Everything is the same as previous version except for motors, wheels and robot's frames.

Step 9: Robot With Led Message (3rd Version)

I added one more layer with LoLShield (Lots of Lights Shield) that I made it by myself previous time. With LoLShield, it consists of 126 single leds arranged into 9x14 matrix & controlled by Charliplexing method to save Arduino pins. It's designed as a "Shield" for Arduino & operating without limit current resistor for LED.

It's so cool now!!!

Step 10: PID CONTROL DIAGRAM

Self-balancing robot is similar to an inverted pendulum and balancing an inverted pendulum is a challenge, because it is not stable. To deal with this problem, we applied a PID controller that uses angle feedback, generate PWM to control motors and keep the robot balancing.

The P.I.D Set Point (SP) is the equilibrium point (vertical) that can be found during calibration. Or it can be slightly adjusted at fine-tuning period. If robot's body is perfect, balanced and symmetrical, then Set Point will be 180,0 degree, in my case it is 180.70 degree. Why 180,0 or 180.70 degree, let read the program below. Feedback signal or Process Variable (PV) is a combination of Gyroscope and Accelerometer collected from the MPU-6050. The P.I.D Output is PWM signal that continuously control two DC motors so that the PV achieve the SP.

An simplistic self balancing algorithm with PID controller would be applied as following:

If robot is tilting forward, PID controller will adjust motors to move wheels in the direction forward until got balance point (Set Point).

Else, if robot is tilting backward, PID controller will adjust motors to move wheels in the backward direction until got balance point (Set Point)..
Else, robot's at balance point, do nothing.

As I said before, the gyroscope can measure the tilt speed of robot so feedback signal from the MPU-6050 will give PID controller a proper adjustments based on inclination angle & inclination speed.

With addition of potentiometers to adjust P, I, D coefficients, it saved us an immeasurable amount of time while tuning the PID controller. The coefficients P, I and D are found as follows:

Set all P, I, D potentiometers to 0.
Increase P potentiometer until the robot starts to oscillate rapidly around the equilibrium point but the robot does not fall.
Increase D potentiometer until robot is no longer oscillating. At this time, robot works relatively stable but will be oscillated when pushing at one side.
Gradually increase I potentiometer until the system operates smoothly even if robot is pushed at one side. If "I coefficient" becomes big it will cause robot to respond slowly.

Step 11: Robot Library

For my code, we need to include the following libraries in main program:

Step 12: Main Program

Two_Wheel_Self_Balance_Robot

/*******************************************************************************************
 * Two Wheel Self Balancing Robot
 *
 * REVISION
 * ________________________________________________________________________________________
 *
 * Main reference from Franco Robot (https://github.com/lukagabric)
 *
 ******************************************************************************************/
#include "PID_v1.h"
#include "LMotorController.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif
// uncomment "OUTPUT_TEAPOT" if you want output that matches the
// format used for the InvenSense teapot demo
#define OUTPUT_TEAPOT 0 // Only for testing with processing "Teapot" program
#define LOG_INPUT 0
#define MANUAL_TUNING 1
#define LOG_PID_CONSTANTS 0 //MANUAL_TUNING must be 1
#define MOVE_BACK_FORTH 0
#define MIN_ABS_SPEED 5
//MPU
#define MPU_YPR_SELECT 1
MPU6050 mpu;
// MPU control/status vars
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
// orientation/motion vars
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
// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
//PID
#if MANUAL_TUNING
  double kp , ki, kd;
  double prevKp, prevKi, prevKd;
#endif
double originalSetpoint = 180.70;// 181.13
double setpoint = originalSetpoint;
double movingAngleOffset = 0.15;// 0.3- OK, 0.15 - OK
double input, output;
int moveState=0; //0 = balance; 1 = back; 2 = forth
#if MANUAL_TUNING
  PID pid(&input, &output, &setpoint, 0, 0, 0, DIRECT);
#else
  PID pid(&input, &output, &setpoint, 10.50, 67.44, 0.88, DIRECT);// time 5ms & 10ms, sometimes Kp(17.35, 16.86) Ki(302.05, 301.05) Kd(1.21)
#endif
//MOTOR CONTROLLER
int ENA = 3;
int IN1 = 4;
int IN2 = 8;
int IN3 = 5;
int IN4 = 7;
int ENB = 6;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, 1, 1);
//timers
long time1Hz = 0;
long time5Hz = 0;
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
    mpuInterrupt = true;
}
void setup()
{
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif
    // 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(115200);
    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(39);
    mpu.setYGyroOffset(14);
    mpu.setZGyroOffset(6);
    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(0, 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(5);// 10 - OK, 5 - GOOD, 1- CHANGE PID
        pid.SetOutputLimits(-255, 255);// 80 - OK Strong enough  
    }
    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)
    {
        //no mpu data - performing PID calculations and output to motors
        pid.Compute();
        motorController.move(output, MIN_ABS_SPEED);
        unsigned long currentMillis = millis();
        if (currentMillis - time1Hz >= 1000)
        {
            loopAt1Hz();
            time1Hz = currentMillis;
        }
        if (currentMillis - time5Hz >= 5000)
        {
            loopAt5Hz();
            time5Hz = currentMillis;
        }
    }
    // 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);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
            Serial.print("\tout: ");
            Serial.println(ypr[MPU_YPR_SELECT] * 180/M_PI + 180);
        #endif
        input = ypr[MPU_YPR_SELECT] * 180/M_PI + 180;  
        #ifdef OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
        #endif            
   }
}
void loopAt1Hz()
{
#if MANUAL_TUNING
    setPIDTuningValues();
#endif
}
void loopAt5Hz()
{
    #if MOVE_BACK_FORTH
        moveBackForth();
    #endif
}
//move back and forth
void moveBackForth()
{
    moveState++;
    if (moveState > 2) moveState = 0;
    if (moveState == 0)
      setpoint = originalSetpoint;
    else if (moveState == 1)
      setpoint = originalSetpoint - movingAngleOffset;
    else
      setpoint = originalSetpoint + movingAngleOffset;
}
//PID Tuning (3 potentiometers)
#if MANUAL_TUNING
void setPIDTuningValues()
{
    readPIDTuningValues();
    if (kp != prevKp || ki != prevKi || kd != prevKd)
    {
#if LOG_PID_CONSTANTS
        Serial.print(kp);Serial.print(", ");Serial.print(ki);Serial.print(", ");Serial.println(kd);
#endif
        pid.SetTunings(kp, ki, kd);
        prevKp = kp; prevKi = ki; prevKd = kd;
    }
}
void readPIDTuningValues()
{
    int potKp = analogRead(A0);
    int potKi = analogRead(A1);
    int potKd = analogRead(A2);
    kp = map(potKp, 0, 1023, 0, 25000) / 100.0; //0 - 250
    ki = map(potKi, 0, 1023, 0, 100000) / 100.0; //0 - 1000
    kd = map(potKd, 0, 1023, 0, 500) / 100.0; //0 - 5
}
#endif

view raw Two_Wheel_Self_Balance_Robot hosted with ❤ by GitHub

Step 13: Program Explanation

KP, KI, KD coefficients can be found easily through 3 potentiometers following previous step. Another way is tuning PID through Serial but it will not work effectively due to the Arduino randomly pending / freezing. Let check the topic:

https://github.com/jrowberg/i2cdevlib/issues/252

With main program, if you got trouble like hanging/ pending/ freezing, try running it without the Serial prints.

With 2nd & 3rd version robots, balance point is 180.70 degree & the P.I.D coefficients are:

KP = 10.50.
KI = 67.44.
KD = 0.88.

With toy version robots, balance point is 178.70 degree & the P.I.D coefficients are:

KP = 17.10.
KI = 301.07.
KD = 1.20

Step 14: Testing Toy Robot

Robot from toy is easy to find balance points because of its small spines. Due to broken toy, it is slightly crooked. As video, it worked still good.

Step 15: Testing 2nd Version Robot

First test:

Carry things test:

Carry things and un-leveled road test:

Slope test:

The two-wheeled self-balancing robot worked well even on rugged roads, sloping surfaces, on rugs, on mattresses and carrying lightweight things on it. It can also regain balance when it is rotated left - right or pushed forward - backward.

Step 16: Testing Robot With Led Message (LoLShield)

It showed some animations as: test scrolling, double sine wave, plasma effect and so on.....

I'd satisfied with this results!

Step 17: Summary

Fine-tuning PID parameters through potentiometers will give us a better understanding of the closed loop control that is inherently complicated and objects motion simulation through the MPU-6050 gives us a real sense of motion in space. With above sharing, I think it will definitely be many mistakes. I am looking forward to your contributed suggestions to do better.

The self-balancing robot project is an exciting experience because it's the first time my son loved the toy that I created and started asking questions about the Arduino. It also helped me to relieve stress and seemed to find a balance for myself.

You can see all videos at:

https://www.youtube.com/playlist?list=PLfrr5s2He43...

HAPPY NEW YEAR 2018!!!