So this is my balancing robot. I have had a look at others on Instructables and decided my version is worth writing up. Sometimes it nice to have a few different versions to looks because there are several ways to achieve this goal!

Firstly I am going to state that my programming ability is very poor! I program in a very long-winded basic way, And I generally like to understand what I am doing so don’t tend to use library’s. Also this robot is what I would say a “Really good starting point” it’s by no ways perfect but all the bits work and improvements to the program will only make it better.

BALANCE ROBOT.pdf
Download

Step 1: The Main Body.

There are not many parts to this robot it basically consists of the following

1 Two suitable motors with wheels.

2 Motor control board.

3 Microprocessor

4 Sensor

5 And let’s not forget the battery and body.

Body.

So I am going to look at these separately starting with the body.

I have made the body from 1/8” plywood and whilst I have designed it to take servos it could be modified for other motor types. The pieces of ply were cut out on my fret saw and in the case of the sides were cut out in pairs. The top and middle plates were cut out together then modified to suit. The top piece has a hole in the middle so the sensor can be seen, and the middle plate has mounting holes drilled for the motor control board.

Once I was happy with the fit of all the bits I glued them together and then added doublers to the servo mounting area. The last wood bit is the strip which holds the sensor underneath the top.

Step 2: Motors and Wheels.

This is the second balancing robot I have built. The first one used servos converted to continuous rotation. This one still uses servos but all the electronics have been removed and the servo is basically used as a convent motor and gearbox and driven by a L298N Dual H bridge board.

To convert a servo to continues rotation you need to do the following.

1. Open up the servo.

2. Identify the connections to the pot and to the motor, you may find the pot is connected via wires to the PCB, or as in my case the pot has been fitted first then the PCB placed over the pot legs and soldered in. either way you will have to get the pot out and if you are going to keep the PCB control replace the POT with fixed resistors.

3. If you want to drive the servo via a drive board like the L298N then you will need to remove the PCB totally and solder wires onto the motor.

4. The next stage is to allow the main output to rotate fully. Normally the servo horn will be limited to 180 degrees of travel. On the main output gear identify what is preventing the gear turning fully and remove! I tend to choose servos with metal gears as there is normally a pin stopping the travel. If you choose a plastic geared servo you may find that the output gear doesn’t have teeth around the whole perimeter, in which case it can’t be converted.

5. Now you have done the modifications you can put the servo back together again, but before you do make sure you haven’t got any dust/dirt in the gears.

The wheels took a bit of time to modify. Basically you have to open up the hole and then screw a servo horn to the hub. The pictures show how I have done this.

Step 3: Arduino UNO, MPU6050, L298N.

Microcontroller

I have used a ARDUINO UNO for this project. The Arduino is so well supported that projects like this are relatively easy to do. Also this is a clone UNO and is only a few pounds.

Sensor MPU6050

This sensor is very easy to use and easy to pick up on Ebay. The chip has a built in Digital Motion Processor (DMP), but I have chosen to read the values and do my own code.

Motor control board.

The first robot I made used servos and the code had to give out a pulse to control the direction and speed of the servo. So this time I wanted to drive the motor via a L298N chip. The modules you get from the likes of eBay are easy to use. And although there is an Arduino library I chose to do the simple code. Basically you have two logic level input (IN1/2) and one pulse (ENA) to drive the motor in one direction you set one logic input high and the other low, to drive the opposite way you swop the logics around, to change the speed you adjust the pulse width. To stop with a break you set both to the same logic level. If you wish to freewheel then the pulse should be set to zero.

Step 4: Program Part 1, Reading the Sensor and Taking an Averaging Reading

Please dont judge my poor programming ability!
Below you can see the setup section. In this section i have carried out a neutral valve check to enable any offset to be taken into consideration.
This works by taking the average of 32 readings of the two relevant readings i need, which are gx and ay. this gives you two offset values which are used in calculations later.
After the Averaging section i then define the pins for the L295N, there isn't much to say here other than you must choose PWM pins for ENA and ENB.


void setup()
{
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    Wire.begin();
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
  #endif
  Serial.begin(115200);
  accelgyro.initialize();
  for (int i = 1; i < 33; i = i + 1)
  {
    accelgyro.getRotation(&gx, &gy, &gz);
    sumgx = sumgx + gx;
  }
  sumgx = sumgx / 32;
  for (int i = 1; i < 33; i = i + 1)
  {
    accelgyro.getAcceleration(&ax, &ay, &az);
    sumay = sumay + ay;
  }
  sumay = sumay / 32;
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT); 
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);   
}

Step 5: Program Part 2, Reading the Sensor and Using a Complementary Filter.

So this is where my first bodge occurs!!!
So to allow the robot to balance you need to know two different things.
1) What angle the robot is at? (it's not level and falling over)
2) How fast it is moving? (how fast its falling over)
So for these two readings you use the ACCELEROMETER for the angle and the GYRO for speed of rotation.
Basically the MPU6050 is very accurate however its has a problem, the GYRO readings are accurate but only give a reading when movement occurs. The ACCELEROMETER data which detects the angle of the sensor is very shaky and not very good to use it its raw condition. If you consider the gyro value you could if theory work out the angle by looking at how fast it is rotating in a particular time frame. Then you won't have to use the shaky accelerometer reading. BUT this wont work, because slight errors in the calculation and timing and reversing direction all cause the value to drift. (remember its not giving the angle its giving you how fast its turning)
This is where we come to the complementary filter. Basically you use mostly the reading gained fro the gyro (98%) and then use 2% of the accelerometer to correct the drift.
the Bodge i have used is to NOT convert any readings to any particular unit. For example the gyro could be scaled to give you deg/sec and the accelerometer deg, but i figured the UNO doesn't really care what the units are, it just needs to know the raw values of ay and gx.


  currentTime = micros();
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  accelData = (ay-sumay)>>2;// / by 4
  accelTotal = accelData + setpoint;
  gyroData = (gx-sumgx)>>1;// / by 2
  input = (0.98*((lastInput)+((gyroData)*cycleTime)))+(0.02*(accelTotal));
  /*
   * the above takes the last reading of gyro (lastInput), and then takes the current 
   * reading * cycle time (to work out a turning movement in a time frame)
   * and then adds the two together. this works out how much it has turned since last time
   * this is then multiplied by 98% and then added to 2% of the current accel reading.
   */

At the end of this section for have an input value for the PID calculation. (which is next)

Step 6: Program Part 3, PID. and L298n

This next section is really clever and i have to be honest this is the section i had to do some research for. I spent some time reading over "Improving the Beginner’s PID" by Brett Beauregard. Please check out his blogg for the details and its a really good guide to the PID workings.

Brett Beauregard's PID guide


void calcOutput()
{
  error = setpoint - input;
  error = error;
  errSum = errSum + (error * cycleTime); //intergral part sum of errors past
  double dInput = input - lastInput;
  output = (kp * error) + (ki * errSum) - (kd * dInput);
}

Now the motor section is my "code" you could just use the library? but i didn't think it was to much of a problem.
Basically the output form the PID is a number (i don't really care how big!!) and that number will be either positive or negative. when i checked out the values on the serial port i realised it was far to big for a PWM number which is limited to 255 so i decided to divide it by two then just limit the output to 255. (i told you this was a crude robot!)
I think the code below is easy to read, if the robot is falling one way it sets IN1 HIGH and IN2 LOW and then the ENA to the output valve however because the output value will be negative in one direction i have multiplied the output by -1 to make it positive. i am sure there is a better way to do this but i don't really care as i understand this code!
to be honest driving the L298n is easy.


void MotorL298()
{
  output = (output / 2);
  if (output > 255)
  {
    output = 255;
  }
  if (output < -255)
  {
    output = -255;
  }
  if (output < 0)
  {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    analogWrite(ENA, (output*-1));
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    analogWrite(ENB, (output*-1));
  }
  else
  {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    analogWrite(ENA, output);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    analogWrite(ENB, output);
  }
}

Step 7: The Whole Program

The whole program is included here. you will see that there are other sections for reading the serial port and displaying information. I needed to have these sections to allow me to run the robot connected to the computer and adjust the values of the PID settings.

I hope you have enjoyed this rather crude balancing robot and i hope if you are going to make one then this is useful in some way!