Everyone wants to build a balancing robot. But it's complicated, involves heavy math and knowledge of a sensor fusion algorithm called Kalman Filter. What if you can build a simpler version, even transform your $50 robot into a balancing robot?
Well, some guys on the Parallax forums did it with a BasicStamp2sx. Oh, but that is Stamp and Basic. What about AVR and C? Can it be done? Sure it can! This paper will describe a simple balancing robot using 2 continuous rotation servos and a Ping))) ultrasonic sensor on a $50 robot or an Arduino compatible board, with a Mega168 microcontroller. It uses the Ping))) sensor to read the distance to the ground and command the servos to move until the distance becomes equal to the preset value for a perfect balance.
If you have build a $50 robot (and it works) this project is half way done. All you need is a different frame to attach the servos, the electronic board, the sensor and the batteries, so it looks like a... hmm, it really looks like a dolly! Then program it and watch it balance!
Front view -> Back view->
Want to see it in action? I just took a short video recording this morning for you! Sorry, I got an error if I wanted to embed the Youtube video directly in the HTML code...
List of materials.
Here is a list of materials needed for the $50 robot users:
- about 2 feet of right angled PVC (or aluminum) bar, 1"x1" wide
- a small box of small nuts and bolts
- 2 standoffs 1 1/2" long
- one Parallax Ping))) ultrasonic sensor
- the $50 robot
If you have an Arduino board (and no $50 robot), you also need:
- 2 servos modified for continuous rotation
- 2 large wheels (over 3" diameter)
- a battery box
Building the chassis.
The chassis can be done in different ways. You can get a rectangular board, some double side sticky foam and use it to attach all the parts on the board. Or you can go my way and build a long lasting chassis from a piece of an L shaped right angled PVC (or aluminum) bar.
To minimize the number of nuts and bolts used, I decided to bend the bar in a rectangular shape, like a box frame. I measured the total length and width of the parts for the robot, layed them along the bar, marked both sides of the bar where the bend lines will be and cut one side along the marked line. On each side of the cut I marked a line at 45 degrees, doing the same for the ends of the bar, then I cut them off. Using a pair of pliers I have carefully bended the bar into a rectangular shape. On the outer side, I marked and cut out 2 rectangles to mount the servos, then drilled the holes and mounted them.
On the interior side (or the bottom of the box frame) I marked the place for the electronics board and drilled the holes for it.
Between the servos and the electronics board, on the bottom of the frame, I marked and drilled 2 more holes to mount the sensor, then I mounted the electronics board.
On the top of the frame I have used a rubber band to hold in place a 6 AA battery box. To mount the sensor I needed a piece of the same L shaped PVC bar. I marked on one side the place for the ultrasonic elements and the sensor's LED and mounting holes and drilled them. On the other side I marked and drilled 2 holes matching the ones from the chassis. Using 2 standoffs I mounted the sensor on the chassis, facing away from the end where I mounted the battery box.
Using a longer metal screw similar with the one that came with the servos, I have mounted 2 large motorcycle wheels from a Lego set, over the round servo horns that came with the servos. You can use any other wheels with a bigger diameter than 3".
$50 robot users will have to keep their servos connected where they are and connect the Ping))) sensor on any free digital pin. I will let you know exactly after I try the C code.
Arduino users will have to connect the servos tho the digital pins 4 and 5 and the Ping))) sensor on digital pin 7. A very good example of how to connect parts to Arduino (with pictures!) you can find here: http://wiring.org.co/learning/tutorials/diagrams.html#030
I have used a board that I designed a few years ago. It is $50 robot compatible and Arduino compatible. If you need to see the schematic and PCB layout, look here: http://www.societyofrobots.com/robotforum/index.php?topic=2228.msg14977#msg14977 The board looks like this (click on the image to enlarge):
First of all, let me explain why the use of Parallax Ping))) ultrasonic sensor. I also have a Maxbotics EZ01 ultrasonic sensor and a Sharp GP2D12 infrared sensor. From all of them, the Ping))) sensor was the most accurate, with stable readings and short response time. I have tryed EZ1 sensor in analog mode and in PWM mode (similar with Ping) but didn't liked the results. The robot could not balance at all. The Sharp sensor was a little better, but it does not offer stable readings in a fixed position, it has spikes from time to time. Probably averaging 5 measurements would get better results.
All the tests were done in Basic (Bascom-AVR), since I don't have problems loading the code in the micro. I have also tested the Arduino code and I will port the code from Arduino to the $50 robot.
But let's take a look at the code:
There are 3 functions that are looped at about 20ms: Read_ping_sensor, PID, Drive_motors.
The Ping))) sensor has only one signal pin which acts as both trigger and echo. The microcontroller has to output a high signal for 5us (microseconds), then it switches to input mode, waits for the pin to go high, starts a counter and waits for the pin to go low again. At that moment the counter will return the time of sound flight from the sensor to the surface and back to the sensor. If distance is needed, this value has to be divided by 2 and multiplied by 29.034 to get the distance in centimeters or 11.3236 to get the distance in inches. For this application, the time of flight will be stored in the Ptime variable. Here is the code:
Config Ping_port = Output 'set pin as output for ping
Ping_port = 1 'set high
Timer1 = 0 'reset timer1
Loop Until Timer1 >= 5 '5us delay
Ping_port = 0 'set low
Config Ping_port = Input 'set pin as input for echo
Ping_port = 1 'turn on pull up
Bitwait Ping_pin , Set 'wait a high response from ping (hold off time)
Timer1 = 0 'reset timer1, ready to count time elapsed
Bitwait Ping_pin , Reset 'wait a low response from ping (distance time)
Ptime = Timer1 'copy timer1 value for distance measurement
'Print "Ping = " ; Ptime
A simple PID control was used to calculate the values for driving the servos. A Setpoint has to be determined for the perfect balance point by reading the sensor values through the serial port on the computer. To do that, unhook the servos from the board and hold by hand the robot in a vertical balance, read the value and then write it to the Setpoint constant.
For easier variable manipulation, a 5 elements array has been used to store the errors. There is Current, Sum, Last, SecondToLast and Delta, a friendlier name association for the elements of the array. To make the calculations easier, deltaT was considered 1. The result will be stored in the variable Drive. Here are the calculations:
Error(current) = Setpoint - Ptime
P = Error(current) * Kp
Error(sum) = Error(current) + Error(last) + Error(secondtolast)
I = Ki * Error(sum)
Error(delta) = Error(current) - Error(last)
D = Kd * Error(delta)
Drive = P + D + I
Error(secondtolast) = Error(last)
Error(last) = Error(current)
'Print "PID value = " ; Drive
The PID result stored in the variable Drive is added or substracted from the Middle point of the servo (the value that makes the servo to stay in place, stopped) and stored in the Lwheel and Rwheel variables . Then the servos are pulsed for an ON time equal with Lwheel or Rwheel variables. Here is the code:
Lwheel = Midl + Drive
Rwheel = Midr - Drive
Pulseout Servo , Sleft , Lwheel
Pulseout Servo , Sright , Rwheel
'Print "Left = " ; Lwheel
'Print "Right = " ; Rwheel
You can download the complete program in Bascom-AVR, and Arduino at the bottom of the tutorial. You have to change the txt extension to bas for the balance2ping file, pde for BalancingArduino file. I have added a potentiometer to adjust the SetPoint value for the perfect balancing point and updated the code for Arduino. I still need to adjust the values a little bit more in the Arduino code since the robot wobbles more than it does using the Bascom code.
The $50 robot users can use the Arduino code (generate the hex file in the Arduino environment and then upload it manually using PonyProg or AVRdude). Pomprocker has written a tutorial about using Ping with the $50 robot here, I'll try to get the code done as soon as I can.
And here it is, balancing on the table!
- This works as proof of concept (balancing), but it lacks fine control.
- The robot balance is heavily influenced by the surface incline level. If you adjust the setpoint for a certain inclination, it will not work properly for a different one.
- The servos do not have a fast enough response (RPM = 60). Use of DC motors with an RPM over 200 is needed.
- To make the robot drive or turn, an Offset variable has to be added or subtracted from the servo pulse variables, this proved to be difficult for this setup.
The next step will be controlling the PWM for DC motors, then replace the Ping))) sensor with an Accelerometer and a Gyro. Of course, some sort of sensor fusion algorithm will be needed, Kalman Filter or something similar.