by Jose Cruz
Learn how to build a two-wheel self-balancing Java robot using Raspberry Pi.
In my last four-part series of articles, I explained how to connect electronic sensors or devices to the Raspberry Pi 2 Model B using various interfaces: Part 1 showed how to use general-purpose input/output (GPIO) interfaces, Part 2 used inter-integrated circuit bus (I2C) interfaces, Part 3 used universal asynchronous receiver transmitter circuit (UART) interfaces, and Part 4 used serial peripheral interface (SPI) interfaces.
This article, which is the first in a new series, focuses on using two types of interfaces, GPIO and I2C, to create a prototype two-wheel self-balancing robot using the following modules:
- MPU-6050 sensor, which contains a three-axis gyroscope and an accelerometer. The accelerometer measures linear acceleration and earth gravity vectors and the gyroscope measures angular velocity. The sensor uses an I2C interface at address 0x68h to read a value raw that contains the accelerometer and gyroscope data.
- Adafruit PCA9685, which is a 16-channel, 12-bit pulse width modulation (PWM) servo driver that produces pulses at a desirable frequency to control motor speed. It uses an I2C interface at address 0x41h to prevent the Raspberry Pi from producing a software pulse that forces a large delay in CPU response.
- L298N dual H-bridge DC motor driver, which controls the speed and direction of the two robot motors. It uses a GPIO interface to produce the direction signal and connects to the PWM module to activate and control the speed of each motor.
Note: The complete example code for this NetBeans IDE 8.0.2 project can be downloaded here.
The MPU-6050, the PCA9685, and the L298N modules create a feedback control loop, as shown in Figure 1, which balances the robot and automatically corrects its position upon disturbance.
The robot is prevented from falling by giving acceleration to the wheels according to the robot's inclination from the vertical axis. If the robot tilts at an angle, then in the frame of the wheels, the center of mass of the robot will experience a pseudo force that will apply a torque opposite to the direction of tilt.
Figure 1. Steps involved in feedback control loop for the balancing robot
Circuits We Will Create
Based on the block diagram shown in Figure 2, the electronic components shown Figure 3, and the mechanical parts shown in Figure 4, we will create the circuits shown in Figure 5 and the two-wheeled balanced robot prototype shown in Figure 6.
Figure 2. Block diagram of the circuits we will create
| 
| 
| 
|
| 
| 
| 
|
| 
| 
| 
|
Figure 3. Electronic components we will use
List of electronic components:
- 1 x Raspberry Pi 2 Model B
- 1 x MPU-6050 three-axis gyroscope with accelerometer sensor module
- 1 x L298N dual H-bridge DC motor driver
- 1 x Adafruit PCA9685 16-channel 12-bit PWM servo driver
- 1 x DC/DC power converter LM2596
- 1 x Adafruit Perma-Proto Pi HAT
- 1 x 7.4V 6A LiPo rechargeable battery (Note: For good results, it is suggested that the power supply be a 7.4v LiPo battery with a minimum of 2.200 mAh connected to a DC-DC power converter generating 5v final voltage.)
- 2 x 0.1 uF capacitor
- 1 x ThePiHut Wi-Fi dongle 802.11N
| 
| 
| 
| 
|
| 
|
Figure 4. Mechanical parts we will use
List of mechanical parts:
- 2 x wheel
- 2 x shaft coupling
- 2 x motor
- 4 x brass stud M4x40
- 4 x brass stud M4x30
- 8 x brass stud M4x10
- 2 x motor brackets
- 3 x acrylic board
- 20 x screw M4
- 14 x nut M4
Figure 5. Schematic of the circuits we will create
Figure 6. Two-wheeled prototype of balanced robot we will create
A Little Mathematics
The Inverted Pendulum Problem
Balancing robots represents the classic inverted pendulum problem, in which a large mass is placed at the end of a pole. The pole is free to rotate around the base, and the base is free to move in the plane perpendicular to the vertical. The goal is to keep the pole vertical by moving the base in response to changes in the angle.
Complementary Filter
Figure 7 shows a two-wheeled robot with a three-axis accelerometer to measure tilt angle θa and a single-axis gyroscope to measure dynamic tilt angle θg. Both angles must be fused by using a filter to obtain a final tilt angle θ. In our case, we will use a complementary filter, as shown in Figure 8, which controls the speed of the motors based on the angular velocity information.
Figure 7. Robot with three-axis accelerometer and a single-axis gyroscope
Figure 8. Complementary filter block diagram (source: Mouser)
We obtain the θ angle reading from the accelerometer in a continuous loop by using a low-pass filter, and we sum the results with the gyroscope values by applying a high-pass filter.
We build this filter inside the method Filter in the JBalancePI class, as shown in Listing 1. The variable angle_filtered is the θ angle:
/\*\*
\* Read data from accelerometer and gyroscope and apply a complementary
\* filter
\*/
private void Filter() {
//Data from MPU-6050 Accelerometer and Gyroscope
data = accel.getMotion6();
//Calculate angle and convert from radians to degrees
float angle\_raw = (float) (Math.atan2(data.AccelY, data.AccelZ) \* 180.00 / pi + accel\_offset);
float omega = (float) (data.GyroX / gyro\_gain + gyro\_offset);
// Filter data to get the real value
long now = System.currentTimeMillis();
float dt = (float) ((now - preTime) / 1000.00);
preTime = now;
//Calculate error using complementary filter
float K = 0.8F;
float A = K / (K + dt);
angle\_filtered = A \* (angle\_filtered + omega \* dt) + (1 - A) \* angle\_raw;
Listing 1. Filter method that implements a complementary filter
For details about the mathematical formulas, please see this information.
It is very important that the MPU-6050 sensor's Y orientation is mounted parallel to the robot wheels, as shown in Figure 9.
Figure 9. MPU-6050 sensor's Y orientation (in the red circle), mounted parallel to the wheels
Proportional, Integral, and Derivative (PID) Control System
The control algorithm that we will use to maintain the balance off the autonomous self-balancing two-wheeled robot is the PID controller. The PID controller is well known as a three-term controller; it is an incredibly powerful and ubiquitous control algorithm. An output signal u can be generated by summing the three components, as shown in Figure 10:
Figure 10. u(t) output control signal
The input to the controller is the error from the system. The Kp, Ki, and Kd terms are referred to as the proportional, integral, and derivative constants (the three terms get multiplied by these constants respectively), and e(t) is the error from the desired output at time t.
The closed-loop control system is also referred to as a negative feedback system. The basic idea of a negative feedback system is that it measures the process output from a sensor. The measured process output gets subtracted from the reference set-point value to produce an error. The error is then fed into the PID controller, where the error gets managed in three ways.
The error will be used on the PID controller to execute the proportional term, the integral term for the reduction of steady-state errors, and the derivative term to handle overshoots. After the PID algorithm processes the error, the controller produces a control signal u. The PID control signal then gets fed into the process under control.
The process under PID control is the two-wheeled robot, as shown in Figure 11. The PID control signal will try to drive the process to the desired reference set-point value. In the case of the two-wheeled robot, the desired set-point value is the zero degree vertical position.
Figure 11. PID controller and complementary filter working together
PID Tuning
Here are some steps that will help us to get Kp, Ki, and Kd faster:
- Set the Ki and Kd terms to 0, and adjust Kp so that the robot starts to oscillate (move back and forth) about the balance position. Kp should be large enough for the robot to move but not too large; otherwise, the movement will not be smooth.
- With Kp set, increase Ki so that the robot accelerates faster when it is off balance. With Kp and Ki properly tuned, the robot should be able to self-balance for at least a few seconds.
- Finally, increase Kd so that the robot will move about its balanced position more gently. There shouldn't be any significant overshoots.
We build this PID controller inside method PID in the JBalancePI class, as shown in Listing 2.
//Set- point values used in PID controller
private final float Kp = 17F;
private final float Kd = 840F;
private final float Ki = 0.1F;
/*
* Proportional, Integral, Derivative control.
*/
private void PID() {
long now = System.currentTimeMillis();
int timeChange = (int) (now - lastTime);
lastTime = now;
float error = angle_filtered; // Proportion
errSum += error * timeChange; // Integration
float dErr = (error - lastErr) / timeChange; // Differentiation
float output = Kp * error + Ki * errSum + Kd * dErr;
lastErr = error;
LOutput = output - Turn_Speed + Run_Speed;
ROutput = output + Turn_Speed + Run_Speed;
}
Listing 2. PID Controller method
Summary of the Code We Will Write
Motor Driver Controller with PWM Pulse
The PID controller produces the necessary data to create a PWM pulse to move the motors forward, move them back, or stop them. To do that, we create a class called L298Device that uses class GPIOPin and the class PCA9685Device from my previous article. See Listing 3.
/**
* Interface to L298N Mmotor Ddriver using GPIO bus
*
* @author Jose Cruz
*/
public class L298Device {
//Enable motor left
private GPIOPin IN1 = null;
private GPIOPin IN2 = null;
//Enable motor right
private GPIOPin IN3 = null;
private GPIOPin IN4 = null;
//PWM port 0 for motor left
private byte ENA = 0;
//PWM port 1 for motor right
private byte ENB = 1;
//Define PWM motor object
PCA9685Device pwm;
Listing 3. Creating the L298Device class
Next, we define a constructor L298Device that creates a device at the defined GPIO pins and a PWM object, as shown in Listing 4.
/**
* Initialize GPIO IN1 to IN4 motor input terminal
*
* @param in1
* @param in2
* @param in3
* @param in4
*/
public L298Device(int in1, int in2, int in3, int in4) {
try {
// define device for IN1 pin
IN1 = (GPIOPin) DeviceManager.open(
new GPIOPinConfig.Builder()
.setControllerNumber(0)
.setPinNumber(in1)
.setDirection(GPIOPinConfig.DIR\_OUTPUT\_ONLY)
.setDriveMode(GPIOPinConfig.MODE\_OUTPUT\_OPEN\_DRAIN)
.setInitValue(false)
.build());
// define device for IN2 pin
IN2 = (GPIOPin) DeviceManager.open(new GPIOPinConfig.Builder()
.setControllerNumber(0)
.setPinNumber(in2)
.setDirection(GPIOPinConfig.DIR\_OUTPUT\_ONLY)
.setDriveMode(GPIOPinConfig.MODE\_OUTPUT\_OPEN\_DRAIN
.setInitValue(false)
.build());
// define device for IN3 pin
IN3 = (GPIOPin) DeviceManager.open(new GPIOPinConfig.Builder()
.setControllerNumber(0)
.setPinNumber(in3)
.setDirection(GPIOPinConfig.DIR\_OUTPUT\_ONLY)
.setDriveMode(GPIOPinConfig.MODE\_OUTPUT\_OPEN\_DRAIN)
.setInitValue(false)
.build());
// define device for IN4 pin
IN4 = (GPIOPin) DeviceManager.open(new GPIOPinConfig.Builder()
.setControllerNumber(0)
.setPinNumber(in4)
.setDirection(GPIOPinConfig.DIR\_OUTPUT\_ONLY)
.setDriveMode(GPIOPinConfig.MODE\_OUTPUT\_OPEN\_DRAIN)
.setInitValue(false)
.build());
pwm = new PCA9685Device();
pwm.setPWMFreq(1000);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}
Listing 4. Initializing GPIO pins and creating pwm object
Then create methods to move forward (moveL_Forward, moveR_Forward), move back (moveL_Back, moveR_Back), and stop both robot motors (stopL, stopR), as shown in Listing 5.
/**
\* Motor left move forward
\*/
public void moveL\_Forward() {
try {
IN1.setValue(false);
IN2.setValue(true);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}
/**
* Motor left move back
*/
public void moveL_Back() {
try {
IN1.setValue(true);
IN2.setValue(false);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}
/**
* Motor right move forward
*/
public void moveR_Forward() {
try {
IN3.setValue(false);
IN4.setValue(true);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}
/**
* Motor right move back
*/
public void moveR_Back() {
try {
IN3.setValue(true);
IN4.setValue(false);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}
/**
* Stop motor left
*/
public void stopL() {
try {
IN1.setValue(true);
IN2.setValue(true);
motorL\_PWM((short) 0);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}
/**
* Stop motor right
*/
public void stopR() {
try {
IN3.setValue(true);
IN4.setValue(true);
motorR\_PWM((short) 0);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}
Listing 5. Methods to move and stop both motors
To start moving the motors, we need set the PWM pulse to the desired frequency by using the pwm object, as shown in Listing 6.
/\*\*
\* Set PWM pulse for motor left speed
\* [@param](https://forums.oracle.com/ords/apexds/user/param) \_pwm
\*/
public void motorL\_PWM(short \_pwm) {
pwm.setPWM(ENA, (short) 0, \_pwm); }
/\*\*
\* Set PWM pulse for motor right speed
\* [@param](https://forums.oracle.com/ords/apexds/user/param) \_pwm
\*/
public void motorR\_PWM(short \_pwm) {
pwm.setPWM(ENB, (short) 0, \_pwm);
Listing 6. Create PWM pulses at desired frequency for both motors
Complete JBalancePI Class
Using the L298Device class, we create a motor control method PWMControl that uses values calculated inside PID controller methods PID, LOutput, and ROutput to control the movement, direction, and velocity, as shown in Listing 7.
/\*
\* PWM motor control
\*/
private void PWMControl() {
if (LOutput > 0) {
motor.moveL\_Forward();
} else if (LOutput \< 0) {
motor.moveL\_Back();
} else {
motor.stopL();
}
if (ROutput > 0) {
motor.moveR\_Forward();
} else if (ROutput \< 0) {
motor.moveR\_Back();
} else {
motor.stopR();
}
motor.motorL\_PWM((short) (Math.min(4095, Math.abs(LOutput) \* 4095 / 256)));
motor.motorR\_PWM((short) (Math.min(4095, Math.abs(ROutput) \* 4095 / 256)));
Listing 7. Defining the type of movement and creating a PWM pulse for velocity control
We create a feedback control loop, as shown in Figure 1, with the thread class ControlLoop, as shown in Listing 8.
/*
\* Thread for moving and balancing robot
\*/
class ControlLoop extends Thread {
@Override
public void run() {
while (shouldRun) {
Filter();
Logger.getGlobal().log(Level.INFO, "Angle = " + angle\_filtered);
// If angle > 45 or \< -45 then stop the robot
if (Math.abs(angle\_filtered) \< 45) {
PID();
PWMControl();
} else {
motor.stopL();
motor.stopR();
// Keep reading accelerometer and gyroscope values after falling down
for (int i = 0; i \< 100; i++)
Filter();
// Empty data and restart the robot automatically
if (Math.abs(angle\_filtered) \< 45)
{
for (int i = 0; i \<= 500; i++) // Reset the robot and delay 2 seconds
{
angle\_filtered = 0;
Filter();
errSum = Run\_Speed = Turn\_Speed = 0;
PID();
}
}
}
}
accel.close();
motor.close();
}
}
Listing 8. Feedback control loop thread class
Let's now complete the MIDlet class with startApp and destroyApp methods to initialize the complementary filter, the PID controller, the motor controller, the accelerometer, and the gyroscope, and create a task to balance the robot with the feedback control loop., as shown in Listing 9.
/**
\*
\*/
public void startApp() {
loggerHandler.start();
Logger.getGlobal().setLevel(Level.INFO);
Logger.getGlobal().log(Level.INFO, "\*\*\*\*\* JBalancePi v1.3 Started \*\*\*\*\*");
try {
//Activate MPU-6050 with I2C bus
accel = new MPU6050Device();
//Activate motor control with GPIO bus: In1=27, In2=22, In3=24, In4=25
motor = new L298Device(27, 22, 24, 25);
// Loop 200 times to get the real values when starting
for (int i = 0; i \< 200; i++) {
Filter();
}
if (Math.abs(angle\_filtered) \< 45) // Start the robot after cleaning data
{
angle\_filtered = 0;
Filter();
errSum = Run\_Speed = Turn\_Speed = 0;
PID();
}
//Start move and balance thread
controlLoopTask = new ControlLoop();
controlLoopTask.start();
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}
/**
\*
\* [@param](https://forums.oracle.com/ords/apexds/user/param) unconditional
\*/
@Override
public void destroyApp(boolean unconditional) {
//Stop thread
shouldRun = false;
Logger.getGlobal().log(Level.INFO, "\*\*\*\*\* JBalancePi v1.0 Stopped \*\*\*\*\*");
}
}
Listing 9. MIDlet class methods startApp and destroyApp
Performing Some Additional Configuration
Before running our test MIDlets using NetBeans IDE 8.0.2, it is important to establish the appropriate API permissions. To do this, in the IDE, select project JavaMEDemos, and then right-click and select Properties to show the Project Properties window. Select Application Descriptor, and then select the API Permissions tab. Include all the permissions shown in Figure 12.
Figure 12. Establishing API permissions
Conclusion
In this article, we saw how to construct a simple two-wheeled balancing robot using some modules such as the MPU-6050 accelerometer and gyroscope, the PCA9685 PWM servo driver, and the L298Nmotor driver.
It is important to establish some initial values for PID constants Kp, Ki, and Kd and adjust them until the robot is balanced. In the next article in this series, we will connect an OLED display and some buttons to modify the values dynamically.
We are opening a lot of possibilities for remote control of this robot, such as Bluetooth, Wi-Fi, REST, Twitter, web interfaces, and gestures with Kinect. In subsequent articles, we will see how we can implement remote control.
About the Author
Jose Cruzis a software engineer who has been working with Java since 1998. He is a lead developer of Java, Java ME, and Java EE at Ferreteria EPA C.A. in Venezuela. From an early age, his hobby has been electronics. This has led him to combine computing with electronics and develop projects where Java and embedded devices such as Arduino and Raspberry Pi are used.
Join the Conversation
Join the Java community conversation on Facebook, Twitter, and the Oracle Java Blog!