Ministry of Education and Training

Ho Chi Minh City University of Technology and Education

Faculty of Electrical and Electronics Engineering
Department of Automatic Control
-----------------⸙∆⸙-----------------

Project on Automatic Control

Two-Wheeled Self-Balancing Robot

LECTURER : VU VAN PHONG

STUDENT’S NAME : TRAN TUAN HIEU – 20151275
NGUYEN DINH HOAN - 20151277

Ho Chi Minh City, JANUARY 4th 2022

Contents
CHAPTER 1: OVERVIEW OF SELF-BALACED VEHICLES......................... 1
1.1 what is two wheels self-balancing? ............................................................ 1
1.2 Models of self-balancing 2 wheels in labrotary .......................................... 3
1.2.1 nBot ...................................................................................................... 3
1.2.2 JOE ....................................................................................................... 4
1.2.3 NXTway-GS ........................................................................................ 5
1.3 Some actual products are based on self-balancing 2 wheels robot model . 5
1.3.1 Segway Personal Transporter .............................................................. 5
1.3.2 Winglet Toyota .................................................................................... 6
1.3.3 Iswing Toyota ...................................................................................... 7
CHAPTER 2: THEORETICAL BASIC ............................................................... 9
2.1 PID controller.............................................................................................. 9
2.2 Modeling and symbols ................................................................................ 9
2.3 Kinetic model of DC motor ...................................................................... 10
2.4 Two wheels self-balancing robot .............................................................. 11
2.5 Simulation ................................................................................................. 15
2.5.1 System parameter declaration ............................................................ 15
2.5.2 System stability investigation ............................................................ 15
2.5.3 System controlability investigation ................................................... 17
2.5.4 System observability investigation .................................................... 18
2.5.5 controller construction ....................................................................... 19
CHAPTER 3: SYSTEM DESIGN ...................................................................... 22
3.1 Hardware ................................................................................................... 22
3.1.1 Arduino nano...................................................................................... 22
3.1.2 CNC shield V4 ................................................................................... 24
3.1.3 Driver A4988 ..................................................................................... 25
3.1.4 Stepper motor size 42 (1.8º)............................................................... 25
3.1.5 GY-521 MPU 6050 angel sensor ....................................................... 26
3.1.6 65mm wheels ..................................................................................... 28
3.1.7 Battery ................................................................................................ 28
 3.2 Control alogrithm flowchart ..................................................................... 29
3.3 Software design ......................................................................................... 29
3.3.1 Main program..................................................................................... 29
3.3.2 sensor library program ....................................................................... 33
3.4 Realistic model.......................................................................................... 37
3.5 Realistic testing ......................................................................................... 38
3.5.1 Standard PID ...................................................................................... 38
3.5.2 Kp increased/decreased ...................................................................... 39
3.5.3 Ki increased/decreased....................................................................... 41
3.5.4 Kd increased/decreased ...................................................................... 42
CHAPTER 4: CONCLUSION AND DEVELOPMENT ................................... 44
4.1 Results ....................................................................................................... 44
4.2 Limtitations and development directions .................................................. 44
4.2.1 Limitations ......................................................................................... 44
4.2.2 Development directions ..................................................................... 44
REFERENCES.................................................................................................... 45
CHAPTER 1: OVERVIEW OF SELF-BALACED VEHICLES
1.1 what is two wheels self-balancing?

In the field of automation - automatic control in general and cybernetics in

particular, the inverted pendulum model is one of the typical research objects and
is characterized by the unstable dynamic characteristics of the model, so the
control controlling this object in fact poses as a challenge.

The research results of the basic inverted pendulum model, such as the car-
pendulum model, the rotating inverted pendulum, etc. can be applied and
inherited to other similar models but have more practical application. such as
rocket model, self-balancing two-wheeled vehicle model, etc., thereby
overcoming the inherent disadvantages of classic two- or three-wheeled robots.
Classic two- or three-wheeled robots, which consist of a drive wheel and a free
wheel (or whatever else) to support the robot's weight. If a lot of weight is placed
on the rudder, the robot will be unstable and prone to falling, and if put on many
tail wheels, the two main wheels will lose their ability to grip. Many robot designs
can move well on flat terrain but cannot move up and down on convex or inclined
terrain. When moving up a hill, the robot's weight is placed on the rear of the
vehicle, causing it to lose its ability to grip and slip.

3-wheel robot moves on flat terrain, the weight is divided equally between
the rudder and the small guide wheel

Figure 1) 3-wheeled robot moves on flat terrain

3-wheel robot when going downhill, gravity on the rear wheel makes the car
can capsized.
1
 Figure 2) Robot with 3 wheels when going downhill

3-wheel robot when going uphill, the weight is put on the front wheel, making
the friction force to help the car stick on the road surface is not guaranteed

Figure 3) 3-wheel robot when going uphill

In contrast, coaxial two-wheeled robots balance very flexibly when moving

maneuvering over complex terrain, even though the robot itself is an unstable
system determined. When the robot moves on steep terrain, it automatically tilts
forward and holds for the weight to be transferred to the two main wheels.
Similarly, when moving downhill, it lean back and keep the center of gravity on
the main wheel. So there is never present The vehicle's center of gravity falls
outside the wheel support area, which can cause a rollover.

2
 Figure 4) 2-wheel robot moves on different terrains in a direction that
preserves balance

1.2 Models of self-balancing 2 wheels in labrotary

1.2.1 nBot

Robot nBot built by David P.Anderson. The principle of controlling nBot

is as follows: the wheels will run in the direction where the upper part of the robot
is about to fall, if the wheels can be steered in a way that keeps the robot's center
of gravity, the robot will be balanced.

The control process uses signals from two sensors: the sensor for the tilt
angle of the robot body relative to the direction of gravity and the encoder
mounted on the wheel to measure the robot's position. This signal forms four
variables: robot body tilt angle, tilt angular velocity, robot position and robot
speed; These 4 variables are calculated into the motor control voltage for the
robot.

3
 Figure 5) nBot

1.2.2 JOE

JOE was created in 2002 by the industrial electronics laboratory of the

Federal Institute of Technology Lausanne, Switzerland. Its shape consists of two
coaxial wheels, each wheel is attached to a DC motor, this robot can move U-
shaped rotation

The control system consists of two separate "state space" controllers,

controlling the motor to keep the system balance. State information is provided
by two optical encoders and two sensors: angular accelerometer and gyro.

The JOE is controlled by an RC remote controller. The central controller

and signal processing unit is a digital signal processing board (DSP) developed
by the group itself and by the Federal Institute, in conjunction with XILINC's
FPGA.

4
 Figure 6) JOE

1.2.3 NXTway-GS

Figure 7) NXTway-GS by LEGO MINDSTORMS

1.3 Some actual products are based on self-balancing 2 wheels robot

model
1.3.1 Segway Personal Transporter

Segway PT (short for Segway Personal Transporter), commonly known as

Segway for short, is a two-wheeled personal transport vehicle, operating on a self-
balancing mechanism invented by Dean Kamen. This vehicle is manufactured by
Segway Inc. in the state of New Hampshire, USA. The word "Segway" is

5
pronounced akin to "segue" (an original Italian word meaning "to move gently").
The outstanding feature of the Segway is the self-balancing mechanism thanks to
the computer system, engine and gyroscope located inside the vehicle, which
helps the vehicle, even with only one axis of movement with two wheels, but
always in a state of balance. Equally, the user only needs to lean forward or
backward to control the car to go forward or backward

Figure 8) Segway Personal Transporter

1.3.2 Winglet Toyota

The Winglet, developed by Toyota and introduced in 2008, has a base the
size of an A3 sheet of paper. The vehicle is based on a self-balancing 2-wheel
model and has sensors to recognize the movement of the driver, thereby giving
commands to the vehicle to operate according to the wishes of the driver.

6
 Figure 9) Winglet Toyota

1.3.3 Iswing Toyota

Figure 10) Iswing Toyota

Iswing is known as one of the boldest ideas of the auto industry in recent
times. Making its debut at the 2005 Tokyo Motor Show, the Iswing is a symbol
of the future of personal vehicles because it is far different from conventional
cars. The movement, control, communication between people and vehicles are all
human. For example, when placing the seat in the "minimum body opening"
mode, the driver's eyes will be level with the person standing, so it is easy to talk

7
and create a friendly feeling. The i-swing is controlled via handles and buttons,
similar to how people use electronic devices every day

8
CHAPTER 2: THEORETICAL BASIC
2.1 PID controller

A well-functioning system is one that operates optimally according to a set

standard of power (reaching extremes). Whether the optimal state is achieved and
maintained depends on the quality requirements set, the effects on the object and
the working conditions of the object. One of the controllers that helps the object
to operate optimally according to the user's wishes is the PID controller.

Figure 11) PID controller

To design a PID controller, we must set up a negative feedback controller

to get the error value between the output and the set value and then put it into the
PID controller. From the PID controller, it is easy to supply a control voltage so
that the system reaches the equilibrium position

2.2 Modeling and symbols

We have a two-wheeled balanced robot model as follows:

Figure 12) Self-balancing 2-wheeled robot model on a flat surface

9
 Sign Unit Meaning
Mp Kg Mass of wheel
Mw Kg Mass of the robot
r m Wheel radius
𝜃P rad Tilt angle of robot body
𝜃W rad Wheel rotation angle
W rad Angular speed of the wheel
𝑅 Ohm DC motor resistance
L H Motor's inductor
kf Nms/rad Coefficient of Friction
Km 𝑘𝑔. 𝑚2 Moment coefficient
Ke Vs/rad DC motor resistance
Kb Vsec/rad EMF factor of DC motor
a Rad/s2 Engine acceleration
Va V Motor supply voltage
Ve V Armature voltage
i A Armature current
m Nm Engine torque
Iw Moment of inertia of the wheel
Ip Moment of inertia of the vehicle body
Distance from the center of the wheel to the center of
l
gravity of the vehicle body
CL,CR Moment of engine acting on the wheel
HL HR PL PR Impact force between wheel and robot body
HfR , HfL Friction force milk wheel and plane
 Correction factor

2.3 Kinetic model of DC motor

0 1 0 0
𝜃 ̇
[ ] = [0
𝜃
−𝑘𝑚 𝑘𝑒 ] [ ] + [ 𝑘𝑚 −1] [ 𝑉𝑎 ] (1)
𝜔̇ 𝜔 𝜏𝑎
𝐼𝑅 𝑅 𝐼𝑅 𝑅 𝐼𝑅

Where Va is power supplied to motor, τa is moment of motor.

10
 2.4 Two wheels self-balancing robot

Figure 13) Analysis of forces acting on 2 wheels

Apply Newton's law to calculate the total force acting on the wheel in the
horizontal direction:
∑ 𝐹 = 𝑀𝑎 (2)
𝑥

𝑀𝑤 𝑥̈ = 𝐻𝑓𝑅 − 𝐻𝑅 (3)
Total force acting on the wheel axle:
∑ 𝑀𝑜 = 𝐼𝛼 (4)

𝐼𝑤 𝜃̈𝑤 = 𝐶𝑅 − 𝐻𝑓𝑅 𝑟 (5)

From the kinematics of the DC motor, we have the motor torque
𝑑𝜔 (6)
𝜏𝑚 = 𝐼𝑅 + 𝜏𝑎
𝑑𝑡
From the toxicology equation and the differential equation of the DC
motor, we have the output torque of the motor:
𝑑𝜔 −𝑘𝑚 𝑘𝑒 𝑘𝑚 (7)
𝐶 = 𝐼𝑅 = 𝜃̇𝑤 + 𝑉𝑎
𝑑𝑡 𝑅 𝑅
Substituting equation (7) into equation (5) we get:
−𝑘𝑚 𝑘𝑒 𝑘𝑚 (8)
𝐼𝑤 𝜃̈𝑤 = 𝜃̇𝑤 + 𝑉𝑎 − 𝐻𝑓𝑅 𝑟
𝑅 𝑅
We have:
−𝑘𝑚 𝑘𝑒 𝑘𝑚 𝐼𝑤 (9)
𝐻𝑓𝑅 = 𝜃̇𝑤 + 𝑉𝑎 − 𝜃̈𝑤
𝑅𝑟 𝑅𝑟 𝑟
Left wheel equation:

11
 −𝑘𝑚 𝑘𝑒 𝑘𝑚 𝐼𝑤 (10)
𝑀𝑤 𝑥̈ = 𝜃̇𝑤 + 𝑉𝑎 − 𝜃̈𝑤 − 𝐻𝐿
𝑅𝑟 𝑅𝑟 𝑟
Right wheel equation:
−𝑘𝑚 𝑘𝑒 𝑘𝑚 𝐼𝑤 (11)
𝑀𝑤 𝑥̈ = 𝜃̇𝑤 + 𝑉𝑎 − 𝜃̈𝑤 − 𝐻𝑅
𝑅𝑟 𝑅𝑟 𝑟
Since linear motion acts on the motor shaft, the angular velocity can be
converts to angular velocity according to the following equation:
𝑥̈
𝜃̈𝑤 𝑟 = 𝑥̈ ⟹ 𝜃̈𝑤 =
𝑟
𝑥̇
𝜃̇𝑤 𝑟 = 𝑥̇ ⟹ 𝜃̇𝑤 =
𝑟
From the above two equations, we can have the equations of the left and
right wheels as follows:
−𝑘𝑚 𝑘𝑒 𝑥̇ 𝑘𝑚 𝐼𝑤 𝑥̈
𝑀𝑤 𝑥̈ = + 𝑉𝑎 − − 𝐻𝐿
𝑅𝑟 𝑟 𝑅𝑟 𝑟 𝑟
−𝑘𝑚 𝑘𝑒 𝑥̇ 𝑘𝑚 𝐼𝑤 𝑥̈
𝑀𝑤 𝑥̈ = + 𝑉𝑎 − − 𝐻𝑅
𝑅𝑟 𝑟 𝑅𝑟 𝑟 𝑟
Sum of 2 equations of 2 wheels:
𝐼𝑤 −2𝑘𝑚 𝑘𝑒 2𝑘𝑚 (12)
2(Mw + 2 )𝑥̈ = 2
𝑥̇ + 2 𝑉𝑎 − (𝐻𝐿 + 𝐻𝑅 )
𝑟 𝑅𝑟 𝑅𝑟

Figure 14) Analysis of the force acting on the body of the robot

12
 Apply Newton's law to calculate the total force acting on the wheel in the
horizontal direction:
∑ 𝐹𝑥 = 𝑀𝑝 𝑥̈
(𝐻𝐿 + 𝐻𝑅 ) − 𝑀𝑝 𝑙𝜃̈𝑝 𝑐𝑜𝑠𝜃𝑝 + 𝑀𝑝 𝑙𝜃̇ 2 𝑝 𝑠𝑖𝑛𝜃𝑝 = 𝑀𝑝 𝑥̈
We have:
(𝐻𝐿 + 𝐻𝑅 ) = 𝑀𝑝 𝑥̈ + 𝑀𝑝 𝑙𝜃̈𝑝 𝑐𝑜𝑠𝜃𝑝 + 𝑀𝑝 𝑙𝜃̇ 2 𝑝 𝑠𝑖𝑛𝜃𝑝 (13)
Perpendicular force acting on the wheel:
∑ 𝐹𝑥𝑝 = 𝑀𝑝 𝑥̈ 𝑐𝑜𝑠𝜃𝑝

(𝐻𝐿 + 𝐻𝑅 )𝑐𝑜𝑠𝜃𝑝 + (𝑃𝐿 + 𝑃𝑅 )𝑠𝑖𝑛𝜃𝑝 − 𝑀𝑝 𝑔𝑠𝑖𝑛𝜃𝑝 − 𝑀𝑝 𝑙𝜃̈𝑝 (14)

= 𝑀𝑝 𝑥̈ 𝑐𝑜𝑠𝜃𝑝
Total force acting on the center of gravity of the robot body:
∑ 𝑀𝑜 = 𝐼𝛼

−(𝐻𝐿 + 𝐻𝑅 )𝑙𝑐𝑜𝑠𝜃𝑝 − (𝑃𝐿 + 𝑃𝑅 )𝑙𝑠𝑖𝑛𝜃𝑝 − (𝐶𝐿 + 𝐶𝑅 ) = 𝐼𝑝 𝜃̈𝑝 (15)

We have:
−2𝑘𝑚 𝑘𝑒 𝑥̇ 2𝑘𝑚
𝐶𝐿 + 𝐶𝑅 = + 𝑉
𝑅 𝑟 𝑅 𝑎
Substituting to equation 15:
−2𝑘𝑚 𝑘𝑒 𝑥̇ 2𝑘𝑚 (16)
−(𝐻𝐿 + 𝐻𝑅 )𝑙𝑐𝑜𝑠𝜃𝑝 − (𝑃𝐿 + 𝑃𝑅 )𝑙𝑠𝑖𝑛𝜃𝑝 − ( + 𝑉)
𝑅 𝑟 𝑅 𝑎
= 𝐼𝑝 𝜃̈𝑝
Multiplying -1 to equation 14 and substituting to equation 16:
2𝑘𝑚 𝑘𝑒 2𝑘𝑚 (17)
𝐼𝑝 𝜃̈𝑝 − 𝑥̇ + 𝑉𝑎 + 𝑀𝑝 𝑔𝑙𝑠𝑖𝑛𝜃𝑝 + 𝑀𝑝 𝑙 2 𝜃̈𝑝 = −𝑀𝑝 𝑙𝑥̈ 𝑐𝑜𝑠𝜃𝑝
𝑅𝑟 𝑅
Substituting equation 13 to equation 12:
𝐼𝑤 (18)
2 (Mw + 2 ) 𝑥̈
𝑟
−2𝑘𝑚 𝑘𝑒 2𝑘𝑚
= 2
𝑥̇ + 2
𝑉𝑎 − 𝑀𝑝 𝑥̈ − 𝑀𝑝 𝑙𝜃̈ 𝑝 𝑐𝑜𝑠𝜃𝑝
𝑅𝑟 𝑅𝑟
2
+ 𝑀𝑝 𝑙𝜃̇ 𝑝 𝑠𝑖𝑛𝜃𝑝
The two equations (17) and (18) are the system of nonlinear equations. To
linearize the above model, we have:
𝑐𝑜𝑠𝜃𝑝 = −1
𝑠𝑖𝑛𝜃𝑝 = −𝜙

13
 2
𝑑𝜃𝑝
( ) =0
𝑑𝑡
Then we have linear model of the system:
2𝑘𝑚 𝑘𝑒 2𝑘𝑚 (19)
(Ip + Mp l2 )ϕ̈ − 𝑥̇ + 𝑉𝑎 − 𝑀𝑝 𝑔𝑙𝜙 = 𝑀𝑝 𝑙𝑥̈
𝑅𝑟 𝑅
2𝑘𝑚 2𝐼𝑤 2𝑘𝑚 𝑘𝑒 (20)
𝑉𝑎 = (2𝑀𝑤 + 2 + 𝑀𝑝 ) 𝑥̈ + 2
𝑥̇ − 𝑀𝑝 𝑙𝜙̈
𝑅𝑟 𝑟 𝑅𝑟
To get the state variable model of the system, we derive the state variables
from equations (19) and (20), we have:
𝑀𝑝 𝑙 2𝑘𝑚 𝑘𝑒 2𝑘𝑚 𝑀𝑝 𝑔𝑙
ϕ̈ = 𝑥̈ + 𝑥̇ − 𝑉 + 𝜙
Ip + Mp l2 𝑅𝑟(Ip + Mp l2 ) 𝑅(Ip + Mp l2 ) 𝑎 𝑅(Ip + Mp l2 )
2𝑘𝑚 2𝑘𝑚 𝑘𝑒
ẍ = 𝑉𝑎 − 𝑥̇
2𝐼𝑤 2 2𝐼𝑤
𝑅𝑟 (2𝑀𝑤 + 2 + 𝑀𝑝 ) 𝑅𝑟 (2𝑀𝑤 + 2 + 𝑀𝑝 )
𝑟 𝑟
𝑀𝑝 𝑙
+ 𝜙̈
2𝐼𝑤
(2𝑀𝑤 + 2 + 𝑀𝑝 )
𝑟
By substituting 2 equations above to equation (19) and (20), we get state
space equation as:
0 1 0 0
𝑥̇ 2 2 𝑥
2𝑘𝑚 𝑘𝑒 (𝑀𝑝 𝑙𝑟 − 𝐼𝑝 − 𝑀𝑝 𝑙 ) 𝑀𝑝 𝑔𝑙 2
𝑥̈ 0 0 𝑥̇
[𝜙̇] = 𝑅𝑟 2 𝛼 𝛼 [𝜙]
0 0
𝜙 ̈ 0 2𝑘𝑚 𝑘𝑒 (𝑟𝛽 − 𝑀𝑝 𝑙) 𝑀𝑝 𝑔𝑙𝛽 1 𝜙̇
[0 𝑅𝑟 2 𝛼 𝛼
0]

0
2𝑘𝑚 (𝐼𝑝 + 𝑀𝑝 𝑙 2 −𝑀𝑝 𝑙𝑟)
+ 𝑅𝑟 2 𝛼 𝑉𝑎
0
2𝑘𝑚 (𝑀𝑝 𝑙 − 𝑟𝛽)
[ 𝑅𝑟𝛼 ]
Where
2𝐼𝑤
β = (2𝑀𝑤 + + 𝑀𝑝 )
𝑟2
𝐼𝑤
α = [Ip β + 2Mp l2 (𝑀𝑤 + 2 )]
𝑟

14
With the above model, it is only true when the car is always on the plane and the
friction of the wheel with the plane is ignored, and the force is negligible.
2.5 Simulation
2.5.1 System parameter declaration

clear all;
clc;
km=0.022;
ke=0.4;
R=0.0325;
r=3;
Mp=1.05;
Mw=0.06;
Ip=0.0012;
Iw=0.000016;
l=0.1075;
g=9.81;
teta_init=0;
teta_dot_init=0;
x_init=0;
x_dot_init=0;
beta=2*Mw+2*Iw/r^2+Mp;
alpha=Ip*beta+2*Mp*l^2*(Mw+Iw/r^2);

2.5.2 System stability investigation

Using Matlab to calculate transfer function of the system:

%---SYSTEM TRANSFER FUNCTION---%

[num,den] = ss2tf(A,B,C,D)
HTXE1 = tf(num(1,:),den)
HTXE2 = tf(num(2,:),den)

%rlocus (A,B,C,D)
pzmap (A,B,C,D)

nghiem = roots(den)

15
The result is:

HTXE1 =

-51.33 s^2 + 5.47e-13 s + 2.325e04

----------------------------------
s^4 - 6.843 s^3 - 453 s^2 - 23.3 s

Continuous-time transfer function.

HTXE2 =

-0.03946 s^2 - 3668 s + 2.316e-12

----------------------------------
s^4 - 6.843 s^3 - 453 s^2 - 23.3 s

Continuous-time transfer function.

nghiem =

0
24.9996
-18.1048
-0.0515

16
 Figure 15) Pole-Zero of system

There are 2 poles (0 & 25) are not on the left side of Imagine Axis. So the
system is unstable.
2.5.3 System controlability investigation

Based on the theory of automatic control with a linear system, the state
space equation is as follows:
𝑥̇ = 𝐴𝑥 + 𝐵𝑢

𝑦 = 𝐶𝑥 + 𝐷𝑢

Where A ∈ ℝnxn , B ∈ ℝnxu , C ∈ ℝrxn , D ∈ ℝrxm

Controllable matrix construction:
P = [B, AB, A2 B, . . . , An−1 B]
The necessary and sufficient condition for a system to be described
mathematically as a controllable state equation is rank(P)=n. Where n is the
number of state variables of the system.
Using Matlab to check controllability of the system:

%---SYSTEM CONTROLLABILITY INVESTIGATION---%

17
 P = [B A*B A^2*B A^3*B]
rank(P)

The result is:

The rank of the matrix P is equal to the number of state variables of the
system. So the system is controllable.

2.5.4 System observability investigation

1 0 0 0
Choosing matrix C = [ ]
0 0 1 0
𝑥
1 0 0 0 𝑥̇
We have: 𝐶. 𝑥(𝑡) = [ ] [ ] = [𝑥 𝜙]
0 0 1 0 𝜙
𝜙̇
Observable matrix construction:
L = [C, AC, A2 C, . . . , An−1 C]
Using Matlab to check observability of the system:

%---SYSTEM OBSERVABILITY INVESTIGATION---%

L = [C; C*A; C*A^2; C*A^3]
rank(L)

18
The result is:

The rank of the matrix L is equal to the number of state variables of the
system. So the system is observable.

Comment: From the above 2 tests, we can conclude that the system is
controllable and we can observe all 4 state variables of the system. And through
setting the matrix C to be able to observe two state variables, namely the robot's
position and the robot's tilt angle, if these two variables are stable, the whole
system will be stable.

2.5.5 controller construction

Figure 16) system description with controller

19
 In the self-balancing 2-wheel robot control system, we have to stably
control 2 parameters, namely the tilt angle and the position of the system, so we
separate it into 2 parallel or nested controllers and adjust the parameters. Kp Ki
Kd to control the voltage applied to the motor to get the desired response.

In order to stabilize the balance vehicle, we must first adjust the stability
of the vehicle's tilt angle first, and then adjust it to further stabilize the position
controller for the vehicle.

Matlab Simulink construction:

Subsystem:

Figure 17) Simulink simulation

20
 Tilt angle simulation with Kp=76, Ki=608, Kd=2.4. (Exl=0.1,
POT=2400%, txl=20s)

Figure 18) tilt angle simulation

Position simulation with Kp=700, Ki=300, Kd=-780. (Exl=0.3,
POT=200%, txl=20s)

Figure 19) position simulation

21
CHAPTER 3: SYSTEM DESIGN
3.1 Hardware
3.1.1 Arduino nano

Figure 20) Arduinano pins

22
Analog:

Digital:

23
 3.1.2 CNC shield V4

Figure 21) CNC shield V4 pins

CNC shield V4 features:
 3 axis stepper motor driver
 Compatible with micro-drive laser engraving machine, three-axis
CNC engraving machine
 Can control the two-phase four-wire stepper motor up to 2A
 Digital IO interface enables easy connections to other modules, such
as ENDSTOPs
 I2C interface enables connection to the LCD I2C or other I2C
module
 DC 5v interface, 7.5-12V voltage input
 GRBL compatible
 This model is designed to do full steps only
 Dimensions: 71.5 x 58 x 12.5mm

24
 3.1.3 Driver A4988
Circuit connection:

Figure 22) A4988 connection

Working mode:

3.1.4 Stepper motor size 42 (1.8º)

Figure 23) stepper motor

25
 3.1.5 GY-521 MPU 6050 angel sensor

Figure 24) MPU 6050

MPU 6050 pinout configuration:

26
MPU6050 Features
 MEMS 3-aixs accelerometer and 3-axis gyroscope values combined
 Power Supply: 3-5V
 Communication: I2C protocol
 Built-in 16-bit ADC provides high accuracy
 Built-in DMP provides high computational power
 Can be used to interface with other IIC devices like magnetometer
 Configurable IIC Address
 In-built Temperature sensor

27
 3.1.6 65mm wheels

Figure 25) wheels

Specifictions:
 Material: Hard plastic, foam padding, good rubber
 Diameter: 65mm
 Wheel width: 27mm
3.1.7 Battery

Figure 26) Pin lipo 3s

 Voltage: 11.1V (12.6V fully charge)
 Capacity: 2200 mAh
 Continuous discharge current: 25C/55A (Maximum discharge
current 25x2200/1000=55A)
 MAX threshold: 50C/110A.
 Type: Lithium Polymer

28
  Size: 106mm x 33mm x 23mm
 Weight: 166g
3.2 Control alogrithm flowchart

Figure 27) flowchart

3.3 Software design
3.3.1 Main program
 CNC shield configuration:

#define Enable 8 //D8 //PORTB 0

#define Step_3 7 //D7 //PORTD 7
#define Step_2 6 //D6 //PORTD 6
#define Step_1 5 //D5 //PORTD 5
#define Dir_3 4 //D4 //PORTD 4
#define Dir_2 3 //D3 //PORTD 3
#define Dir_1 2 //D2 //PORTD 2
#define MS3 9 //D9 //PORTB 1
#define MS2 10 //D10 //PORTB 2
#define MS1 11 //D11 //PORTB 3

29
 Arduino configuration:

void pin_INI() {
pinMode(Enable, OUTPUT);
pinMode(Step_1, OUTPUT);
pinMode(Step_2, OUTPUT);
pinMode(Step_3, OUTPUT);
pinMode(Dir_1, OUTPUT);
pinMode(Dir_2, OUTPUT);
pinMode(Dir_3, OUTPUT);
pinMode(MS1, OUTPUT);
pinMode(MS2, OUTPUT);
pinMode(MS3, OUTPUT);
digitalWrite(Enable, LOW);
digitalWrite(MS1, HIGH);
digitalWrite(MS2, HIGH);
digitalWrite(MS3, HIGH);
}

 Timer2:
 fo=16.000.000/8=2.000.000 Hz
To=1/fo=1/2.000.000 s=0.5us
timer=40*0.5=20us

void timer_INI() {
TCCR2A = 0;
TCCR2B = 0;
TCCR2B |= (1 << CS21);
OCR2A = 39;
TCCR2A |= (1 << WGM21);
TIMSK2 |= (1 << OCIE2A);
}

 Interupt timer 2:
 1 pulse -> 20us, A4988 (1/16 step mode), 1 step=1.8º=16
pulses.
=> 1 round = 360º = 200 steps = 200*16 = 3200 pulses

ISR(TIMER2_COMPA_vect) {

if (Dir_M1 != 0) {
Count_timer1++;
if (Count_timer1 <= Count_TOP1) PORTD |= 0b00100000;

30
 else PORTD &= 0b11011111;
if (Count_timer1 > Count_BOT1) {
Count_timer1 = 0;
if (Dir_M1 > 0) Step1++;
else if (Dir_M1 < 0) Step1--;
}
}

if (Dir_M2 != 0) {
Count_timer2++;
if (Count_timer2 <= Count_TOP2) PORTD |= 0b01000000;
else PORTD &= 0b10111111;
if (Count_timer2 > Count_BOT2) {
Count_timer2 = 0;
if (Dir_M2 > 0) Step2++;
else if (Dir_M2 < 0) Step2--;
}
}

if (Dir_M3 != 0) {
Count_timer3++;
if (Count_timer3 <= Count_TOP3) PORTD |= 0b10000000;
else PORTD &= 0b01111111;
if (Count_timer3 > Count_BOT3) {
Count_timer3 = 0;
if (Dir_M3 > 0) Step3++;
else if (Dir_M3 < 0) Step3--;
}
}
}

 Speed function:
 V=60/(x*3200*20*10^(-6)) rpm
Example: X=1 => V=937.5 rpm
X=10 => V=93.75 rpm
X=400 => V=2.3 rpm
void Speed_L(int16_t x) {
if (x < 0) {
Dir_M1 = -1;
PORTD &= 0b11111011;
} else if (x > 0) {
Dir_M1 = 1;
PORTD |= 0b00000100;
} else Dir_M1 = 0;

Count_BOT1 = abs(x);
Count_TOP1 = Count_BOT1 / 2;

31
}

void Speed_R(int16_t x) {
if (x < 0) {
Dir_M2 = -1;
PORTD &= 0b11110111;
} else if (x > 0) {
Dir_M2 = 1;
PORTD |= 0b00001000;
} else Dir_M2 = 0;

Count_BOT2 = abs(x);
Count_TOP2 = Count_BOT2 / 2;
}

 Main function:
 Set up:

void setup() {
mpu6050.init(0x68);
Serial.begin(9600); //Khai báo Serial
pin_INI(); //Khai báo PIN Arduino đấu nối 3 DRIVER
A4988
timer_INI(); //Khai báo TIMER2
delay(500);
loop_timer = micros() + 4000;
}

 Read angle:

void loop() {
offset = 0.5;
float AngleY = mpu6050.getYAngle();
input = AngleY + offset;

 PID and speed calculation:

i += input;
output = kp * input + kd * i + kd * (input - input_last);
input_last = input;
if (output > -5 && output < 5) output = 0;
output = constrain(output, -400, 400);

outputL = output;

32
 outputR = output;
if (output > 0) {
M_L = 405 - (1 / (outputL + 9)) * 5500;
M_R = 405 - (1 / (outputR + 9)) * 5500;
} else if (output < 0) {
M_L = -405 - (1 / (outputL - 9)) * 5500;
M_R = -405 - (1 / (outputR - 9)) * 5500;
} else {
M_L = 0;
M_R = 0;
}

if (M_L > 0) motorL = 400 - M_L;

else if (M_L < 0) motorL = -400 - M_L;
else motorL = 0;

if (M_R > 0) motorR = 400 - M_R;

else if (M_R < 0) motorR = -400 - M_R;
else motorR = 0;

Speed_L(motorL);
Speed_R(motorR);
while (loop_timer > micros());
loop_timer += 4000;

3.3.2 sensor library program

#ifndef _SAIGONTECH_MPU6050_H_
#define _SAIGONTECH_MPU6050_H_

#include "Wire.h"
#define RAD2DEG 57.295779

class SMPU6050 {
public:
SMPU6050() {
};

void init(int address) {

this->i2cAddress = address;

this->gyroXOffset = 0;
this->gyroYOffset = 0;
this->gyroZOffset = 0;

this->xAngle = 0;

33
 this->yAngle = 0;
this->zAngle = 0;

this->accX = 0;
this->accY = 0;

this->prevMillis = millis();

Wire.begin();
Wire.beginTransmission(this->i2cAddress);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);

Wire.beginTransmission(this->i2cAddress);

Wire.write(0x19);
Wire.write(0);
Wire.endTransmission(true);

Wire.beginTransmission(this->i2cAddress);
Wire.write(0x1B);
Wire.write(0);
Wire.endTransmission(true);

Wire.beginTransmission(this->i2cAddress);
Wire.write(0x1C);
Wire.write(0);
Wire.endTransmission(true);
}
void calibrate(int times) {
long gyroXTotal = 0, gyroYTotal = 0, gyroZTotal = 0;
int count = 0;
int gyroRawX, gyroRawY, gyroRawZ;
for (int i = 0; i < times; i++) {
Wire.beginTransmission(this->i2cAddress);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(this->i2cAddress, 6, true);

gyroRawX = Wire.read() << 8 | Wire.read();

gyroRawY = Wire.read() << 8 | Wire.read();
gyroRawZ = Wire.read() << 8 | Wire.read();

gyroXTotal += gyroRawX;
gyroYTotal += gyroRawY;
gyroZTotal += gyroRawZ;
count += 1;

34
 }
gyroXOffset = -gyroXTotal * 1.0 / count;
gyroYOffset = -gyroYTotal * 1.0 / count;
gyroZOffset = -gyroZTotal * 1.0 / count;
}

double getXAngle() {
this->readAngles();
return this->xAngle;
};

double getYAngle() {
this->readAngles();
return this->yAngle;
};

double getZAngle() {
this->readAngles();
return this->zAngle;
};

double getXAcc() {
this->readAngles();
return this->accX;
};

double getYAcc() {
this->readAngles();
return this->accY;
};

void getXYZAngles(double &x, double &y, double &z) {

this->readAngles();
x = xAngle;
y = yAngle;
z = zAngle;
}

private:
int i2cAddress;
double accX, accY, gyroX, gyroY, gyroZ;
double gyroXOffset, gyroYOffset, gyroZOffset;
double xAngle, yAngle, zAngle;
unsigned long prevMillis;

void readAngles() {
if (millis() - this->prevMillis < 3)

35
 return;

int accRawX, accRawY, accRawZ, gyroRawX, gyroRawY, gyroRawZ;

Wire.beginTransmission(this->i2cAddress);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(this->i2cAddress, 14, true);

accRawX = Wire.read() << 8 | Wire.read();

accRawY = Wire.read() << 8 | Wire.read();
accRawZ = Wire.read() << 8 | Wire.read();
Wire.read(); Wire.read();
gyroRawX = Wire.read() << 8 | Wire.read();
gyroRawY = Wire.read() << 8 | Wire.read();
gyroRawZ = Wire.read() << 8 | Wire.read();

accX = atan((accRawY / 16384.0) / sqrt(pow((accRawX /

16384.0), 2) + pow((accRawZ / 16384.0), 2))) * RAD2DEG;
accY = atan(-1 * (accRawX / 16384.0) / sqrt(pow((accRawY /
16384.0), 2) + pow((accRawZ / 16384.0), 2))) * RAD2DEG;

gyroX = (gyroRawX + gyroXOffset) / 131.0;

gyroY = (gyroRawY + gyroYOffset) / 131.0;
gyroZ = (gyroRawZ + gyroZOffset) / 131.0;

unsigned long curMillis = millis();

double duration = (curMillis - prevMillis) * 1e-3;
prevMillis = curMillis;

xAngle = 0.98 * (xAngle + gyroX * duration) + 0.02 * accX;

yAngle = 0.98 * (yAngle + gyroY * duration) + 0.02 * accY;
zAngle = zAngle + gyroZ * duration;
}
};

void mpu6050Init(SMPU6050 &smpu, int address) {

smpu.init(address);
}

void mpu6050Calibrate(SMPU6050 &smpu, int times) {

smpu.calibrate(times);
}

double mpu6050GetXAngle(SMPU6050 &smpu) {

return smpu.getXAngle();
}

36
 double mpu6050GetYAngle(SMPU6050 &smpu) {
return smpu.getYAngle();
}

double mpu6050GetZAngle(SMPU6050 &smpu) {

return smpu.getZAngle();
}

void mpu6050GetXYZAngles(SMPU6050 &smpu, double &xAngle, double

&yAngle, double &zAngle) {
smpu.getXYZAngles(xAngle, yAngle, zAngle);
}

#endif /*_SAIGONTECH_MPU6050_H_*/

3.4 Realistic model

Figure 28) realistic model

37
 Figure 29) realistic model
3.5 Realistic testing
3.5.1 Standard PID

kp 14
ki 1
kd 2

setpoint 0
max value 1.07
avr value -0.00566

pot 19019.55 %
Exl 0.005656
txl 0.32 s
Table 1) Standard PID

POT high in order to make the robot responses immediately. Exl and txl
very small which make the robot operate accurately.

38
 Figure 30) Standard PID

3.5.2 Kp increased/decreased

Figure 31) Kp increased

39
 Figure 32) Kp decreased

kp 30 kp 5
ki 1 ki 1
kd 2 kd 2

setpoint 0 Setpoint 0
Max 1.87 Max 6
value value
Avr -0.00084 Avr 0.003566
value value

POT 221889.3 % POT 168137 %

Exl 0.000843 Exl 0.003566
txl 12.53 s Txl 6 s
Table 2) Kp increased Table 3) Kp decrease
Comment: POT & txl Comment: POT & Exl
increased, Exl decreased. decreased, txl increased.

According to theory:
 Kp increased: POT increased, exl decreased, txl small change.
 Kp decreased: POT decreased, exl increased, txl small change.
The result of investigation is quite the same as theory.

40
3.5.3 Ki increased/decreased

Figure 33) Ki increased

Figure 34) Ki decreased

41
 kp 14 kp 14
ki 5 ki 0.01
kd 2 kd 2

setpoint 0 Setpoint 0
Max 1.7 Max 1.8
value value
Avr - Avr -7.22945E-
value 2.43309E- value 05
05

POT 6987100 % POT 2489915.385 %

Exl 2.43309E- Exl 7.22945E-05
05
txl 5.62 s Txl 7.54 s
Table 4) Ki increased Table 5) Ki decrease
Comment: POT & txl Comment: POT & txl increased,
increased, Exl decreased. Exl decreased.

According to theory:
 Ki increased: POT increased, exl decreased sharply, txl increased.
 Ki decreased: POT decreased, exl increased, txl decreased.
The result of investigation is quite the same as theory.
3.5.4 Kd increased/decreased

42
 Figure 35) Kd increased

Figure 36) Kd decreased

kp 14 kp 14
ki 1 ki 1
kd 5 kd 0.5

setpoint 0 Setpoint 0
Max 2.5 Max 2.36
value value
Avr 0.000599 Avr -0.00026
value value

POT 416994.4 % POT 913776 %

Exl 0.000599 Exl 0.000258
txl 1.6 s Txl 21.32 s
Table 6) Kd increased Table 7) Kd decrease
Comment: POT & txl Comment: POT & txl increased,
increased, Exl decreased. Exl decreased.

According to theory:
 Kd increase: POT decreased, txl increased, exl small change.
 Kd decreased: POT increased, txl decreased, exl small change.
The result of investigation is quite the same as theory.

43
CHAPTER 4: CONCLUSION AND DEVELOPMENT
4.1 Results
After finishing the project, we have achived goals:
 Controlling step motor via driver A4988 and Arduino.
 Using angle sensor MPU6050
 Using PID to adjust speed and direction of motor in order to balance
the robot.
 Simultion, tuning PID and testing the realistic model.
However, there are some limitations due to the restrict of knowledge,
hardware and software. In further study, we will try to get better results and fix
current limitations.
4.2 Limtitations and development directions
4.2.1 Limitations
 The tilt angle can not be more than 5º.
 The robot can not carry heavy things (more than 0.5kg)
 The robot just can be balanced on a flat surface.
 PID tuning and hardware are not good, the robot still fluctuating.
4.2.2 Development directions
 The robot can be controlled via Bluetooth.
 Using more controlable algorithms to improve the quality.
 Improve hardware in order to carry heavier things (humans, goods,
boxes,…)

44
REFERENCES
1) Youtube: “Self balancing robot using stepper motor, dành cho người mới
bắt đầu” _ Arduino cho người bắt đầu
2) Youtube: “điều khiển PID xe hai bánh tự cân bằng” _ Nguyen Van Dong
Hai
3) Cù Minh Phước, Nguyễn Khắc Mẫn, "Xây dựng chế tạo robot hai bánh tự
đồ án tốt nghiệp Đại học SPKT TPHCM," 2013.
4) Bùi Lê Anh, Nguyễn Ngọc Sơn, Đào Anh Vũ _ “THIẾT KẾ, MÔ PHỎNG
XE HAI BÁNH TỰ CÂN BẰNG SỬ DỤNG BỘ ĐIỀU KHIỂN PID”. Báo
cáo cuối kỳ Đại học SPKT TPHCM.

45