Self Balancing Robot - PID Control Algorithm

This project was conceived because I was interested in learning more about Control Algorithms and how to effectively implement functional PID loops. The project is still in the development phase as a Bluetooth module is yet to be added which will allow for control over the robot from a Bluetooth enabled smartphone.

The N20 DC motors used were relatively cheap, and hence have a considerable amount of play in them. This leads to a small amount of jerk as the motors overcome the 'slack' as it applies torque to the wheels. Hence, it is next to impossible to achieve perfectly smooth motion. The code I have written is reasonably simple but effectively demonstrates the capabilities of the PID algorithm.

Project Summary:

The chassis of the robot is 3D printed using an Ender 3 printer and is designed to press-fit together.

The robot is controlled by an Arduino Uno which takes sensor data from the MPU6050 and controls the DC motors through an external motor driver. It runs off a 7.4V, 1500mAh battery. The motor driver regulates this to 5V to power the Arduino and supplies 7.4V to the motors.

The software was written from scratch with the aid of the 'Arduino-KalmanFilter-master' and 'Arduino-MPU6050-master' libraries from gitHub.

• 3D Printed Parts
• Arduino UNO
• MPU6050 6-Axis Sensor
• D.C Motor Driver
• N20 D.C Motors (x2)
• 9V Battery

Print and Assembly

The entire build should be press-fit but I have used superglue to secure the components to ensure the robot is entirely rigid when balancing.

I have designed the parts in Fusion 360 and have optimised each part to print without supports to allow for tighter tolerances and a cleaner surface finish.

Settings used on Ender 3 Printer were: 0.16mm Layer Heights @ 40% infill for all parts.

Chassis (x1)

Left Wheel (x2)

Left Motor Housing (x2)

Arduino Case (x1)

I have written a PID Control Algorithm from scratch using the 'Arduino-KalmanFilter-master' and 'Arduino-MPU6050-master' libraries from gitHub.

The premise of the Algorithm is as follows:

• Read raw data from MPU6050
• Use Kalman Filter to analyse data from both Gyroscope and Accelerometer to cancel out inaccuracies in gyroscope readings due to acceleration of the sensor. This returns a relatively smoothed value for the pitch of the sensor in degrees to two decimal places.
• Calculate the Error in the angle, i.e: The angle between the sensor and the setpoint.
• Calculate Proportional error as (Constant of Proportionality x error).
• Calculate Integral Error as the running sum of (Constant of Integration x error).
• Calculate Derivative Error as Constant as [(Differentiation Constant) x (Change in error / Change in Time)]
• Sum all errors to give the the speed output to be sent to motors.
• Calculate which direction to turn motors based on the sign of the error angle.
• The loop will run indefinitely and build upon the output as the input varies. It is a feedback loop, using the output values as the new input values for the next iteration.

The final Step is to Tune the PID loop Kp, Ki & Kd parameters.

1. A good starting point is to slowly increase Kp until the robot oscillates around the balance point and can catch a fall.
2. Next, start Kd at around 1% the value of Kp and increase slowly until the oscillations disappear and the robot glides smoothly when pushed.
3. Finally, start with Ki around 20% of Kp and vary until the robot "overshoots" the setpoint to actively catch a fall and return to vertical.