ABSTRACT

Two-wheeled self-balancing robot is a popular model in control system

experiments which is more widely known as inverted pendulum and cart model.

This is a multi-input and multi-output system which is theoretical and has been

applied in many systems in daily use. Most research just focus on balancing this

model through try-on experiments or by using simple form of mathematical model.

There were few researches that focus on complete mathematical modeling and

designing a controller for the system.

In this project we aim to design, construct and implement the control of a two-

wheel self-balancing robot using LQR Controller. The system consist a pair of DC

motor, motor controller and Arduino 101 microcontroller board with inbuilt 3-axis

gyroscope and a 3-axis accelerometer, employed for vertical angle determination

and encoders for drift and linear velocity determination. In addition, a

complementary filter is implemented to compensate for accelerometer noise and

gyro drifts.

The controller was tested with different case of system condition practically

and through simulation on Simulink and Matlab programming.

iv
 
 TABLE OF CONTENTS

CHAPTER NO. TITLE PAGE NO.

ABSTRACT iv
ACKNOWLEDGEMENT v
LIST OF FIGURES ix
LIST OF TABLES xi
LIST OF SYMBOLS xii
1 INTRODUCTION
1.1 OVERVIEW 1
1.2 LITERATURE REVIEW 1
1.3 PROBLEM STATEMENT 3
1.4 PROBLEM SOLVING 3
1.5 OBJECTIVE OF THE REPORT 4
1.6 METHODOLOGY 4
1.7 OUTLINE OF THE REPORT 4
2 THEORITICAL BACKGROUND
2.1 PENDULUM MODEL 6
2.1.1 Simple Pendulum Model 6
2.1.2 Inverted Pendulum Model 6
2.2 CONTROLLERS 7
2.2.1 PID 7
2.2.2 LQR 8
3 MATHEMATICAL MODEL
3.1 TWO WHEELED INVERTED 10
PENDULUM MODEL

vi
 
 3.2 SYSTEM PARAMETERS 10
3.3 SYSTEM KINEMATICS 11
3.4 MOTOR DYNAMICS 13
3.5 SYSTEM DYNAMICS 13
3.6 LQR CONTROLLER 20
4 MECHATRONIC SYSTEM
4.1 MECHANICAL SYSTEM 22
4.1.1 Main Frame 22
4.1.2 Wheels 22
4.2 ELECTRICAL SYSTEM 23
4.2.1 Microcontroller 23
4.2.2 Motor 23
4.2.3 Motor Driver 24
4.2.4 Encoder 24
4.2.5 PCB 25
4.2.6 Inertial Sensor 26
4.2.7 Power Source 29
4.3 HARDWARE MODEL 30
4.4 OVERALL DESIGN 30
4.4.1 Algorithm 30
4.4.2 Layout 31
4.4.3 Working 31
5 SIMULATION
5.1 SOFTWARE MODEL 32
5.1.1 3D SketchUp Model Design 32
5.1.2 Simulink Model 33

vii
 
 5.1.3 MATLAB Interfacing 33
5.2 SIMULATION OUTPUT 34
5.2.1 Analytical Results 34
(i) Newtonian Method
(ii) Lagrangian Method
5.2.2 Graphical Results 35
(i) Newtonian Method
(ii) Lagrangian Method
6 CONCLUSION 42
7 FUTURE IMPROVEMENTS 43
A APPENDICES 44
REFERENCES 53

viii
 
 LIST OF FIGURES

FIGURE NO. TITLE PAGE NO.

1.1 Application Of Inverted Pendulum 1
2.1 Simple Pendulum 6
2.2 (A) Inverted Pendulum On A Linear Cart 6
(B) Self-Balancing Robot
2.3 Block Diagram Of PID Controller. 7
2.4 Block Representation Of State Space System 8
3.1 Inverted Pendulum Model 10
3.2 System and Its Trajectory 11
3.3 Geometry of the Body 12
3.4 Electrical Motor Model 13
3.5 Forces On The System 14
4.1 Two Wheeled Self Balancing Robot 22
4.2 Motor With Encoder 24
4.3 Quadrature Pulse Of Encoder 25
4.4 Motor Encoder 25
4.5 PCB Layout 25
4.6 Block Diagram For Data Flow 26
4.7 Vector Representation 27
4.8 Working Of Gyroscope 27
4.9 Sensor Fusion 28
4.10 Complementary Filter 29
4.11 Hardware Model 30

ix
 
 4.12 Work flow 30
4.13 Working Layout 31
5.1 3D Sketch Up Model 32
5.2 Simulink Model With Controller 33
5.3 Simulink Model Without Controller 33
5.4 Cart and Pendulum Position vs. Time 35
(Without Controller)
5.5 Cart and Pendulum Position vs. Time 35
(With Controller)
5.6 (a) Roll Angle vs. Time (Without Controller) 36
(b) Linear Velocity vs. Time
(Without Controller)
5.7 (a) Pitch Angle vs. Time (Without Controller) 37
(b) Pitch Velocity vs. Time (Without
Controller)
5.8 (a) Yaw Angle vs. Time (Without Controller) 38
(b) Yaw Velocity vs. Time (Without
Controller)
5.9 (a) Roll Angle vs. Time (With Controller) 39
(b) Linear Velocity vs. Time (With Controller)
5.10 (a) Pitch Angle vs. Time (With Controller) 40
(b) Pitch Velocity vs. Time (With Controller)
5.11 (a)Yaw Angle vs. Time (With Controller) 41
(b) Yaw Velocity vs. Time (With Controller)

x
 
 LIST OF TABLES

TABLE NO. TITLE PAGE NO.

2.1 Comparison between PID and LQR 9
3.1 System parameters 10
4.1 Motor Specification 23
4.2 Encoder Specification 25
4.3 3db Cut-off Frequency for Accelerometer 27
4.4 3db Cut-off Frequency for Gyroscope 28

 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

xi
 
 LIST OF SYMBOLS

SYMBOL DESCRIPTION
Linear velocity
Linear acceleration
Roll angle of left wheel
Roll angle of right wheel
Pitch velocity
Pitch acceleration
Yaw (drift) velocity
Yaw (drift) acceleration
Vertical angle of the pendulum
Roll angle
Pitch angle
Yaw (drift) angle
D Body depth
G Gravitational acceleration at the surface of the earth
H Body height
Friction coefficient between body and DC motor
Friction coefficient between wheel and floor
JW Wheel moment of inertia
JΦ Body yaw inertia moment
JΨ Body pitch inertia moment
Kb DC motor back EMF constant
Kt DC motor torque constant
L Distance of Center of mass from wheel axle

xii
 
 M Wheel weight
M Body weight including wheel and battery
n Gear ratio
R Wheel radius
W Body width

xiii
 
 CHAPTER 1

INTRODUCTION

1.1 OVERVIEW

The Two Wheeled Self-Balancing Robot works on the principle

of inverted pendulum, which is a classical control system that is developed to
balance itself in the vertical position. The system is inherently unstable and even
a slight disturbance would cause the system to fall. Hence a suitable controller
has to be designed to maintain the stability of the system.

Self - Balancing Robots have been a topic of interest of many

researchers, students and hobbyist worldwide. The primary practical application
of a Self-Balancing Robot is the Segway Personal Transporter. It is used in many
industries such as inside factory floors or for tourism in the park. The concept of
self-balancing robot finds its application in Segway, as in Fig. 1.1 and Attitude
control of Quadcopter, Rockets and Space shuttles.

Fig. 1.1 Application of inverted pendulum

1.2 LITERATURE REVIEW

Grasser and Alonso D’Arrigo, (2002). presented a paper on ‘JOE: A Mobile,
Inverted Pendulum’, IEEE Transactions on Industrial Electronics, Vol 49. The

1 
 
overview of the paper was studied and the mathematical model of self balancing
robot was analyzed.
Ogata, K. (2009). authored a book named ‘Modern Control theory, University of
Minnesota, United States of America: Person Publication. Inc.’ The algorithm of
LQR and its analysis gives the theoretical background and mathematical analysis
of self balancing robot.
Mikael Arvidsson. And Jonas Karlsson. (2012). presented a thesis on ‘Design,
construction and verification of a self-balancing vehicle. Chalmers University of
Technology.’ The software modelling is implemented using the proposed
method.
Sundin, C. and Thorstensson, F. (2013), presented a thesis on ‘Autonomous
balancing robot, Chalmers University of Technology.’ The overview of the thesis
was reviewed with help of the procedure proposed and analyzed the mathematical
model of self balancing robot.
Ooi, R. (2013). presented a thesis ‘Balancing a Two-Wheeled Autonomous
Robot. Undergraduate. The University of Western Australia.’ The overall
implementation in software uses sensor calibration, filtering and Arduino
coding.
Amir A. Bature and Mohamed. N.Ahmad (2014). published a journal ‘A
Comparison of Controllers for Balancing Two Wheeled Inverted Pendulum
Robot’. International Journal of Mechanical & Mechatronics Engineering
IJMME-IJENS Vol:14 No:03. The comparison of various controllers like PID,
Linear Quadratic Regulator and LQG were analyzed with the help of this paper.
Osama Jamil et al (2014). Presented a paper ‘Modeling, Control of a Two
Wheeled Self-Balancing Robot’. International Conference on Robotics and
Emerging Allied Technologies in Engineering, Islamabad, Pakistan, April 22-24,
2014. Algorithm implementation using Arduino was referred from this paper.

2 
 
1.3 PROBLEM STATEMENT
The Two wheel Self Balancing Robot has to be controlled so
that the body remains balanced and upright, and is resistant to a step disturbance.
The output of inertial sensors used is noisy and filtering of raw output of sensors
is required before it is fed to the microcontroller for reliable performance.
Another major problem is that the computation time required by microcontroller
to run control algorithm. Excessive computation delay leads to more time for
correction of the tilt angle of inverted pendulum and leaves the system out of
vertical equilibrium position. As compared to conventional robots, the power
required for driving motors to maintain equilibrium position is high in case of
auto balancing robots.

1.4 PROBLEM SOLVING

The goal of project is to investigate the use of digital control

algorithm, implemented on a microcontroller. The digital control algorithm uses
Linear Quadratic Regulator (LQR), which provides optimum control gain to
attain the vertical equilibrium position of the inverted pendulum, to make the
system stable. To filter the noisy outputs of inertial sensors, the outputs of
accelerometer and gyroscope are fused using complementary filter algorithm to
give reliable tilt angle information. Over the period of time, microcontrollers are
providing versatile, quicker, cheaper and reliable options. The project is built
with Arduino 101, powerful and cost effective development board having Intel
Curie module. The Complementary Filter algorithm and LQR controller is
implemented in Arduino 101. This provides the required cost effectiveness along
with low power consumption. High torque DC motors is used to drive the
inverted pendulum to its vertical equilibrium and balance entire physical
structure. Keeping track of speed is important because the information is fed back
to microcontroller in a feedback loop from wheel encoders, to minimize the error
to achieve equilibrium.

3 
 
1.5 OBJECTIVES 

To design controller for a two wheel self balancing Robot using Arduino.
 To achieve vertical stability of the Robot in two wheels.
 To make the Robot robust to external disturbances.

1.6 METHODOLOGY
The objective is fulfilled using the following method
1. Derive dynamical equations based on theory of the inverted pendulum.
2. Form state space model for the angle of deviation and position.
3. Find a controller that can control these two conditions.
4. Modeling and simulation using Simulink and Sketch up.
5. Design controller gain in MATLAB.
6. Hardware construction and physical modeling.
7. Calibration of sensors and actuators of robot.
8. Implementation of controller using Arduino
9. Testing the Final Design.

1.7 OUTLINE OF THE REPORT

Chapter 1 of the report deals with Introduction. It includes the problem
statement, Literature Review objectives of the project.
Chapter 2 of the report deals with Theoretical Background. It includes
study about control techniques namely, PID and LQR.
Chapter 3 of the report deals with Mathematical modeling of the system
(plant) and design of controller. It includes the Kinematics equation,
dynamic equations and the controller.
Chapter 4 of the report deals with Hardware Description. It includes the
Hardware specification for the stated problem and solution to it.

4 
 
 Chapter 5 of the report deals with MATLAB Simulation. It includes 3D
model design by Google SketchUp, Interfacing the 3D model with
MATLAB and the simulation results with and without controller.
Chapter 6 of this report deals with Conclusion.
Chapter 7 of this report deals with Future Improvements.

5 
 
 CHAPTER 2

THEORTICAL BACKGROUND

2.1 PENDULUM MODEL

2.1.1 Simple Pendulum Model
To understand the system better, analysis of a simple pendulum
is crucial. According to Fig. 2.1, a mass, M and moment of inertia, J is joined by
a massless rod of length L to a frictionless pivot. The angular velocity, 𝜔 and the
rate of change of angular velocity, 𝑑𝜔/𝑑𝑡 are given by the equation 2.1 and 2.2.

Fig. 2.1: Simple Pendulum

= 𝜔 (2.1)

= (2.2)

2.1.2 Inverted Pendulum Model

The Inverted pendulum is a classical problem in control systems
to explore the unstable dynamics. The common examples are given in Fig. 2.2.

(a) (b)
Fig. 2.2(a) Inverted Pendulum on a linear cart; (b) Self-Balancing Robot

6 
 
2.2 CONTROLLERS
To maintain the robot upright, commonly used controllers are
Proportional – Integral - Derivative (PID) and the Linear Quadratic Regulator
(LQR). Other research works have also explored the use of Linear - Gaussian
Control (LQG), Fuzzy Logic and Pole-Placement method. In our project, the
robot stability is controlled by LQR.

2.2.1 PID
A proportional–integral–derivative (PID) controller is a control
loop feedback widely used in industrial control systems and in a wide range of
applications requiring continuously modulated control. The controller,
continuously calculates an error value, e(t), as the difference between a desired
set point SP = r(t), measured process variable PV = y(t), and applies a correction
based on proportional, integral, and derivative terms. The controller attempts to
minimize the error over time by adjustment of a control variable u(t), determined
by a weighted sum of the control terms. The Fig. 2.3 shows the block diagram of
the PID controller in a feedback loop.

Fig. 2.3 Block diagram of PID controller

 Term P is proportional to the current value of the error, e(t).
 Term I accounts for past values of the error and integrates them over time
to produce I term. The integral term seeks to eliminate the residual error by
adding a control effect due to the historic cumulative value of the error.
 Term D is the estimate of future trend of the error, based on its current rate
of change. It is sometimes called anticipatory control, as it is effectively

7 
 
 seeking to reduce the effect of the error by exerting a control influence
generated by the rate of error change. The more rapid the change, the
greater the controlling or dampening effect.
The overall control function can be expressed mathematically as,

u(𝑡) = 𝐾𝑝𝑒(𝑡) + 𝐾𝑖 ∫ 𝑒(𝑡)𝑑𝑡 + 𝐾d 𝑒(𝑡) (2.3)

Where, u(t) is the output of the controller and 𝐾𝑝, 𝐾𝑖 and 𝐾𝑑 are non-negative
coefficients for proportional, integral and derivative terms respectively.

2.2.2 LQR
To control a linear system, LQR-control can be applied. The
states of the dynamical system are described by a state space model including the
matrices A, B and C, as seen in Fig. 2.4.

Fig. 2.4 Block representation of state space system.

The linear quadratic regulator problem deals with minimization

of the cost function. In the equation Q is positive definite (or positive semi
definite) symmetric matrix and R is positive definite symmetric matrix. The two
cost matrices Q and R, are known as weight matrices are introduced in order to
weigh how fast each corresponding element in the state vector need to reach their
desired end values.

𝐽=∫(𝑥𝑇𝑄𝑥+𝑢𝑇𝑅𝑢) 𝑑𝑡 (2.4)
The control effort, u input is given by,
u input = − K x (2.5)

8 
 
where, K is the gain matrix, defined as,
K = R−1BTP (2.6)
and P is solved from the Riccatti Equation, given in 2.6,
Q + ATP + PA − PBBTP = 0 (2.7)
For a state space system that is observable and controllable, P
has a unique Solution and the closed loop system poles are strictly in the right
half plane. LQR is a form of optimal control that aims to minimize the
performance index whist taking into account the control effort, as often, higher
input effort would imply higher energy consumption. LQR control requires
derivation of the state-space model of the system, thus it is more challenging to
implement. The mathematical description of LQR is given in Appendix, A1.

Table 2.1 Comparison between PID and LQR

Controller Advantages Disadvantages

- Easy to implement - Applicable to Single Input, Single

PID
Output System(SISO)
- Does not require the state - Does not perform as well as LQR
space model

- Applicable to MIMO - More challenging

LQR
systems - Requires derivation of State-Space
- Performs better Model

9 
 
 CHAPTER 3
MATHEMATICAL MODEL
3.1 TWO WHEELED INVERTED PENDULUM MODEL

The robot's mathematical description was divided in three

parts, one for the inverted pendulum, one for the wheels and one for the
electrical motor system. The pendulum and the wheel have three equations
each, one for rotational direction and two for x and y - direction. The
pendulum has as its vertical angle and ̇ as its angular velocity and the

wheels have and ̇ .for the angle and angular velocity and the system has
yaw, (drift) and ̇ drift velocity. Fig. 3.1 shows the orientation of the Robot
along the three axes.

Body Pitch Angle l,r Wheel Angle(Left, Right) m l,r DC Motor Angle

Fig. 3.1 Inverted Pendulum Model

3.2 SYSTEM PARAMETERS

The parameters that were used in the mathematical model are:

Table 3.1 System parameters
Symbol Units Description
g = 9.81 m/s2 Gravitational acceleration on earth

10 
 
M = 0.03328 Kg Wheel weight
R = 0.0325 M Wheel radius
JW = 1.75x10-5 kg m2 Wheel moment of inertia
M = 0.663 Kg Body weight including wheel and battery
W = 0.15 m Body width
D = 0.9 m Body depth
H= 0.118 m Body height
L=H/2 m Distance of centre of mass from wheel axle
JΨ= 2.304x10-3 kg m2 Body pitch inertia moment
JΦ= 1.21x10-3 kg m2 Body yaw inertia moment
Jm = 1.44x10-5 kg m2 DC motor inertia moment
Rm = 6.00 Ohms DC motor resistance
Kb=0.29 V s/rad DC motor back EMF constant
Kt = 0.29 Nm/A DC motor torque constant
N = 21 Gear ratio
fm = 0.0022 Nm/rad/s Friction coefficient between body and DC
motor
fw=0 N/m/s Friction coefficient between wheel and floor

3.3 SYSTEM KINEMATICS

Kinematic equation describes the system trajectory without
considering the forces that caused the motion. The diagrammatic representation
of the system and its trajectory is shown in Fig. 3.2.

Fig. 3.2 System and its trajectory

11 
 
From the geometry of motion (as shown in Fig. 3.3), we have
= = = (3.1)

where, Rl and Rr are the distance covered by left wheel and right wheel
R is the distance covered by centre of mass
Thus by solving 3.1 and 3.2, we get the roll and drift angle to be,
= ( (3.2)

= ( (3.3)
Thus the left and right wheel position is given by,

𝑥 =𝑅 (3.4)
=𝑅 𝑖 (3.5)


Fig. 3.3 Geometry of the body
𝑥 = 𝑥 𝑖 (3.6)
𝑥 = 𝑥 𝑖   (3.7)

 =  (3.8)

 = (3.9)
Next, we have for the coordinates of the centre of mass to be,
xb = xm + L sincos(3.10)

yb= ym+ L sincos   (3.11)

12 
 
3.4 MOTOR DYNAMICS
The Electrical equivalent model of the motor is shown in the Fig. 3.4.

Fig. 3.4 - Electrical Motor Model

From the circuit, the terminal Voltage is given by,

V = IR + KeL (3.12)

Since the electrical time constant is much smaller than the mechanical the
inductance can be neglected which gives equation
𝑅 𝐾 𝜔(3.13)
Shaft Torque is given by,
T = nKtI (3.14)
The shaft torque needs to overcome the inertia of the motor as well as the
viscous damping and motor friction.

3.5 SYSTEM DYNAMICS

Dynamics is concerned with the study of forces and torques and
their effect on motion.
A. Newtonian mechanics

Dynamics is governed by Newton's laws of motion. This is done

by drawing free body diagram as shown in Fig. 3.5.

The force on cart along horizontal axis is given by,

𝑀𝑅 ̈ 𝑅 ̇ 𝑁= (3.15)

13 
 
 Fig. 3.5 Forces acting on the system

The force on pendulum along horizontal axis is given by,

𝑁= 𝑅 ̈ ̈ ̇ 𝑖 (3.16)
Equating the above equations, we get,
(𝑀 𝑅 ̈ 𝑅 ̇ ̈ ̇ 𝑖 = (3.17)
The force on pendulum along its perpendicular axis is given by,
𝑖 𝑁 𝑔 𝑖 = ̈ 𝑅 ̈ (3.18)
The torque about the center of mass of the pendulum is,
𝑖 𝑁 = ̈ (3.19)
Thus,
( ̈ 𝑔 𝑖 = 𝑅 ̈ (3.20)
Since the analysis will be employed only to linear systems,
= 𝑖 = ; ̇ = (3.21)
The final equations of motion are,

( 𝑔 ̈ = 𝑅 ̈ (3.22)
(𝑀 𝑅 ̈ 𝑅 ̇ ̈ =𝑢 (3.23)
Taking Laplace transform,
( ( 𝑔 ( = ( (3.24)
(𝑀 ( ( ( = ( (3.25)

14 
 
Solving the equations,
(
=* + (3.26)
(

𝑔 𝑔
(𝑀 [ ] ( [ ] ( ( = (

(3.27)

(
= ( (
(3.28)
(

Where,
= [(𝑀 ( ( ]

(
( = = ( ) (
(3.29)
(

The transfer function with the cart position X(s) as the output can be derived as,

( )
(
( = = ( ) (
(3.30)
(

The equations can be represented in state space form as,

̇ ( 𝑔 (
̈ (𝑀 𝑀 (𝑀 𝑀 ̇ 𝑅[ (𝑀 𝑀 ]
̇ = 𝑢
[ ̈] 𝑔 (𝑀 [ ̇]
[ (𝑀 𝑀 (𝑀 𝑀 ] [ (𝑀 𝑀 ]
(3.31)

̇
= * +[ ] * +𝑢 (3.32)
̇
Where,
𝑢= ( 𝐾𝜔

15 
 
Substituting we get,

̇ ( 𝐾 𝐾( 𝑔 ( 𝐾 𝐾
̈ (𝑀 𝑀 𝑅 [ (𝑀 𝑀 ] (𝑀 𝑀 ̇ 𝑅 [ (𝑀 𝑀 ]
̇ =
[ ̈] 𝐾 𝐾 𝑔 (𝑀 [ ̇] 𝐾 𝐾
[ (𝑀 𝑀 𝑅 [ (𝑀 𝑀 ] (𝑀 𝑀 ] [𝑅 [ (𝑀 𝑀 ]]

(3.33)
The equation represents the state space model with supply voltage as input.
B. Lagrangian mechanics.
Newtonian system analysis is generally not followed for
complex systems because,
 The presence of motion constraints equations by invoking forces like
Coriolis force complicates the analysis.
 It involves vector manipulations and it is easy to make mistakes in drawing
the free body diagram.
The Lagrangian formulation of mechanics overcomes all these
drawbacks and provides an elegant frame work for mechanical modeling:
For a system of N particles with P constraints on its coordinates (holonomic
constraints), let ( 1, 2,……………………. 3 𝑁− ) be 3N-P independent
coordinates that are independent and let the real 3N Cartesian Coordinates of N
particles be a function of these generalized coordinates.
𝒓𝒊=( 1,2,……….. 3𝑁 − )                 
ri(xi,yi,zi), position vector of the ith particle (i=1,2,......N)
Then the equations of motion for those generalized coordinates is given by the
celebrated Euler Lagrange Equations as

( ̇
) ̇
= (3.34)

for i=1,2.............(3N-P) for all the generalized coordinates.

'L' is called the Lagrangian or Action and is a scalar and is defined as
L =T -V
(3.35)

16 
 
T: Total Kinetic Energy of all the particles (in the generalized velocities)
V: Total Potential Energy of all the particles (in the generalized coordinates)
Fqi : Generalized force (Force along the ith generalized coordinate)
The translational Kinetic Energy (𝑇 ), Rotational Kinectic
Energy (𝑇 ) and Potential Energy (U) is given by,
𝑇 = (𝑥̇ ̇ ̇ ) (𝑥̇ ̇ ̇ ) 𝑀(𝑥̇ ̇ ̇ )

(3.36)

𝑇 = 𝐽 ( ̇ ̇ ) 𝐽 ( ̇ ) 𝐽 ( ̇ ) 𝐽 ( ̇ ̇)

𝐽 ( ̇ ̇) (3.37)

= 𝑔 𝑔 𝑀𝑔 (3.38)
𝑇 consists of left wheel, right wheel and body’s translational kinectic energy.
𝑇 consists of wheel, body’s rotational kinectic energy along Psi, Phi axes and
Motor’s rotational energy where the effective angle moved by shaft is 
Using Euler Lagrange Equations for the generalized coordinates are

( ̇) = (3.39)

( ̇) = (3.40)

( ̇) = (3.41)

The generalised coordinate forces are given by,

[( 𝑅 𝐽 𝐽 ] ̈ (𝑀𝐿𝑅 𝐽 ̈

𝑀𝐿𝑅 ̇ 𝑖 =
(𝑀𝐿𝑅 𝐽 ̈ (𝑀𝐿 𝐽 𝐽 ) ̈ 𝑀𝑔𝐿 𝑖
𝑀𝐿𝑅 ̇ 𝑖 =

[ 𝐽 (𝐽 𝐽 𝑀𝐿 𝑖 ] ̈
𝑅
𝑀𝐿 ̇ ̇ 𝑖 =
(3.42)

17 
 
To transform the generalised force to the applied force, D’Alembert’s Principle is
used.
(
𝑄 =∑ (3.43)

𝑄 includes , , and applied consists of left and right. Substituting in the

above equation,
= (3.44)

= ( (3.45)

And the Applied force produces opposite effects on generalised coordinates i.e., 
and .
= (3.46)
The applied torque on Left and Right wheel is given as the sum of Load and
friction torque.
= 𝐾𝑖 𝑓 ( ̇ ̇

= 𝐾𝑖 𝑓 ( ̇ ̇ (3.47)
The Motor equation is given by,
=𝑅 𝐾 ( ̇ ̇ (3.48)
For small angles of , the following approximations are carried out,
𝑖 = =
̇ = ̇ =
𝑖 = (3.49)
The above equations in matrix form can be represented as,
̈ ̇ 𝑣
[ ̈] [ ̇] [ ] = *𝑣 + (3.50)

Where,
( 𝑀 𝑅 𝐽 𝐽 𝑀𝐿𝑅 𝐽
=[ ] (3.51)
𝑀𝐿𝑅 𝐽 𝑀𝐿 𝐽 𝐽

= [ ] =[ ] =* + (3.52)
𝑔𝐿𝑀

18 
 
 Where  and  are given by,
𝐾 𝐾𝐾
= =
𝑅 𝑅
[ ̈] 𝐽[ ̇ ] = [𝑣 𝑣] (3.53)

= 𝐽 (𝐽 𝐽 (3.54)

𝐽= ( 𝑓 (3.55)

𝐾= (3.56)

𝑥̇ = 𝑥 𝑢 (3.57)
Thus the state space representation with States and Outputs is as follows,

𝑥 = ̇ = ̇
[ ̇] [ ̇]

𝑥 = [ ̇] = [ ̇] (3.58)

Taking Inputs as,

𝑣
𝑢 = *𝑣 + (3.59)

And State Matrix as,

= 𝑔𝐿𝑀(𝑀𝐿𝑅 𝐽 (𝑀𝐿 𝐽 𝑀𝐿𝑅) (𝑀𝐿 𝐽Ψ 𝑀𝐿𝑅

[ 𝑔 𝑀[(𝑀 𝑅 𝐽 𝐽 ] [(𝑀 𝑅 𝐽 𝑀𝐿𝑅] [(𝑀 𝑅 𝐽 𝑀𝐿𝑅]]
Where, (2m + M) =M' (3.60)

= (𝑀𝐿 𝐽 𝑀𝐿𝑅 (𝑀𝐿 𝐽 𝑀𝐿𝑅 (3.61)

[ (𝑀𝑅 𝑅 𝑀𝐿𝑅 (𝑀𝑅 𝑅 𝑀𝐿𝑅 ]

= [ ] (3.62)

= [ ] (3.63)

19 
 
 = [ ] (3.64)

= * + (3.65)

(𝑣 𝑣 affects only the dynamics of 𝑥 and (𝑣 𝑣 affects only the yaw

dynamics. Let, 𝑢 = (𝑣 𝑣 and 𝑢 = (𝑣 𝑣 .

𝑣 = (𝑢 𝑢
𝑣 = (𝑢 𝑢 (3.66)
Thus,
𝑥̇ = 𝑥 𝑢
𝑥̇ = 𝑥 𝑢 (3.67)
Where, 1′: First column of 1
2′: First column of 2
3.6 LQR CONTROLLER

LQR controller design for the system was formulated. The linear
quadratic regulator problem deals with minimization of the cost function.

The control law that minimizes the cost function is linear, given by,
u(𝑡 = 𝑅 𝑇 x(𝑡)
The control law guarantees that, x(𝑡) 𝑎 𝑑 u(𝑡 → 𝑎 𝑡→∞ at a rate that is an
optimal tradeoff between control effort and performance, demanded by Q and R.

The crucial and difficult task in the LQR controller design is a

choice of the weighting matrices. We generally select weighting matrices Q and
R to satisfy expected performance criterion. The different Q and R values give a
different system response. The system will be more robust to disturbance and the
settling time will be shorter if Q is larger (in a certain range). But there is no
straightforward way to select these weighting matrices and it is usually done
through an iterative simulation process.

20 
 
 For various values of Q and R matrix, the controller gain is
calculated by solving Ricatti's Equation in MATLAB. An advantage of using the
LQR optimal control scheme is that system designed will be stable and robust,
except in the case where the system is not controllable.

LQR controller for the models obtained from both mathematical

analyses and state space matrix namely, Lagrangian and Newtonian Mechanics
are implemented.

21 
 
 CHAPTER 4

MECHATRONIC SYSTEM

The section aims to provide an overview of the design considerations and

implementation of the robot physically.   

4.1 MECHANICAL SYSTEM

The general design of the robot was a rectangular body on two
wheels. The wheels are placed parallel to each other. The battery was placed as
on the body.

4.1.1 Main Frame

The main frame of the body consists of three shelves and four
steel rods that can represent the four legs of the shelf. As in the Fig. 4.1, two
shelves are made of Acrylic to support the battery and fixing the motor wheel set
up. The other shelf is a PCB layer that is mounted with Arduino 101 and motor
driver circuit.

Fig. 4.1 Two Wheeled Self Balancing Robot

4.1.2 Wheel
The wheels are of dimensions, 65×26.5 mm. The wheels are
selected by considering the angular velocity of the motor and the desired speed of
the robot to keep the robot upright. The wheels are made of light plastic with a
rubber tire and attached to the motor axis hub.
22 
 
4.2 ELECTRICAL SYSTEM
4.2.1 Microcontroller
The microcontroller used in the robot is Arduino 101. It contains
the Intel Curie Module, designed to integrate the core's low power consumption
and high performance with the Arduino ease-of-use. The 101 adds Bluetooth Low
Energy capabilities and has on-board 6-axis accelerometer/gyroscope. Arduino is
programmed using the Arduino Software (IDE) which enables quick
development. The module contains two tiny cores, an x86(Quark) and a 32-
bit ARC architecture core, both clocked at 32MHz. It requires an input
voltage between 7-12V for optimal functioning and is powered and
programmed through a USB-cable.

4.2.2 Motor
To determine the appropriate motors for the robot, the first consideration is the
minimum torque required. To estimate the torque, the model is considered,
𝜏 = ‖𝒓‖‖𝑭‖sin , (4. )
where, 𝒓 is the position vector and is angle between force and position vectors.
For distance between the pivot point and the centre of mass (𝐿) is 12cm, the
maximum tilt angle (θ max) is 40° and the mass of the robot ( ) is 0.7kg, the
minimum torque, 𝜏 = 𝑔𝐿 𝑖 = 0.530 𝑁 .
Table 4.1 Motor Specification
Rated Voltage 6V
No-Load Speed 210 rpm
Max efficiency 2.0 Kg.cm/170rpm/2.0W/0.60A
Max power 5.2 Kg.cm/110rpm/3.1W/1.10A
Stall torque 10 Kg.cm 3.2A
Retarder reduction ratio 1 : 21

23 
 
 To begin with, the torque required by the motor is estimated and
the compromise between torque and RPM is done. The Motor used is DC Shunt
Gear Motor, as in the Fig. 4.2, with specification, as in the Table 4.1.

Fig. 4.2 Motor with Encoder

4.2.3 Motor Driver
To power the motors a motor driver, L298N is needed for two
6V DC-motors. The motor chosen is designed to operate at 6V and has a stall
current of 2A. The microcontroller cannot supply this power, thus a Full bridge
driver is required to allow the motor to be controller in both directions. The motor
driver circuit consists of L298N and diodes to protect the microcontroller and
battery from back EMF. The L298N chips can operate with a supply voltage up
to 42V, and can supply an RMS current of 4A (peak 5A).
The motors is controlled by setting up PWM on the direction pin
for deciding which direction the motor would go and the speed of the motor,
where 255 was full power of supply voltage Vin and 0 is completely OFF.

4.2.4 Encoder
The encoders in the motors are quadrature encoders. There are
two signals and the rising edges are counted to establish the displacement and
angular velocity. The direction in which the shaft is rotating is determined by the
phase difference between the two signals. This is shown in the Fig. 4.3. To count
the edges and determine the phase different, rising edge interrupts were setup on
channel A.
𝑅 𝑡 𝐷𝑖 𝑝 𝑎 𝑒 𝑒 𝑡 [𝑑𝑒𝑔 𝑒 ] = 𝑒𝑑𝑔𝑒 𝑢 𝑡 × (4.2)

24 
 
Where, 𝑁ppr is the number of pulses per revolution of the encoder and 𝑇 is the
time window between readings. The Encoder specification is given in Table 4.2.

Fig. 4.3 Quadrature pulse of Encoder

Table 4.2 Encoder Specification
Encoder motor end 11 signals
Hall Resolution (Nppr) 234

Fig. 4.4 Motor Encoder

4.2.5 PCB

The PCB is designed and routed using Eagle software. Refer appendix, A8 for

Fig. 4.5 PCB Layout

25 
 
schematic design of the circuit. The PCB board is made by copper coated plastic
material. The PCB Layout is shown in the Fig. 4.5. The prepared layer is printed
on the copper clad by a laminator and then etched using FeCl3 solution. Now the
tracks are obtained and the PCB is drilled by using PCB drilling machine. After
drilling, the motor driver IC and other components are soldered on the PCB. The
PCB is designed by hands and used after short circuit and voltage check.

4.2.6 Inertial Sensor

The BMI l60 is a highly integrated, low power inertial
measurement unit (IMU) that provides precise acceleration (angle) and angular
rate (gyroscopic) measurement. The data flow is shown in the Fig. 4.6.

Fig. 4.6 Block Diagram for Dataflow

The BMI l60 integrates:
 16 bit digital, triaxial accelerometer
 16 bit digital, triaxial gyroscope
a. Accelerometer

The force of gravity always acts perpendicular to the earth's

surface. The accelerometer measures the total external acceleration of the
balancing robot, which includes the gravitational and motion accelerations. From

26 
 
the Fig. 4.7, Following, the direction cosine method, one can use the X, Y and Z-
axis gravitational acceleration measurements to calculate the tilt angle. Although
this method gives quickly, it is easily influenced by external forces and noise.
Table 4.3 shows the 3db cut off frequency for accelerometer.

Fig. 4.7 Vector Representation

Table 4.3 3-db Cut-off Frequency for Accelerometer
Accelerometer ODR(Hz) 12.5 25 50 100 200 400 800 1600
3dB Cutoff Frequency 5.06 10.12 20.25 40.5 80 162 324 684

b. Gyroscope

A gyroscope measures angular velocity, the rate of change in

orientation angle. The motion of a pair of sensing arms produces a potential
difference from which angular velocity is sensed. The angular velocity is
converted to, and output as, an electrical signal. Fig 4.3, shows the working of
gyroscope. Table 4.4 shows the 3db cut off frequency for gyroscope.

(a) (b) (c)

Fig. 4.8 Working of Gyroscope

27 
 
 a) When the gyro is rotated, the Coriolis force acts on the drive
arms, producing vertical vibration.
b) Direction of rotation
c) The stationary part bends due to vertical drive arm vibration, producing a
sensing motion in the sensing arms.

Table 4.4 3-dB cut-off Frequency for Gyroscope

Gyroscope ODR(Hz) 25 50 100 200 400 800 1600 3200

3dB Cutoff Frequency 10.7 20.8 39.9 74.6 136.6 254.6 523.9 890

c. Sensor Fusion

The gyro measurements can only give us angles relative to initial

orientation, as we simply integrate the angular velocity and it is prone to
accumulate error. In addition to this, the sensor has bias errors, the angle
estimate drift. But, it is less sensitive to linear mechanical movements.

Fig. 4.9 Sensor Fusion

On the other hand, the accelerometer provides an absolute

measure of inclination, but the output signal is often corrupted with mechanical
noise and vibration.

28 
 
 Fig. 4.10 Complementary Filter

As in the Fig. 4.9, averaging the data that comes from

accelerometers and gyroscope can produce a better estimate of orientation than
obtained using a single sensor data alone. We use complementary filter which
fuses the accelerometer and gyro data by passing through a 1 st order low pass and
high pass filter respectively and adding their outputs, as shown in Fig. 4.10

The filter is represented as,

= ( ( ̇ 𝑡 (4.3)
where,  = ⁄(  

The Control Loop and Sampling rate of gyro and accelerometer: 200 Hz (1/ 𝑡)
and the Cut off frequency for low pass and high filter for raw data: 74.6 Hz (1/T)
The mathematical description of the filter is done in Appendix, A2. The
implementation is described in Appendix, A6.

4.2.7 Power Source

To provide power while maintaining the robot mobile, a Lithium
Polymer (Li-Po) 3 – cell, 12 V, 2200mAh - 8C Battery was used. The battery can
supply a current of up to 24 A. Given that the stall current of each of the motors
is 2A and the remaining components (Arduino101 and encoders) have an
estimated current draw of 500 mA, thus the battery can power up. The motor
operates with 6V which is fed from a battery through buck converter.

29 
 
4.3 HARDWARE MODEL

The hardware model that was used is shown in Fig. 4.11.

Fig. 4.11 Hardware model

4.4 OVERALL DESIGN

This section explains the sequence of events in the robot.

4.4.1 Algorithm

Fig. 4.12 Work Flow

30 
 
4.4.2 Layout

Fig. 4.13 Working Layout

4.4.3 Working
• The robot undergoes initialization and the sensor calibration takes place to
find the offset and bias error.
• The sensor data is acquired and the data is processed after filtering and the
encoder data, acquired using interrupts are the states
• Using LQR, the gain is computed by Ricatti Equation.
• The Pitch ( ) is obtained by sensor fusion using complementary filter.
The Roll angle and velocity of wheels ( ) and Yaw ( ) is obtained
from encoder.
• In Real time the states are obtained and the actuation fed to the motor is
calculated as the product of states and gain.
Refer appendix for Arduino codes of the controller.

31 
 
 CHAPTER 5

SIMULATION

5.1 SOFTWARE MODEL

5.1.1 3D Sketchup Model Design

The 3D model of the Self balancing robot as shown in Fig.

5.1, is designed using Google SketchUp software. SketchUp is a 3D
Modelling computer program used to draw 3D models of automobiles,
buildings and hardware set up for various engineering projects. In the 3D
SketchUp model, the right wheel along with the right motor form one
group, left wheel with left motor form another and the chassis forms a
separate group. Once the model is done, it needs to be interfaced with
MATLAB Simulink for further simulation. This is done with the help of
VR Sink tool (Virtual Reality Toolbox). Once it is interfaced with
MATLAB, the system behaviour can be observed for different values of
parameters.

Fig 5.1 3D SketchUp Model

32 
 
5.1.2 Simulink Model

Fig. 5.2 Simulink Model with Controller

Fig. 5.3 Simulink Model without Controller

5.1.3 MATLAB Interfacing

Once the 3D Model is generated with the help of

SketchUp, in order to understand the system behaviour, it is interfaced with
MATLAB. To perform this, the model is exported as VRML File in to the
MATLAB environment. Once this is done, the VRML File is linked with
the VR Sink Block of the 3D Animation Toolbox. The system behavior is
viewed as a simulation.

33 
 
5.2 SIMULATION OUTPUT

5.2.1 Analytical Results

The output obtained from MATLAB simulation with and without controller is
described in this section. The Matlab codes are given in given in the Appendix.
(i) Using Newtonian Method
Without controller,

 
Since all the Eigen values are not negative, the system is not completely stable
With controller, 

 
Since all the eigen values are negative, the system is stable
(ii) Using Lagrangian Method
Without controller,

 
Since all the Eigen values are not negative, the system is not completely stable
With controller,

34 
 
Since all the Eigen values are negative, the system is stable
 
5.2.2 Graphical Results
(i) Using Newtonian Method
 

Fig. 5.4 Cart and pendulum position vs. time (Without Controller)
 

Fig. 5.5 Cart and pendulum position vs. time (With Controller)

35 
 
(ii) Using Lagrangian Method

Fig. 5.6(a) Roll Angle vs. time (Without controller)

Fig. 5.6(b) Linear velocity vs. time (Without controller)

36 
 
 Fig. 5.7(a) Pitch Angle vs. time (Without controller)

Fig. 5.7(b) Pitch velocity vs. time (Without controller)

37 
 
 Fig. 5.8(a) Yaw Angle vs. time (Without controller)

Fig. 5.8(b) Yaw velocity vs. time (Without controller)

38 
 
 Fig. 5.9(a) Roll Angle vs. time (With controller)

Fig. 5.9(b) Linear velocity vs. time (With controller)

39 
 
 Fig. 5.10(a) Pitch Angle vs. time (With controller)

Fig. 5.10(b) Pitch velocity vs. time (With controller)

40 
 
 Fig. 5.11 Yaw Angle vs. time (With controller)

Fig. 5.11 Yaw velocity vs. time (With controller)

41 
 
 CHAPTER 6

CONCLUSION

The two wheeled Self balancing robot is implemented using a

LQR with Lagrangian and Newton Analysis on the self - balancing robot coded
in Arduino, by calculating gain from MATLAB. The robot stays upright and
tracks required trajectories as commanded. Before the hardware, the simulations
were performed for various controllers using MATLAB coding. The 3D model
was designed using Google SketchUp and integrated into the MATLAB
environment with the help of its 3D animation facility.

The project has been carried out from the design and production
of specific parts to the integration of electronic, mechanical and software
sections. Due to the need to use the knowledge in the fields of mechanics,
electronics, programming and control, this project is extremely interdisciplinary
and as such one of the most representative mechatronic problems.

42 
 
 CHAPTER 7

FUTURE IMPROVEMENTS

Further work will include increasing the level of autonomy of the

robot by adding a vision system, thus allowing the robot to avoid obstacles. Also,
by improving the components of the robot we hope to achieve higher speeds.

Another possible extension is to combine the Arduino system

and other sensors such ultrasonic and IR senses, GPS, digital compass and
Camera to address other advanced applications, e.g. obstacle avoidance and
perimeter following.

This system serves as a benchmark for testing controllers. So,

 Non-linear controllers can be designed and their performance can be
tested.
 Intelligent controllers can be designed and tested.
 This system can act as a model to perform comparative study on
performance for various controllers.

Also, Stability analysis of the linearized control system on actual

non-linear system can be quantitatively investigated.

43 
 
 A. APPENDICIES

A1. LQR Mathematical Analysis

44 
 
A2. Complementary Filter Mathematical Analysis

The angle is determined by, passing the accelerometer and gyro

signal to High pass and Low pass filter.
̇
= a+ ̇=

According to bilinear transformation,

𝑇 =( ) and
= ( ̇ ( (
Where, = ⁄(
= ( ( ̇ 𝑡

45 
 
A3. Matlab Code Using Lagrangian Mechanics
m = 0.03328; % Mass of Wheel
M = 0.663; % Mass of Body including Wheel, Motor, Battery.
R = 0.0325; % Radius of Wheel
Jw = 1.757 * 10 ^ (-5); % Moment of Inertia of Wheel
Jm = 1.4453* 10 ^ (-5); % Moment of Inertia of Motor
H = 0.118; % Bot Height
L = H/2;
Jpsi = 2.304 * 10 ^ (-3);
r = 6.00; % Resistance
Kt = 0.12; % Torque Constant
Kb = 0.29; % EMF Constant
n= 21.3; % Gear Ratio
fm = 0.0022; % Friction coefficient between body and DC motor
b = ((n*Kb*Kt)/r)+fm;
g = 9.8;
a = (n*Kt/r)
W = 0.15; % Width
D = 0.09; % Depth
Jphi = 1.2168 * 10 ^ (-3);
T_ = 1/100;
E = [ ((2*m+M)*R*R+2*Jw+2*n*n*Jm) (M*L*R-2*n*n*Jm); (M*L*R-2*n*n*Jm)
(M*L*L + Jpsi + 2*n*n*Jm) ];
F = 2* [b -b; -b b ]
G = [0 0; 0 -M*g*L ]
H = [a a; -a -a ]
I = 0.5*m*W*W + Jphi + (W*W)*0.5*(Jw+n*n*Jm)/(R*R)
J = ((W*W)*0.5*b)/(R*R)
K = W*a*0.5/R
Z = zeros([2,2]);
A1 = [Z eye([2,2]); inv(E)*G -inv(E)*F];
B1 = [0; 0; a*(M*L*L + Jpsi + M*L*R); -a*(2*m*R*R + M*R*L + M*R*R)]
A2 = [0 1; (-K/I) (-J/I)]
B2 = [0; (-K/I)]
Q1 = ([0.02 0 0 0; 0 100 0 0; 0 0 0.3 0; 0 0 0 1])
Q2 =([0.1 0;0 0.1]);
R1=10;
R2=10;
K1= ([1.0000 -100.3022 10.3845 -1.0666])
K2 = ([-0.002 -0.0010])
s = tf('s');
Rm = 1/(((r*Jm/Kt) * s^2) + ((Kb + (r * b/Kt))* s));
Lm = 1/(((r*Jm/Kt) * s^2) + ((Kb + (r * b/Kt))* s));
Lm_d = c2d(Lm,T_);
Rm_d = c2d(Rm,T_);

A4. Matlab Code Using Newtonian Mechanics

M =
0.5;
m =
0.2;
b =
0.1;
I =
0.006;
g =
9.8;
l =
0.3;
p =
I*(M+m)+M*m*l^2; %denominator for the A and B matrices
A =
[0 1 0 0;
0 -(I+m*l^2)*b/p (m^2*g*l^2)/p 0;
0 0 0 1;
0 -(m*l*b)/p m*g*l*(M+m)/p 0];
B = [ 0;

46 
 
 (I+m*l^2)/p;
0;
m*l/p];
C = [1 0 0 0;
0 0 1 0];
D = [0;
0];
states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'u'};
outputs = {'x'; 'phi'};
sys_ss =
ss(A,B,C,D,'statename',states,'inputname',inputs,'outputname',outputs);
poles = eig(A);
co = ctrb(sys_ss);
controllability = rank(co);
Q = C'*C;
R = 1;
K = lqr(A,B,Q,R)
Ac = [(A-B*K)];
Bc = [B];
Cc = [C];
Dc = [D];
states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'r'};
outputs = {'x'; 'phi'};
sys_cl =
ss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);
t = 0:0.01:5;
r =0.2*ones(size(t));
[y,t,x]=lsim(sys_cl,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
title('Step Response with LQR Control')

A5. Arduino code for obtaining values from Encoder

#define encoder0PinA 2
#define encoder0PinB 4
#define encoder1PinA 3
#define encoder1PinB 5
volatile long encoder0Pos=0, encoder1Pos=0;
float newposition_0=0, newposition_1=0;
float oldposition_0 =0, oldposition_1 = 0;
unsigned long newtime, oldtime = 0;
float vel_0, vel_1, vel, drift;
float R = 1, W = 1;

void setup()
{
pinMode(encoder0PinA, INPUT);
digitalWrite(encoder0PinA, HIGH); // turn on pullup resistor
pinMode(encoder0PinB, INPUT);
digitalWrite(encoder0PinB, HIGH); // turn on pullup resistor
attachInterrupt(2, doEncoder0, RISING); // encoDER ON PIN 2
pinMode(encoder1PinA, INPUT);
digitalWrite(encoder1PinA, HIGH); // turn on pullup resistor
pinMode(encoder1PinB, INPUT);
digitalWrite(encoder1PinB, HIGH); // turn on pullup resistor
attachInterrupt(3, doEncoder1, RISING); // encoDER ON PIN 3
Serial.begin (9600);

47 
 
 Serial.println("start"); // a personal quirk
}
void loop()
{
newposition_0 = encoder0Pos;
newposition_1 = encoder1Pos;
newtime = millis();
vel_0 = 360* abs((newposition_0-oldposition_0)/235) * 1000 /(newtime-
oldtime);
vel_1 = 360* abs((newposition_1-oldposition_1)/235) * 1000 /(newtime-
oldtime);
vel = (vel_0 + vel_1)/2;
drift = (R*(vel_0 - vel_1))/W;
Serial.print ("Theta Dot = ");
Serial.println (vel);
Serial.print ("Theta = ");
Serial.println (vel * (newtime-oldtime) / 1000);
Serial.print ("Drift_Velocity = ");
Serial.println (drift);
Serial.print ("Drift = ");
Serial.println (drift * (newtime-oldtime) / 1000);
oldposition_0 = newposition_0;
oldposition_1 = newposition_1;
oldtime = newtime;
delay(1000);
}

void doEncoder0()
{ { if (digitalRead(encoder0PinA) == digitalRead(encoder0PinB))
{ encoder0Pos++; }
else { encoder0Pos--; }}
{ if (encoder0Pos > 235)
{ encoder0Pos = encoder0Pos - 235;}
else if (encoder0Pos < -235)
{ encoder0Pos = encoder0Pos + 235;} } }

void doEncoder1()
{ if (digitalRead(encoder1PinA) == digitalRead(encoder1PinB))
{ encoder1Pos++; }
else { encoder1Pos--; }
{ if (encoder1Pos > 235)
{ encoder1Pos = encoder1Pos - 235;}
else if (encoder1Pos < -235)
{ encoder1Pos = encoder1Pos + 235;} } }

A6. Arduino code to obtain values from Gyroscope

#include "CurieIMU.h"
unsigned long microsPerReading, microsPrevious;
int gx, gy, gz, ax, ay, az, Psi_a;
float Total_angle, ga; //scaled Gyro values
void setup()
{
microsPerReading = 1000000 / 25;
microsPrevious = micros();
Serial.begin(9600); // initialize Serial communication
while (!Serial); // wait for the serial port to open
Serial.println("Initializing IMU device...");
CurieIMU.begin();
CurieIMU.setGyroRange(1000);
CurieIMU.setAccelerometerRange(2);

48 
 
 CurieIMU.setGyroRate(25);
CurieIMU.setAccelerometerRate(25);
}
if(e == 0)
{
for(int i=0; i<100; i++)
{
CurieIMU.readGyroScaled(gx, gy, gz);
CurieIMU.readAccelerometer(ax, ay, az);
Psia = atan2 (sqrt(ay*ay + ax*ax), az);
offset = offset + Psia;
if(i==99)
{ offset = offset/100;
e = 1;
Totalangle = 0;
Serial.print("OFFSET IS SET");
Serial.println(offset); }
delay(25); } } }
void loop()
{
unsigned long microsNow;
microsNow = micros();
if (microsNow - microsPrevious >= microsPerReading)
{
CurieIMU.readMotionSensor(ax, ay, az, gx, gy, gz);
Psi_a = (180/3.14) * atan2 (sqrt(ay*ay + ax*ax), az);
ga = (1000*gx)/(32768*25);
Total_angle = 0.70 *(Total_angle + ga) + 0.30 * Psi_a;
{ if(Psia > 0)
{ Psi = (offset - Totalangle); }
else
{ Psi = -(Totalangle + offset); } }
Serial.print(" Angle : ");
Serial.println(Total_angle);
Serial.print(" Angular Velocity : ");
Serial.println(ga*25);
microsPrevious = microsPrevious + microsPerReading;
}
}

A7. Arduino Controller Code

#include "CurieIMU.h"
#define encoder0PinA 0
#define encoder0PinB 1
#define encoder1PinA 2
#define encoder1PinB 4
int ax, ay, az;
float vel = 0, vela = 0, drift = 0, drifta = 0, Vl = 0, Vr = 0, Vlp = 0,
Vrp=0, u1=0, u2=0, offset = 0, Psia = 0, gx, gy, gz, e = 0;
float encoder0Pos = 0, encoder1Pos = 0, newposition0 = 0, newposition1 = 0,
oldposition0 = 0, oldposition1 = 0, Totalangle = 0, v0 = 0, v1 = 0, Psi = 0;
unsigned long microsPerReading, microsPrevious = 0, microsNow;

// R = 0.0325, W = 0.202

void setup()
{
pinMode(encoder0PinA, INPUT);
pinMode(encoder0PinB, INPUT);
attachInterrupt(0, doEncoder0, RISING);

49 
 
 pinMode(encoder1PinA, INPUT);
pinMode(encoder1PinB, INPUT);
attachInterrupt(2, doEncoder1, RISING);
pinMode(3, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(9, OUTPUT);
// Motor Max Speed is 4 RPS so Sampling Frequency is > 4. Taken as 25
microsPerReading = 1000000 / 200;
microsPrevious = micros();
CurieIMU.begin();
CurieIMU.setGyroRange(250);
CurieIMU.setGyroRate(200);
CurieIMU.setAccelerometerRate(200);
Serial.print(CurieIMU.getGyroOffset(X_AXIS));
Serial.print("\t");
Serial.print(CurieIMU.getGyroOffset(Y_AXIS));
Serial.print("\t");
Serial.println(CurieIMU.getGyroOffset(Z_AXIS));
Serial.println("About to calibrate. Make sure your board is stable and
upright");
delay(5000);
Serial.println("Starting Gyroscope calibration and enabling offset
compensation...");
CurieIMU.autoCalibrateGyroOffset();
Serial.println("Starting Acceleration calibration and enabling offset
compensation...");
CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
Serial.println("Internal sensor offsets AFTER calibration...");
Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS));
Serial.print("\t"); //
Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS));
Serial.print("\t"); //
Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS));
Serial.print("\t"); //
Serial.println(CurieIMU.getGyroOffset(X_AXIS));
Serial.println("CHANGE position");
delay(5000);
if(e == 0)
{
for(int i=0; i<100; i++)
{
CurieIMU.readGyroScaled(gx, gy, gz);
CurieIMU.readAccelerometer(ax, ay, az);
Psia = atan2 (sqrt(ay*ay + ax*ax), az);
offset = offset + Psia;
if(i==99)
{ offset = offset/100;
e = 1;
Totalangle = 0;
Serial.print("OFFSET IS SET");
Serial.println(offset); }
delay(25); } } }
void loop()
{ microsNow = micros();
if (microsNow - microsPrevious >= microsPerReading)
{
CurieIMU.readGyroScaled(gx, gy, gz);
CurieIMU.readAccelerometer(ax, ay, az);

50 
 
 gx = gx*(3.14/180);
Psia = (ay/abs(ay))*(atan2 (sqrt(ay*ay + ax*ax), az));
Totalangle = 0.70 *(Totalangle - 0.005*int(gx)) + 0.30 * Psia;
{ if(Psia > 0)
{ Psi = (offset - Totalangle); }
else
{ Psi = -(Totalangle + offset); } }
newposition0 = encoder0Pos;
newposition1 = encoder1Pos;
// 200*2*pi/235 = 5.347
v0 = ((5.347)*(newposition0-oldposition0));
v1 = ((5.347)*(newposition1-oldposition1));
vel = (v0 + v1)*(0.5);
drift = (0.16089)*(-v0 + v1);
vela = vela + (vel*(0.04));
{ if (vela > 6.28)
{ vela = vela - 6.28;}
else if (vela < -6.28)
{ vela = vela + 6.28;} }
drifta = drifta + (drift*(0.04));
{ if (drifta > 6.28)
{ drifta = drifta - 6.28;}
else if (drifta < -6.28)
{ drifta = drifta + 6.28;} }
Serial.print("Angle : ");
Serial.println(Psi);
Serial.print("Angular Velocity : ");
Serial.println(-gx);
Serial.print("Drift_Velocity = ");
Serial.println(drift);
Serial.print("Drift = ");
Serial.println(drifta);
Serial.print("Theta Dot = ");
Serial.println(vel);
Serial.print("Theta = ");
Serial.println(vela);
u1 = -1*( (vela)* 0.1594 + (Psi)*(-0.9974) + (vel)*(9.9941) + (-gx)*(-
38.7041) );
u2 = -1*( (drifta) * (-0.0494) + (drift) * (-6.7489) );
Vlp = (constrain((abs(Vl)) + 50, 0, 255));
Vrp = (constrain((abs(Vr)) + 90, 0, 255));
Vl = 0.5*(u1 + u2);
Vr = 0.5*(u1 - u2);
Serial.print ("Voltage Left= ");
Serial.println (Vl);
Serial.print ("Voltage Right= ");
Serial.println (Vr);
Serial.print ("Voltage Left Pulse= ");
Serial.println (Vlp);
Serial.print ("Voltage Right Pulse= ");
Serial.println (Vrp);
Serial.println ();
{ if(Vl>0)
{ analogWrite(3, Vlp);
analogWrite(5, 0); }
else
{ analogWrite(3, 0);
analogWrite(5, Vlp); } }

{ if(Vr>0)
{ analogWrite(6, Vrp);

51 
 
 analogWrite(9, 0); }
else
{ analogWrite(6, 0);
analogWrite(9, Vrp); } }

oldposition0 = newposition0;
oldposition1 = newposition1;
microsPrevious = microsPrevious + microsPerReading; } }

void doEncoder0()
{ { if (digitalRead(encoder0PinA) == digitalRead(encoder0PinB))
{ encoder0Pos++; }
else { encoder0Pos--; }}
{ if (encoder0Pos > 235)
{ encoder0Pos = encoder0Pos - 235;}
else if (encoder0Pos < -235)
{ encoder0Pos = encoder0Pos + 235;} } }

void doEncoder1()
{ if (digitalRead(encoder1PinA) == digitalRead(encoder1PinB))
{ encoder1Pos++; }
else { encoder1Pos--; }

{ if (encoder1Pos > 235)

{ encoder1Pos = encoder1Pos - 235;}
else if (encoder1Pos < -235)
{ encoder1Pos = encoder1Pos + 235;} } }

A8. PCB schematic circuit

52 
 
 REFERENCE

[1] Amir A. Bature, Salinda Buyamin, Mohamed. N.Ahmad, Mustaphpa

Muhammad (2014) A Comparison of Controllers for Balancing Two
Wheeled Inverted Pendulum Robot. International Journal of Mechanical
& Mechatronics Engineering IJMME-IJENS Vol:14 No:03

[2] Dorf, Richard & Robert H. Bishop. 2001 ‘Modern Control Systems’,
Prentice-Hall, United States of America.

[3] Grasser, Felix and Alonso D’Arrigo, 2002 ‘JOE: A Mobile, Inverted
Pendulum’, IEEE Transactions on Industrial Electronics, Vol 49.

[4] Kuo-Chun Wu (2007) Analysis and comparison of PID and state

feedback controller in inverted pendulum system. 2007 Master’s thesis,
National Kaohsiung University Department of Electrical Engineering,
Kaohsiung, Taiwan

[5] Ogata, K. (2009). ‘Modern Control theory, University of Minnesota,

United States of America: Person Publication. Inc.’

[6] Ooi, R. (2013). ‘Balancing a Two-Wheeled Autonomous Robot.

Undergraduate. The University of Western Australia.’

[7] Osama Jamil, Mohsin Jamil, Yasar Ayaz, Khubab Ahmad (2014)
Modeling, Control of a Two Wheeled Self-Balancing Robot. 2014
International Conference on Robotics and Emerging Allied

53 
 
 Technologies in Engineering (iCREATE)Islamabad, Pakistan, April 22-
24, 2014

[8] Sundin, C. and Thorstensson, F. (2013), ‘Autonomous balancing robot,

Masters of Science, Chalmers University of Technology.’
Mikael Arvidsson. and Jonas Karlsson. (2012). ‘Design, construction
and verification of a self-balancing vehicle. Chalmers University of
Technology.’

 
 
 
 

54