I have had success with getting PiBBOT to balance.

When I saw my first two wheel balancing robot I was very fascinated.  And after receiving my Raspberry Pi, I decided to build one myself.
PiBBOT (Pi Balancing roBOT) is my first successful balancing robot. And it has room for improvement and extra functionality.

When building PiBBOT, I had a few roadblocks I needed to overcome;

1. I originally had the Anker as the power source for both Pi and the motors, however the amperage was too low.
2. Original H-Bridge not powerful enough for my motors
3. Gyro calculation off by 15 degrees

The TFT displays the angles from the accelerometer, gyro, complementary filter and power to the motors.
The buttons are to turn the motors on and off and to reset the gyro.

What next?

• Incorporate the wheel encoders for better balance
• Build a sturdier frame
• Implement direction control
• The able to self right itself after falling over
• Room mapping
• Retrieve a can of beer from the fridge

PiBBOT consists of these components;
Adafruit:
TFT; 2.2" 18-bit color TFT LCD display
RF Receiver :RF M4 Receiver - 315MHz

Pololu;
Motors;  9.7:1 Metal Gearmotor 25Dx48L mm with 48 CPR Encoder
Wheels; Pololu Wheel 90x10mm
IMU; MinIMU-9 v2 Gyro, Accelerometer, and Compass (L3GD20 and LSM303DLHC).

Anker;
Battery; Anker® Astro3 10000mAh  5V / 9V / 12V 2A Dual USB Output External Battery

Tenergy;
Battery; 7.2V Tenergy 3800mAh Flat NiMH High Power
[wp_ad_camp_3]

Theory

The basics behind a balancing robot is based on the Inverted Pendulum concept. The goal is to have a control algorithm called Proportional Integral Derivative (PID) to keep the robot balanced by trying to keep the wheels under the center of gravity. Eg. If the robot leans forwards, the wheels spin forward trying correct the lean.

One axis of an accelerometer and one axis from a gyroscope are used to measure the current angle and the rate of rotation. A well timed looped is needed to keep track of everything.
Calculations are then done to provide power via PWM to the motors and in the right direction to keep the robot upright.

Angle Measurement

To measure the angles, I use the MinIMU-9 v2 Gyro, Accelerometer, and Compass inertial measurement unit(IMU) from Pololu.

This is a digital IMU and can be read using I2C. It is also powered off the Raspberry Pi 3v pin.

Here you can see the 3 individual I2C addresses;

When using the IMU to calculate the angle, readings from both the gyro and accelerometer are needed, which are then combined. This is because using either on their own will result in inaccurate readings.

Here is why;
Gyro - A gyro measures the rate of rotation, which has to be tracked over time to calculate the current angle. This tracking <and noise> causes the gyro to drift.  However, gyros are good at measuring quick sharp movements.

Accelerometer - Accelerometers are used to sense both static (e.g. gravity) and dynamic (e.g. sudden starts/stops) acceleration. They don't need to be tracked like a gyro and can  measure the current angle at any given time. Accelerometers however are very noisy and are only useful for tracking angles over a long period of time.


The gyro and accelerometer have a number of sensitivity levels.  For the gyro I am using a measurement range of 250dps, which has a sensitivity of is 0.00875 dps/LSB. And the accelerometer I am using 8 g full scale, 4 mg/LSB and FS = 10.

Later I will discuss how to combine the angles from both sensors.

Converting the RAW gyro and accelerometer values to something useful.

The gyro I use is a L3GD20

For the gyro, we need to work out how fast it is rotating in degrees per second(dps). We then need to track the angle deviation from point zero. Point zero is when the PiBBOT is upright and balanced. We then need to track this over time.

Gyro Rotation Rate;

rate_gyr_x = (float) gyr_x_raw * G_GAIN

gyr_x_raw = raw data acquired from the gyro X axis.
G_GAIN  = 0.00875, which is based off the sensitivity level used by the gyro.
rate_gyr_x =  the rate of rotation per second.

Gyro Angle Tracking

gyroXangle+=rate_gyr_x * DT

DT = loop period.
gyroXangle = is the current X angle calculated from the gyro X data.

I have DT set top 0.02, which is 20ms. This is how long it takes to complete one cycle of the main loop. This loop period has to be constant and accurate, otherwise your gyro will drift.

Accelerometer Angle

The accelerometer I use is LSM303DLHC
The X angle can be calculated by using trigonometry and the raw values from the accelerometer Y and Z axis. This is done using the Atan2 function to return the principal value of the tangent of Y and Z, expressed in radians.

We add π to the radians so that we get a result between 0 and 2. We then convert the radians to degrees by multiplying the radians by 57.29578 (180/π).

AccXangle = (float) (atan2(acc_y_raw,acc_z_raw)+M_PI)*RAD_TO_DEG

M_PI = 3.14159265358979323846
RAD_TO_DEG = 57.29578 , 1 radian = 57.29578 degrees

Combining the angles

Once you have the both X angles, you will have to combined them to overcome the gyro drift and the accelerometer noise.
We can do this by using a filter, which will trust the gyro for short periods of time and the accelerometer for longer periods of time.

There are two filers you could use, the Kalman Filter or the Complementary Filter.   I used the Complementary filter as it is simple to understand and less CPU intensive.   The Kalman filter is way to far complex and would be very processor intensive.

Complementary filter;

Current angle = 98% x (current angle + gyro rotation rate) + (2% * Accelerometer angle)

or

CFangleX=AA*(CFangleX+rate_gyr_x*DT) +(1 - AA) * AccXangle;

AA = 0.98  Complementary filter constant

CFangleX is our final angle which will be used to balance PiBBOT.


In summary, our main piece of code for angles is;

rate_gyr_x = (float) gyr_x_raw * G_GAIN;
AccXangle = (float) (atan2(acc_y_raw,acc_z_raw)+M_PI)*RAD_TO_DEG;
CFangleX=AA*(CFangleX+rate_gyr_x*DT) +(1 - AA) * AccXangle;

Proportional Integral Derivative (PID)

A PID control algorithm is used to balance the robot by driving the motors based on the tilt.

The proportional(P) term of the PID is based on the current angle difference or error from Point Zero (where robot balances) multiplied by the P gain. The P gain is a number we need to tweak to get the right amount of proportional control.
The proportional control will try and correct the balance based on the current error from point zero.

The portion of code for the P term is;

Pterm = KP * CFangleX;

The integral (I) term of the PID is based on the current angle difference or error from Point Zero multiplied by the I gain, which is that accumulated over time. The integral control assist in balancing the robot if it is moving.

The portion of code for the I term is;

iTerm += KI * CFangleX;

The derivative (D) term of the PID is based on the current rate of rotation. It is used to dampen the response as the robot reaches Zero Point. Without the derivative control, the robot will overshoot and than start to oscillate.

The portion of code for the D term is;

dTerm = KD *  (CFangleX - lastAngle);
lastAngle = CFangleX;


The addition of all the PID values will be used to drive the motors in the right direction and with the right power;

output = Pterm + iTerm + dTerm;

Getting the PID values correct(KP,KI,KD) is the hardest part of building PiBBOT.

The PID values need to be manually tuned;

1. Set KI and KD to zero.
2. Set KP high enougth so that the motors drive the wheels under the robot in the direction it is falling. The robot should overshoot Point Zero a little bit and then start to oscillate.
3. Reduce KP by about 10% to so you get just below the oscillation.
4. Increase KI. This will help the robot reach Point Zero faster and will also start to oscillate. Try and get a value so that the robot just oscillates
5. Increase KD to dampen the oscillation and until the robot balances.

It is best to change these values while trying to balance to see the results in real time, a potentiometer could be used for this. I have used the RF module to increase or decrease the values.

[wp_ad_camp_1]