DEGREE PROJECT IN TECHNOLOGY,

FIRST CYCLE, 15 CREDITS

STOCKHOLM, SWEDEN 2020

Self-balancing robot
FREDRIK IHRFELT

WILLIAM MARIN

KTH ROYAL INSTITUTE OF TECHNOLOGY

SCHOOL OF INDUSTRIAL ENGINEERING AND MANAGEMENT
 Self-balancing robot

WiFi steerable self-balancing robot.

FREDRIK IHRFELT, WILLIAM MARIN

Bachelor’s Thesis at ITM

Supervisor: Nihad Subasic
Examiner: Nihad Subasic

TRITA ITM-EX 2020:36

 Abstract
This bachelor thesis aims to investigate the viability of us-
ing two wheeled self-balancing robots for package deliveries.
The movement of the two wheeled self-balancing robot re-
sembles the human movement more than a traditional four
wheeled vehicle. The goal of the report is to build a self-
balancing robot to investigate how far from the center axis
a weight can be added, as well as what the response time of
a Wireless Fidelity (WiFi) connection for steering the robot
is and how it compares to a Bluetooth connection. Balance
of the robot was achieved by using a Proportional-Integral-
Derivative (PID) controller with inputs from a gyroscope
and accelerometer. Stepper motors were used to maneuver
the robot. When the robot was constructed tests were per-
formed to evaluate how far from the center axis a weight
could be added. A test was also performed to evaluate the
WiFi connection response time with regard to the distance
between the operator and the robot, as well as the maxi-
mum range and how it compares to Bluetooth. The results
showed that a one kilogram weight could be added five cen-
timeters from the center axis, that the response time was
around 10-20 milliseconds for a distance up to 35 meters. A
WiFi connection has a longer range compared to Bluetooth
and also has a lower response time.

Keywords
Mechatronics, self-balancing, robot, package delivery,
remote controlled
 Referat
Tvåhjulig själv-balanserande robot
Denna rapport strävar efter att undersöka möjligheterna
av att använda en själv-balanserande robot för paketleve-
ranser. Rörelsen av en tvåhjulig själv-balanserande robot
liknar den mänskliga rörelsen mer än ett traditionellt fyr-
hjuligt fordon. Målet med rapporten är att bygga en själv-
balanserande robot för att undersöka hur långt från dess
centeraxel en vikt kan placeras, samt undersöka vilken re-
sponstid som uppnås med en Wireless Fidelity (WiFi)-länk
och hur en WiFi-länk jämför med en Bluetooth-länk. Ba-
lans uppnåddes genom att använda en Proportional-Integral-
Derivative (PID) regulator med input från ett gyroskop och
en accelerometer. Stegmotorer användes för att manövrera
roboten. När roboten hade konstruerats utfördes tester för
att undersöka hur långt från centrumaxeln en vikt kun-
de placeras. Ett test utfördes för att undersöka respons-
tiden för en WiFi-länk med avseende på avståndet mel-
lan operatör och robot, samt att undersöka den maximala
räckvidden och jämföra den mot Bluetooth. Resultaten vi-
sade att en vikt på ett kilogram kunde placeras fem centi-
meter från centeraxeln, att responstiden var ungefär 10-20
millisekunder för avstånd upp till 35 meter. En WiFi-länk
har en längre räckvidd än Bluetooth och kortare responstid.

Nyckelord
Mekatronik, själv-balanserande, robot, paketleverans,
fjärrstyrd
Acknowledgements

We would like to thank Nihad Subasic, supervisor and examiner, for giving us
the right knowledge and support that made this project possible. Seshagopalan
Thorapalli among other students have helped us a lot with the project.
Contents

1 Introduction 1
1.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Scope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.4 Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2 Theory 5
2.1 Inverted pendulum principle . . . . . . . . . . . . . . . . . . . . . . . 5
2.2 Control Theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.3 PID controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.4 Weight placed off center . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.5 Gyroscope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.6 Accelerometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.7 Complementary filter . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.8 Bipolar stepper motors . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.9 Stepper motor driver . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.10 WiFi communication . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3 Robot construction 13
3.1 Mechanical components . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.1.1 Baseplates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.1.2 Wheels . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.1.3 Assembly of the demonstrator . . . . . . . . . . . . . . . . . . 15
3.2 Electrical Components . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.2.1 Arduino Uno . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.2.2 Sensor for angular data . . . . . . . . . . . . . . . . . . . . . 15
3.2.3 Stepper motors and driver . . . . . . . . . . . . . . . . . . . . 16
3.2.4 Battery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
3.2.5 Remote control and receiver . . . . . . . . . . . . . . . . . . . 17
3.3 Steering of the robot . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.4 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.4.1 Robot Software . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.4.2 Remote Software . . . . . . . . . . . . . . . . . . . . . . . . . 20
4 Results 21
4.1 Off mass center test . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4.1.1 Normal weight offset . . . . . . . . . . . . . . . . . . . . . . . 22
4.1.2 Maximum weight offset . . . . . . . . . . . . . . . . . . . . . 24
4.2 Remote latency test . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
4.2.1 WiFi Latency . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
4.2.2 WiFi Range . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

5 Discussion 27
5.1 The stepper motors impact . . . . . . . . . . . . . . . . . . . . . . . 27
5.2 The ability to balance with off center weights . . . . . . . . . . . . . 27
5.3 Is it a good idea to control a robot with WiFi? . . . . . . . . . . . . 28

6 Conclusion 29

7 Recommendations and future work 31

7.1 Recommendations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
7.2 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

Bibliography 33

Appendices 36

A Weight off center model A:1

A.1 Pendulum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A:2
A.2 Weight . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A:2
A.3 Combining the equations . . . . . . . . . . . . . . . . . . . . . . . . . A:2

B Component table B:1

C Stepper motor and driver specifications C:1

D Circuit diagram D:1

E Code E:1
E.1 Robot code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . E:1
E.2 Remote code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . E:9
E.2.1 Transmitter . . . . . . . . . . . . . . . . . . . . . . . . . . . . E:9
E.2.2 Reciever . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . E:14
List of Figures

1.1 Amazon’s six wheeled autonomous delivery robot [2]. . . . . . . . . . . . 1

1.2 A CAD render of a package with uneven weight placed on the robot and
desired robot response. Made in Solid Edge [6]. . . . . . . . . . . . . . . 2

2.1 Model for an inverted pendulum. Made in Illustrator[8]. . . . . . . . . . 5

2.2 A model of the forces with the weight placed off center. Made in Illus-
trator [8]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.3 A model of the stepper motor. Made in Illustrator [8]. . . . . . . . . . . 9
2.4 A model of the stepper motor. Made in Illustrator [8]. . . . . . . . . . . 10
2.5 Half stepping of the stepper motor. Made in Illustrator [8]. . . . . . . . 10
2.6 The function of an H-bridge. Made in Illustrator [8]. . . . . . . . . . . . 11

3.1 A CAD rendering of the robot. Made in Solid Edge[6]. . . . . . . . . . . 13

3.2 The electrical components used. Made in Solid Edge [6]. . . . . . . . . . 15
3.3 The stepper motor used in the robot[25]. . . . . . . . . . . . . . . . . . . 16
3.4 The remote control attached to the 5V powerbank. Made in Solid Edge
[6]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.5 The remote display. Made in Solid Edge [6]. . . . . . . . . . . . . . . . . 17
3.6 Figure showing flow chart for the Arduino code. Made in Lucidchart [30]. 20

4.1 A CAD render of the weight mounted on the robot. Made in Solid Edge
[6]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4.2 The graph of the response when the weight was placed one centimeter
from center and a φc value of 0,0015. Made in Mathlab [31]. . . . . . . . 23
4.3 The graph of the response when the weight placed one centimeter from
center and a φc value of 0,0060. Made in Mathlab [31]. . . . . . . . . . . 23
4.4 The graph of the response when the weight was placed five centimeters
from center and a φc value of 0,0115. Made in Mathlab [31]. . . . . . . . 24
4.5 The principle of the remote latency test. Made in Illustrator [8]. . . . . 25
4.6 The graph from the latency. Made in Mathlab [31]. . . . . . . . . . . . . 25
4.7 An illustration of the signal radius and area. Made in Photoshop [32]. . 26

A.1 A model of the forces with the weight placed off center. Made in Illus-
trator [8]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . A:1
D.1 The circuit diagram for the robot. Made in Fritzing [38]. . . . . . . . . . D:1
D.2 The circuit diagram for the remote. Made in Fritzing. . . . . . . . . . . D:2
List of Tables

4.1 The masses and dimensions of the robot . . . . . . . . . . . . . . . . . . 21

4.2 The PID controller tuning parameters. . . . . . . . . . . . . . . . . . . . 21
4.3 The angular data for one centimeter offset. . . . . . . . . . . . . . . . . 23
4.4 The angular data for five centimeter offset. . . . . . . . . . . . . . . . . 24
4.5 Table showing the WiFi range with a stable, unstable and both stable
and unstable connection. . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

B.1 List of all components, quantity, weight and price . . . . . . . . . . . . . B:2

C.1 The key specifications for Japan servo KH56JM2 851 stepper motor. . . C:1
C.2 The key specifications for DRV8825 stepper motor driver. . . . . . . . . C:1
Acronyms

3D Three Dimensional.

ADC Analouge-Digtal Converter.

CAD Computer Aided Design.

IMU Inertial Measurement Unit.

LiPo Lithium Polymer.

OLED Organic Light-Emitting Diode.

PID Proportional Integral Derivative.

PLA Polylactic Acid.

PMMA Polymethyl Methacrylate.

SEK Swedish Krona.

TCP Transmission Control Protocol.

WiFi Wireless Fidelity.

Chapter 1

Introduction

In this chapter the background, purpose of the thesis, scope and method used will
be presented.

1.1 Background
Growing e-commerce drives the need for faster and more flexible package deliveries,
leading to the increase of package delivering robots. These types of robots are
expected to revolutionize the last mile delivery systems since they offer a cheaper
and more efficient way of delivery [1]. Amazon has tested a six wheeled autonomous
delivery robot for last mile deliveries in urban environments, one of the problems
with their delivery robot was that it had to share the sidewalk space with pedestrians
[2]. One way to combat the problem is to utilize a two wheeled self-balancing robot
since it has a smaller footprint and is more maneuverable due to it having a zero
turn radius. This type of robot also has the advantage of being more stable in
inclines since they lean into the incline [3].

Figure 1.1. Amazon’s six wheeled autonomous delivery robot [2].

1
 CHAPTER 1. INTRODUCTION

The principle of a two wheeled self-balancing robot is similar to that of the human
body, which is an inverted pendulum balancing the upper body around the ankles
[4]. In order to maintain balance a gyroscope and accelerometer, in the form of
an Inertial Measurement Unit (IMU) can be used to deviate the angle of the robot
relative the vertical axis. This angle is then used to increase or decrease the speed
of the motors with the goal of maintaining a constant angle [5].

1.2 Purpose
The purpose of this thesis is to evaluate the possibility of using two wheeled self-
balancing robots for package deliveries, by testing their ability to maintain balance
with an off-centered weight on top and evaluating the viability of remote control over
WiFi. In order to fulfill the purpose of this thesis, the following research questions
have been defined:

• How far off-center can a one kilogram weight be added while maintaining
balance of the robot?

• What is the response time when controlling the robot with a WiFi connection?

• How do WiFi and Bluetooth steering compare?

A package with uneven weight distribution and desired robot response is shown in
figure 1.2.

Figure 1.2. A CAD render of a package with uneven weight placed on the robot
and desired robot response. Made in Solid Edge [6].

1.3 Scope
The focus of the thesis is to build a robot and implement a code that enables the
robot to balance and to be controlled over a wireless connection. The robot should
also be able to an off centered weight without loosing balance. The budget for the
components amounts to 1 000 Swedish Krona (SEK) and the time constraint for
the thesis is five months.

2
1.4. METHOD

1.4 Method
First research was conducted to understand the system and the electronics. Then
a PID controller was designed that would enable balancing of the robot. The robot
and a remote controller was designed as a Computer Aided Design (CAD) model
in Solid Edge. The physical robot and remote controller was built with the CAD
model as a guideline. Experiments with the physical robot and remote controller
was then conducted in order to determine the robots ability to balance with an off
center weight, as well as determine the the performance of WiFi communication
between the robot and the remote controller.

3
Chapter 2

Theory

In this chapter the theory that was needed to achieve the purpose of this thesis is
presented.

2.1 Inverted pendulum principle

A two wheeled self-balancing robot builds upon the inverted pendulum principle,
seen in figure 2.1 where F is the force applied, φ is the angle from the equilibrium.
When a tilt from the equilibrium occurs the motors will generate a torque that
drives the wheels in the same direction as the tilt. The wheels will move the same
distance as the center of gravity in order to maintain balance [7]. In order to achieve
forward movement the angle set point will be increased, changing the equilibrium
point. This will lead to the wheels constantly moving in the same direction as the
lean of the robot to maintain the angle set point.

Figure 2.1. Model for an inverted pendulum. Made in Illustrator[8].

5
 CHAPTER 2. THEORY

2.2 Control Theory

Control theory is about making a system behave in a desired way, by measuring
the output signal y(t) from the system a controller can be utilized to change the
input signal u(t) the achieve a desired behaviour. By implementing a controller an
unstable system, where the output grows unlimited even though the input remains
limited, can be made stable [9]. Since the system of a two wheeled self-balancing
robot is inherently unstable a controller needs to be implemented.
The settling time of a system is defined as the time it takes after a disturbance
until the output signal of the system remains within a fixed range of the reference
signal [10], which in this thesis will be 35%.
The overshoot of a system is defined as the peak output signal measured from
the wanted output signal after a disturbance [10].

2.3 PID controller

The PID controller is a simple controller that is the most commonly used controller
in the industry. One of the advantages of using a PID controller is that no model
for the system has to be derived.
The control signal u(t) of the PID controller is based on the error e(t) between
the reference signal r(t) and the actual signal y(t) according to e(t) = r(t) − y(t)
[11]. In this thesis the reference signal is the angle set point of the robot, 0◦ when
there is no steering command or weight placed on the robot. The output signal is
the angle of the robot and the control signal is the speed of the motors. Equation
2.1 shows the PID controller:

Z t
d
u(t) = KP e(t) + KI e(τ )d + KD e(t) (2.1)
0 dt

Where KP is the proportional coefficient of the regulator, KI is the integral coeffi-

cient and KD is the derivative coefficient.
By increasing the proportional term of the regulator the control signal for the
same level of error is increased proportionally, leading to a quicker reaction but with
more overshoot. An increase of the proportional term can also reduce the steady-
state error. If the integral term is increased the static error can be reduced, but the
stability margin of the system will decrease leading to the system becoming more
oscillatory. By increasing the derivative term of the regulator the stability margin
will increase, but the impact of measurement errors will increase [12].
The regulator will be tuned by setting KI and KD to zero and increasing KP
until steady oscillations are achieved. Then KP will be halved and KI and KD will
be tuned empirically [13].

6
2.4. WEIGHT PLACED OFF CENTER

2.4 Weight placed off center

In order to maintain balance when a weight is placed off center relative the vertical
axis of the robot, the angle set point of the PID controller will be increased or
decreased when there is no steering input from the remote controller but the robot
is moving either forward or backward. The angle set point will be increased or
decreased by a constant, φc , 250 times per second as per the frequency of the code,
until the robot is at standstill. This will lead to the robot finding its balancing point
where it is at standstill even if a weight was to be placed off center on the robot.
By increasing the value of φc the angle set point will be changed in larger steps
until the robot is at standstill, which will reduce the time it takes for the robot to
find its balance point but might induce oscillations and overshoot.
In order to calculate a theoretical settling time a mechanical model of the robot
describing the angle of the robot and the torque acting on the pendulum was derived,
seen in figure 2.2.

Figure 2.2. A model of the forces with the weight placed off center. Made in
Illustrator [8].

The system was modeled as a pendulum and a weight connected to the pendulum.
The pendulum was approximated as the base plates, threaded rods and the electrical
components except the motors [14]. To simplify the system the mass of the beam
connecting the weight to the robot was neglected and only the angles and torque
in the x- and y-axis were calculated. It was also assumed that the wheels will not
loose contact with the ground and there will be no slip between the wheels and the

7
 CHAPTER 2. THEORY

ground [15].
Equation 2.2 describing the relationship between the angle for the balance point
and the length from the pendulums center axis to the weight was then derived. See
Appendix A

mw lwc
φ = arctan ( ) (2.2)
mpend l + mw lw
Where φ is the lean angle, mw is the mass of the weight, mpend is the mass of the
pendulum, l is the length from the center of the wheel to the center of mass for
the pendulum, lw is the length from the center of the wheel to center of the weight
perpendicular to the pendulum and lwc is the length from the top of the pendulum
to the center of the weight.
Equation 2.3 could then be derived in order to calculate the theoretical settling
time from the balance point angle, code frequency and the constant φc .

φ
Tst = f (2.3)
φc
Where Tst is the theoretical settling time, φ is the lean angle and f is the frequency
of the code.

2.5 Gyroscope
A gyroscope is a sensor that measures angular velocity in degrees per second (◦ /s).
It can be used to calculate an angle by integrating the angular velocity over time.
The problem with using a gyroscope to calculate an angle is that the integration
of the angular velocity leads to the value drifting over time, not returning to zero
when the system is returned to its starting position [16].

2.6 Accelerometer
An accelerometer is a sensor that measures linear acceleration in m/s2 . It can be
used to calculate an angle by determining the position of the gravity vector. The
problem with using an accelerometer to calculate an angle is that it is sensitive to
external forces, for example vibrations [16].

8
2.7. COMPLEMENTARY FILTER

2.7 Complementary filter

In order to compensate for potential inaccuracies in readings from the accelerometer
and the drifting value from the gyroscope a complementary filter can be used. The
filter works by using the gyroscope reading on the short term since it is accurate
and not susceptible to external forces. On the long term the accelerometer value
will be used since it does not drift over time. The complimentary filter is shown in
equation 2.4.

φ = cgyro (φ + gyroData · dt) + cacc (accData) (2.4)

Where φ is the calculated angle of the robot, gyroData is the data read from the
gyroscope and accData is the data read from the accelerometer. cgyro and cacc are
constants that can be changed to tune the filter and they must add up to one.
The data from the gyroscope is integrated with the calculated angle and then
combined with the data from the accelerometer [16].

2.8 Bipolar stepper motors

A stepper motor is a type of DC-motor, meaning it runs on direct current. The
core of the motor is a permanent magnet surrounded by two coils divided into a
number of small coils that act as electromagnets when current is passed through
them. By controlling the current passed through the coils the motor can be rotated
in discrete steps [17].
One coil is energized and the magnetized core is attracted to the coil making
the core lock into place, see figure 2.3. Then the electricity is removed from that
coil and applied to the next coil, resulting in the core being attracted to the next
coil and rotating one step, see figure 2.4. By pulsating the current applied to the
coils the stepper motor rotates [17].

Figure 2.3. A model of the stepper motor. Made in Illustrator [8].

9
 CHAPTER 2. THEORY

Figure 2.4. A model of the stepper motor. Made in Illustrator [8].

The motor used rotates 1,8◦ per step and microstepping was used in order to reduce
the angle traveled per step. By applying the same current to both coils at the same
time the rotor locks into place between the two coils resulting in a half step, see
figure 2.5. To further reduce the step length the size of the current in the two coils
can be changed independently, resulting in the rotor locking into place in multiple
positions between the coils. This method was be used in order to achieve eighth
steps, resulting in a smoother motion [17].

Figure 2.5. Half stepping of the stepper motor. Made in Illustrator [8].

10
2.9. STEPPER MOTOR DRIVER

2.9 Stepper motor driver

A stepper motor driver can be used to control a stepper motor. The driver mainly
consists of a microstepping indexer and two H-bridges, one for each coil group of
the stepper motor. The H-bridges can change the current flow through the coils and
therefore change the polarity of the coils. This determines the direction the stepper
motor rotates. A figure displaying a H-bridge is shown in figure 2.6. By increasing or
decreasing the pulse frequency the motor speed will increase or decrease respectively
[18].

Figure 2.6. The function of an H-bridge. Made in Illustrator [8].

2.10 WiFi communication

WiFi is a radio based wireless network. To obtain a WiFi connection between two
devices the receiver acts as an access point that another device can connect to. The
other device acts as a station that connects to the access point. There are several
different WiFi protocols, a set of rules for how the WiFi communication works. In
this thesis the Transfer Control Protocol (TCP) will be used, it is not as fast as
other protocols but it makes sure that all the data is sent uncorrupted. If sent data
was to be missing or corrupted it will ask for the data to be re-sent [19].
WiFi latency is the time that passes between a user action at the station and
the response at the access point, a higher latency leads to a longer delay from when
the user sends a steering command to when the command is executed by the robot.
One of the principle causes of higher latency is the distance between the access
point and station [20].

11
Chapter 3

Robot construction

The following chapter lists and describes the software, the chosen pre-fabricated
components as well as the components that were constructed and manufactured.
The robot and the hand controller were both constructed with Solid Edge [6]. A
render of the CAD-model is displayed in figure 3.1. A complete list of all components
used is found in Appendix B.

Figure 3.1. A CAD rendering of the robot. Made in Solid Edge[6].

13
 CHAPTER 3. ROBOT CONSTRUCTION

3.1 Mechanical components

A handfull of different mechanical components were used to construct the demon-
strator.

3.1.1 Baseplates
Four different base plates were designed to create layers for the robot. Two base
plates were designed to clamp the two motors in place. Another base plate was
designated as a frame for mounting the Arduino, WiFi module and stepper motor
drivers. This base plate also increased stability for the robot. The top base plate
was designed to mount the IMU, a counter balance weight used to raise the center of
gravity and a mount enabling the mounting of the sliding weight used for the tests.
All base plates were Three Dimensional (3D) printed in Polylactic Acid (PLA).

3.1.2 Wheels
Wheels were designed consisting of two components, rim and tire. Weight was
intended to be low to reduce the moment of inertia and thereby improve acceleration.
The two rims were laser cut in Polymethyl Methacrylate (PMMA). This material
was chosen for the wheels because of the high yield strength [21]. The tires were
cut from an inner tube of a bicycle tire and subsequently glued to the rims. This
material provided high friction between the wheels and the maneuvering base.

14
3.2. ELECTRICAL COMPONENTS

3.1.3 Assembly of the demonstrator

Four threaded rods, 24 nuts and 24 washers were used to tighten the base plates in
a vertical stack and clamp the motors. The wheels were press fitted to the motor
shaft by a radius difference between the motor shaft and the inner hole on the rim
with a tolerance of the fit of H6.

3.2 Electrical Components

This section presents the electrical components used.

3.2.1 Arduino Uno

The microprocessor used in this thesis was the Arduino Uno running the AT-
mega328. The Arduino has 14 digital in-/output pins and six analogue pins. It
operates with a clock speed of 16 MHz and supports I2 C communication used with
the accelerometer and gyroscope module [22]. In figure 3.2 the stepper motor drivers
are to the left, the Arduino Uno is in the middle and the WiFi module is to the
right.

Figure 3.2. The electrical components used. Made in Solid Edge [6].

3.2.2 Sensor for angular data

In order to determine the angular deviation and angular velocity an IMU of model
MPU6050 was used, which is a three axis accelerometer and gyro. It communicates
with the Arduino via I2 C at 400 KHz, the range for the angular velocity was set to
±250◦ /s and the range for the accelerometer was set to ±4g [23].

15
 CHAPTER 3. ROBOT CONSTRUCTION

3.2.3 Stepper motors and driver

The stepper motors used were the Japan servo KH56JM2-851 [24], a bipolar stepper
motor with a step angle of 1,8◦ . The motor used is shown in figure 3.3. In order to
drive the stepper motors two DRV8825 drivers were used, 1/8 microstepping was
enabled on the drivers in order to reduce the step angle to 0,225◦ . See Appendix C
for the key specifications of the stepper motors and motor driver.

Figure 3.3. The stepper motor used in the robot[25].

3.2.4 Battery
A standard 12V 2200mAh Lithium Polymer (LiPo) battery was used to power the
robot. LiPo batteries have the advantage of being lightweight relative their capacity
[26].

16
3.2. ELECTRICAL COMPONENTS

3.2.5 Remote control and receiver

The remote controller consists of one ESP8266 WiFi module [27], an Analogue to
Digital Converter (ADC), an analogue joystick [28] and a 5V power bank. On the
receiving side an ESP8266 was used together with a resistor divider connected to
the battery. The remote control also has an Organic Light-Emitting Diode (OLED)
screen displaying data. A render of the remote is shown in figure 3.4. In figure 3.5
the display is shown, where SPD and ROT are the speed and rotation respectively,
refreshTime is the mean latency, refreshMax is the maximum latency, Vin is the
battery voltage, Bat is the battery voltage in %.

Figure 3.4. The remote control attached to the 5V powerbank. Made in Solid Edge
[6].

Figure 3.5. The remote display. Made in Solid Edge [6].

17
 CHAPTER 3. ROBOT CONSTRUCTION

3.3 Steering of the robot

To enable remote control of the robot two ESP8266 WiFi modules [27] were used,
one in the remote controller and one in the robot. The WiFi module in the remote
controller acts as an access point and the WiFi module in the robot acts as a station.
Steering signals are sent remotely from the remote controller to the WiFi module in
the robot. When a steering signal is received the WiFi module in the robot sends
the command to the Arduino using serial communication.
To enable forward and backward movement the angle set point of the PID-
controller is increased or decreased, resulting in the robot leaning forward or back-
ward in order to maintain the angle set point. This makes the robot move in the
leaning direction.
To enable turns the output signals to the motors are divided for the left motor
and the right motor. The robot turns by reducing the pulse frequency to one motor
and increasing the frequency to the other motor. If a turn command is sent while
the robot is at standstill the robot has a zero turn radius, rotating around its center
axis. If the turn command is sent while the robot is moving forward or backward
the robot turned with a larger turn radius.

18
3.4. SOFTWARE

3.4 Software
In this section the software developed to balance the robot and to steer the robot
is described.

3.4.1 Robot Software

The flow chart in figure 3.6 explains the logic of the Arduino code. The code was
based on Joop Brokkings code for a self-balancing robot [29] and was modified
to receive steering inputs from the ESP8826 via serial communication. The logic
follows the steps below:

• System initialized by including libraries, initializing variables and configuring

in- and outputs

• The IMU is calibrated by taking 500 measurements at standstill and correct

the gyroscope and accelerometer offset values

• Data is read from the IMU and apply a complementary filter and low pass
filter to reduce noise and gyroscope drift before the angle is calculated

• A condition checks if the the lean angle is withing 0, 5◦ > φ > −0, 5◦ in order
to reduce the effect of measurement errors and high frequency shaking

– If true the motor speed is set to zero and break from the loop
– If false continue the loop

• A condition to check if there is serial data sent from the ESP8266

– If true change the angle set point for forward/backward movement or

change the speed of the two motors for rotational movement
– If false continue the loop

• A condition to check if the angle set point is zero and the motor speed > 0 in
order to find the robots balance point even with an off centered weight

– If true increase or decrease the angle set point by φc depending on what

direction the motors are turning
– If false continue the loop

• Calculate the stepper motor pulse frequency with PID controller

• Control the stepper motors with the DRV8825 driver

• A condition to set the loop time to 40 ms, 250 Hz

19
 CHAPTER 3. ROBOT CONSTRUCTION

Figure 3.6. Figure showing flow chart for the Arduino code. Made in Lucidchart
[30].

3.4.2 Remote Software

The joystick output varies between 0-5V depending on position and the data is
converted to an integer in the ADC, then scaled to a number between 0-2. The
data is subsequently transmitted as a client request from the transmitting ESP8266
in the remote controller and the server ESP8266 in the robot receives and responds
to the request as a client. As a next step the data is sent to the Arduino via serial
communication if it differs from the previous data. The server ESP8266 is then
responding with the text with a integer that changes between battery level and
response time every loop. When this respond reaches the remote, it displays on
screen. See Appendix E for the full code.

20
Chapter 4

Results

This chapter contains the results from the tests performed with the robot. The
values of the robots mass and dimensions are shown in table 4.1 where mcart is the
mass of the wheels and motors [14], mpend is the mass of the pendulum, l is the
length to from the center of the wheels the center of gravity, lw is the length from
the center of the wheels to the center of the weight perpendicular to the pendulum.
The PID controller tuning parameters KP , KI and KD are shown in table 4.2.

Table 4.1. The masses and dimensions of the robot

mcart [Kg] 0,917
mpend [Kg] 1,501
l [m] 0,1468
lw [m] 0,36

Table 4.2. The PID controller tuning parameters.

KP 12
KI 0,5
KD 22

21
 CHAPTER 4. RESULTS

4.1 Off mass center test

In order to evaluate the robots ability to balance with a weight placed off its center
axis an experiment was conducted. In the experiment a one kilogram weight was
placed a fixed distance from the center vertical axis of the robot. The angular
data was plotted and the distance the robot travelled due to adding the weight was
measured. Figure 4.1 shows the weight mounted on the robot.

Figure 4.1. A CAD render of the weight mounted on the robot. Made in Solid Edge
[6].

4.1.1 Normal weight offset

To test how the value off the constant φc would affect the settling time and oscillation
a test was performed where the distance, lwc , that the weight was placed from the
center axis of the robot was one centimeter. The graphs from two tests with different
values of φc are shown in figure 4.2 and 4.3. The theoretical settling time Tst , real
settling time Tsr , oscillations ∆φ, distance travelled d and overshoot O are shown
in table 4.3. The test showed that a smaller φc -value of 0,0015 led to an overshoot
of 42,9%, oscillations of 0,65◦ , a real settling time of 3,75 seconds and a distance
travelled from when the weight was added of 46 centimeters. When φc was increased
to 0,0060 the oscillations increased to 1,1◦ , the overshoot increased to 100%, the
real settling time was reduced to 2,6 seconds and the distance travelled to ten
centimeters.

22
4.1. OFF MASS CENTER TEST

Figure 4.2. The graph of the response when the weight was placed one centimeter
from center and a φc value of 0,0015. Made in Mathlab [31].

Figure 4.3. The graph of the response when the weight placed one centimeter from
center and a φc value of 0,0060. Made in Mathlab [31].

Table 4.3. The angular data for one centimeter offset.

lwc [cm] φc [◦ ] ∆φ [◦ ] O [%] Tst [s] Tsr [s] d [cm]

1 0,0015 0,65 42,9 2,94 3,75 46
1 0,0060 1,1 100 0,73 2,6 10

23
 CHAPTER 4. RESULTS

4.1.2 Maximum weight offset

The requirements for the maximum weight offset test was that the oscillations must
be less than four degrees and the travelled distance must be less than 100 centime-
ters. The graph from the test with the maximum weight offset is shown in figure
4.4 and the theoretical settling time Tst , real settling time Tsr , oscillations ∆φ, dis-
tance travelled d and overshoot O are shown in in table 4.4. The test showed that
the maximum length lwc a weight can be added from the center is five centimeters.
The maximum value of φc is 0,0115. With that value the oscillations were 4◦ , the
overshoot was 65%, the real settling time was 3 seconds and the distance travelled
from when the weight was added was 95 centimeters.

Figure 4.4. The graph of the response when the weight was placed five centimeters
from center and a φc value of 0,0115. Made in Mathlab [31].

Table 4.4. The angular data for five centimeter offset.

lwc [cm] φc [◦ ] ∆φ [◦ ] O [%] Tst [s] Tsr [s] d [cm]

5 0,0115 4 65 1,9 3 95

4.2 Remote latency test

Latency was tested by measuring the time between the TCP request is sent from
the remote until the response has been returned, see figure 4.2. The response time is
then printed to the display with the 20 request average and the maximum response
time in this window. The maximum response time is 5000 milliseconds, after which
connection is considered lost. The antennas used were the standard PCB antennas.

24
4.2. REMOTE LATENCY TEST

Figure 4.5. The principle of the remote latency test. Made in Illustrator [8].

4.2.1 WiFi Latency

The average response time was between 10-20 milliseconds for most of the test.
When the range was greater than 35 meters, response spikes began to appear which
affected the average. These appeared when the robot moved to a new position.
When the robot did not move, the response time reduced to between 20-30 millisec-
onds after around 10 seconds. This spike in response time was reoccurring a few
times in the 35-50 meter span. When the robot moved to a distance of 50 meters,
the response time became greater than 5000 milliseconds and the connection timed
out.

Figure 4.6. The graph from the latency. Made in Mathlab [31].

25
 CHAPTER 4. RESULTS

4.2.2 WiFi Range

The WiFi connection was stable until around 35 meters from client to the robot. At
distances greater than 35 meters the connection became unstable and at a distance
of 50 meters from the client, the signal was disrupted and connection to the server
was lost. When the signal was disrupted the robot continued to maintain balance,
but no steering command could be sent to the robot. An illustration of signal radius
and area is shown in figure 4.7.

Figure 4.7. An illustration of the signal radius and area. Made in Photoshop [32].

Table 4.5. Table showing the WiFi range with a stable, unstable and both stable
and unstable connection.
Connection type Radius [m] Area [m2 ]
Stable 0-35 4000
Unstable 35-50 4000
Stable, Unstable 0-50 8000

26
Chapter 5

Discussion

This chapter provides a discussion of the results obtained from the tests performed
with the robot and remote controller.

5.1 The stepper motors impact

From the testing it became evident that the length of the stepper motor step had
an impact on the robot’s balancing performance. By using a smaller step via mi-
crostepping the robots movement became more fluid and led to it having a faster
settling time after a disturbance and lower oscillations. The smaller step also im-
proved the robot’s ability to turn. This is likely due to the larger step length leading
to the robot overshooting since the positioning of the stepper motors is not accurate
enough. The larger step length can also lead to faulty angular measurements due
the the mechanical noise created by the stepper motors [5].
When using DC motors for a two wheeled self-balancing robot the two motors
can have different performance, resulting in the two wheels rotating at different
speeds making the robot rotate [33]. When using stepper motors this problem did
not occur since the two motors always rotate in discrete steps, resulting in them
always rotating at the same speed regardless of performance differences.

5.2 The ability to balance with off center weights

The method of changing the PID controllers angle set point in order to balance the
robot with an off centered weight was successful. The increase of φc led to larger
oscillations and overshoot. This is likely caused by the larger value of φc resulting
in the angle set point being changed too much for a small forward speed, resulting
in the angle set point constantly being changed by a large amount when the robot
is close to standstill. Since the robot cannot be completely still this will cause
oscillations to occur.
By doubling the value of φc the traveled distance from when the weight was
placed on the robot was approximately halved, the increase of φc also reduced the

27
 CHAPTER 5. DISCUSSION

settling time slightly. The robot’s ability to maintain balance after a disturbance
was also affected by the increase of φc , when using a higher value the robot’s stability
was reduced and a small disturbance led to large oscillations. From the testing it
was determined that the highest usable value of φc was 0,0115.
When comparing the theoretical settling time,Tst , to the real settling time, Tsr ,
it was evident that the theoretical settling time was shorter than the actual. The dif-
ference between the theoretical and real settling time increased as φc was increased,
which is probably caused by the larger overshoot and oscillations when using a
larger φc . If a PID controller was used where speed of the robot was the input and
φc was the output the real settling would probably be closer to the theoretical.
Remote control of the robot was affected by the off centered weight, when the
weight was placed over four centimeters from the center axis the robot would fail to
maintain balance when a full forward command was given to the robot. This was
probably related to the motors not being able to rotate fast enough to maintain
balance.

5.3 Is it a good idea to control a robot with WiFi?

Self-balancing robot’s such as the one developed by Cassirer et al [13] have mostly
used Bluetooth as a protocol for steering the robot. Bluetooth has a typical effective
range of around ten meters [34], depending on the class of Bluetooth used. Our
thesis indicate that WiFi has an effective range of 35 meters, making it a significantly
better option for controlling the robot as it enables the operator to control the robot
at a greater distance. The response time is also typically lower when using a WiFi
connection. Our thesis indicate that WiFi has an average response time of 10-20
milliseconds, where Bluetooth has a typical response time of 200 milliseconds [35].
One benefit with using Bluetooth is that it has a lower power consumption than
WiFi [36]. Another option for controlling the robot is 4G broadband cellular network
that has a response time of 40-50 milliseconds outdoors. By May 2020, 4G cellular
covers around 80 percent of Sweden [37]. To control the range with 4G cellular
would give the robot the ability to be controlled from a remote distance, but the
hardware is expensive and requires a subscription with the cellular operator. The
cellular signal is easily blocked indoors. WiFi is a good choice when a low response
time is needed and the operator is at line of sight of the robot.

28
Chapter 6

Conclusion

How far off-center can a one kilogram weight be added while maintaining balance of
the robot?

A one kilogram weight can be placed five centimeters from the center axis of the
robot while maintaining balance.

What is the response time when controlling the robot with a WiFi connection?

For distances under 35 meters the average response time was 10-20 milliseconds,
when the range was over 35 meters spikes increased the average response while
the robot was moving, when the robot did not move the response time was 20-30
milliseconds. At a distance of 50 meters the response time became greater than
5000 milliseconds.

How do WiFi and Bluetooth steering compare?

The main difference between WiFi and Bluetooth is that WiFi typically have a
larger effective range of around 35 meters while Bluetooth has a effective range of
around ten meters. The response time is typically shorter when using WiFi.

29
Chapter 7

Recommendations and future work

7.1 Recommendations
A external antenna is recommended for the robot and the controller, since it would
give the robot a longer and more stable range. By adding a motor operated sliding
counterweight it could be used to balance the robot when an off center weight is
added. This would enable the robot to stand vertically even when loaded. A sun-
shield for the display is also recommended, since the screen was hard to read in
daylight conditions.

7.2 Future work

The value of φc could be regulated via a PID controller where the input would be
the speed of the motors, this would reduce the oscillations and overshoot while still
being able to have a fast response time to a weight being placed on the robot.
A cascaded PID controller could be used that has the lean angle and the position
of the robot as inputs, to calculate the position either encoders could be used or the
steps of the stepper motors could be used. This would give the robot the ability to
return to its starting position after a weight has been placed on top of the robot.
It would also make autonomous control of the robot possible.

31
Bibliography

[1] Mordor intelligence. ”AUTONOMOUS DELIVERY ROBOTS MARKET”. Date

accessed: 2020-03-20. [Online]. Available: https://www.mordorintelligence.
com/industry-reports/autonomous-delivery-robots-market

[2] ZDNet. ”Amazon delivery robots are officially on the streets of California”. Date
accessed: 2020-03-20. [Online]. Available: https://www.zdnet.com/article/
amazon-delivery-robots-are-officially-on-the-streets-of-california/

[3] Daniel Jones and Karl Stol. ”Modelling and Stability Control of Two-
Wheeled Robots in Low-Traction Environments”, Autralasian conference on
robotics and automation, 2010. Date accessed: 2020-02-10. [Online]. Available:
https://www.researchgate.net/publication/267941440_Modelling_
and_Stability_Control_of_Two-Wheeled_Robots_in_Low-Traction_
Environments

[4] Loram, I, 2002. ”Human balancing of an inverted pendulum: position control

by small, ballistic-like, throw and catch movements”, Journal of Physiology.
[Online]. DOI: https://doi.org/10.1113/jphysiol.2001.013077

[5] Bruno Bosco, André Enllnefjärd, Victor Ellqvist Nordenmark, Alexander Hem-
berg, Henrik Olsson, Jonas Tegelberg ”Utveckling av en självbalanserande
tvåhjulig robot”, KTH, Tech Rep., 2012. Date accessed: 2020-01-15. [On-
line]. Available: http://www.diva-portal.org/smash/get/diva2:550532/
FULLTEXT01.pdf

[6] Solid Edge ST10, Siemens. [Software]. Available: https://solidedge.

siemens.com/en/

[7] Rui Zhang, Gang Xiong, Changjian Cheng, Xiuqin Shang, Yonghong Ma,
Zichen Lu. ”Control system design for two-wheel self-balanced robot based on
the stepper motor”, IEEE, 2008. Date accessed: 2020-01-25. [Online]. DOI:
https://doi.org/10.1109/SOLI.2013.6611417

[8] Illustrator, Adobe. [Software]. Available: https://www.adobe.com/products/

illustrator.html

[9] Torkel Glad and Lennart Ljung, Reglerteknik. Poland: Dimograf, 2014, pp. 9-14.

33
 BIBLIOGRAPHY

[10] Torkel Glad and Lennart Ljung, Reglerteknik. Poland: Dimograf, 2014, pp.
63-64.

[11] Torkel Glad and Lennart Ljung, Reglerteknik. Poland: Dimograf, 2014, p. 20.

[12] Control tutorials for MATLAB and SIMULINK. ”Introduction: PID Controller
Design”. Date accessed: 2020-02-10. [Online]. Available: http://ctms.engin.
umich.edu/CTMS/index.php?example=Introduction&section=ControlPID

[13] Albin Cassirer, Viktor Eriksson, Mikael Gnospelius, Anna Kramarz and Jo-
han Landerholm, ”Timing Grey: En självbalanserande robot med ultraljud och
egenutvecklad hastighetsmätare”, KTH, Tech Rep., 2012. Date accessed: 2020-
01-25. [Online]. Available: http://www.diva-portal.org/smash/get/diva2:
550518/FULLTEXT01.pdf

[14] Hanna Hellman and Henrik Sunnerman, ”Two-Wheeled Self-Balancing Robot”,

KTH, Tech Rep., 2015. Date accessed: 2020-01-25. [Online]. Available: http:
//www.diva-portal.org/smash/get/diva2:916184/FULLTEXT01.pdf

[15] F. Grasser, A. D’Arrigo, S. Colombi and A.C. Rufer, ”JOE: a mobile, inverted
pendulum”, IEEE, 2002. Date accessed: 2020-02-15. [Online]. DOI: https://
doi.org/10.1109/41.982254

[16] Pieter-Jan. ”Reading a IMU Without Kalman: The Complementary Filter”.

Date accessed: 2020-02-10. [Online]. Available: https://www.pieter-jan.com/
node/11

[17] DroneBot Workshop. ”Stepper Motors with Arduino - Getting Started with
Stepper Motors”. Date accessed: 2020-01-20. [Online]. Available: https://
dronebotworkshop.com/stepper-motors-with-arduino/

[18] Texas instruments. ”DRV8825 stepper motor controller IC”. Date ac-
cessed: 2020-01-05. [Online]. Available: https://www.electrokit.com/
produkt/stegmotordrivare-drv8825/

[19] CircuitDigest. ”Getting Started with ESP8266 WiFi Transciever (Part 1)”.
Date accessed: 2020-04-10. [Online]. Available: https://circuitdigest.com/
microcontroller-projects/getting-started-with-esp8266-modul

[20] Cloudflare. ”What Is Latency? — How to Fix Latency”. Date accessed:

2020-04-05. [Online]. Available: https://www.cloudflare.com/learning/
performance/glossary/what-is-latency/

[21] MatWeb. ”Material data for PMMA”. Date accessed: 2020-03-20. [On-
line]. Available. http://www.matweb.com/search/datasheet.aspx?bassnum=
O1303&ckck=1

34
BIBLIOGRAPHY

[22] Arduino, Arduino Uno Rev3. Date accessed: 2020-01-15. [Online]. Available:
https://store.arduino.cc/arduino-uno-rev3

[23] MPU-6050. Date accessed: 2020-01-15. [Online].

Available: https://www.electrokit.com/produkt/
mpu-6050-accelerometer-3-axel-monterad-pa-kort/

[24] Japan servo KH56JM2-851. Date accessed: 2020-04-20. [Online]. Available:

https://www.datasheetarchive.com/KH56JM2-851-datasheet.html

[25] Nema23 stepper motor. Date accessed: 2020-02-15. [Online]. Available: https:
//joy-it.net/en/products/NEMA23-02

[26] Turnigy 2200mAh 3S 40C Lipo Pack, Turnigy. Date ac-

cessed: 2020-02-10. [Online]. Available: https://hobbyking.
com/en_us/turnigy-2200mah-3s-40c-lipo-pack.html?queryID=
55d6b2436f898e0523467dbbbc686552&objectID=18304&indexName=hbk_
live_magento_en_us_products

[27] ESP8266, Espressif. Date accesed: 2020-01-15. [Online]. Available: https://

www.lawicel-shop.se/nodemcu-v3-with-esp-12e-ch340

[28] Joystick till Ps2 handkontroll. Date accessed: 2020-02-

20. [Online]. Available: https://pchbutik.se/kretskort/
138-joystick-till-ps2-handkontroll-passar-arduino.html?search_
query=joystick&results=5

[29] Brokking. ”Your Arduino Balancing Robot”. Date accessed: 2020-01-20. [On-
line]. Available: http://www.brokking.net/yabr_main.html

[30] Lucidchart. [Software]. Available: https://www.lucidchart.com/

[31] Matlab, MathWorks. [Software]. Available: https://www.mathworks.com/

products/matlab.html

[32] Photoshop, Adobe. [Software]. Available: https://www.adobe.com/

products/photoshop.html

[33] Dan Nguyen and Kayan Phuong, ”SB-bot”, KTH, Tech Rep., 2016. Date ac-
cessed: 2020-01-25. [Online]. Available: http://www.diva-portal.org/smash/
get/diva2:957115/FULLTEXT01.pdf

[34] Bluair. ”Bluetooth Range: 100m, 1km or 10km?”. Date accessed: 2020-05-08.
[Online]. Available: http://www.bluair.pl/bluetooth-range

[35] R. A. Gheorghiu, V. Iordache, M. Minea and A. C. Cormos, ”Bluetooth latency

analysis for vehicular communications in a Wi-Fi noisy environment,” 2017 40th
International Conference on Telecommunications and Signal Processing (TSP),

35
 BIBLIOGRAPHY

Barcelona, 2017, pp. 148-151. Date accessed: 2020-05-08. [Online]. DOI: https:
//doi.org/10.1109/TSP.2017.8075956

[36] Lifewire. ”Bluetooth vs. Wi-Fi: What’s the Difference?”. Date ac-
cessed: 2020-05-08. [Online]. Available: https://www.lifewire.com/
bluetooth-vs-wi-fi-4088218

[37] Telia. ”Täckningskartor”. Date accessed: 2020-05-08. [Online]. Available:

https://www.telia.se/privat/support/tackningskartor

[38] Fritzing, Interaction Design Lab Potsdam. [Software]. Available: https://

fritzing.org/

36
Appendix A

Weight off center model

The system was modelled as an pendulum and a weight attached to the pendulum
to simplify the equations. The model is shown in figure A.1.

Figure A.1. A model of the forces with the weight placed off center. Made in
Illustrator [8].

A:1
 APPENDIX A. WEIGHT OFF CENTER MODEL

A.1 Pendulum
The length along the horizontal axis from the origin to the center of mass for the
pendulum was derived:
xcgpend = l sin φ (A.1)
Then the torque acting on the origin due to the pendulums mass was derived:

Tpend = mpend gl sin φ (A.2)

A.2 Weight
The length along the horizontal axis from the origin to the center of the weight was
derived:
xcgweight = lw sin φ − lwc cos φ (A.3)
Then the torque acting on the origin due to the weights mass was derived:

Tw = mw g(lw sin φ − lwc cos φ) (A.4)

A.3 Combining the equations

A.2 and A.4 was then added since there is equilibrium of torque at the balance
point:
0 = mpend gl sin φ + mw g(lw sin φ − lwc cos φ) (A.5)
A.5 was then solved for the angle φ:
mw lwc
φ = arctan ( ) (A.6)
mpend l + mw lw

A:2
Appendix B

Component table

B:1
 APPENDIX B. COMPONENT TABLE

QTY Component Specification Weight (g) Price (SEK)

Hardware
1 Arduino Uno Rev3 40 228
2 Steppermotors Indonesia 859 500
2 Motor drivers DRV8825 3 158
1 Balance sensor MPU-6050 6 49
1 ESP8266 ESP12-E 30 49
1 Battery 12V 185 200

Bought mechanical components

4 Threaded rods M6, length 250mm 165 27
24 Nuts M6 49 24
24 Washers M6 23 24

Manufactured mechanical components

1 Counterweight 35x225x12mm 719
1 Baseplate, bottom 81.5x170x10mm 53
1 Baseplate, w/ cutout 81.5x170x10mm 47
1 Component Baseplate 81.5x170x5mm 34
1 Counterweight Baseplate 81.5x170x20mm 34
1 Battery holder 28x112x62mm 39
2 PMMA Wheel rims 100x25mm 50
2 Wheel holder 10x20x20mm 4
2 Wheel decks 106x25mm 1

Total 1 657 1 239

Table B.1. List of all components, quantity, weight and price

B:2
Appendix C

Stepper motor and driver specifications

Table C.1. The key specifications for Japan servo KH56JM2 851 stepper motor.

Number of phases 2
Step angle [◦ /step] 1,8
Voltage [V] 3,51
Current [A/phase] 1,3
Inductance [mH/phase] 5,6
Holding torque [kgf·cm] 5,0
Max speed at 12 V [rpm] 253

Table C.2. The key specifications for DRV8825 stepper motor driver.

Operating voltage [V] 8,2 - 45

Max drive current [A] 2,5
Max microstepping 1/32

C:1
Appendix D

Circuit diagram

Figure D.1. The circuit diagram for the robot. Made in Fritzing [38].

D:1
 APPENDIX D. CIRCUIT DIAGRAM

Figure D.2. The circuit diagram for the remote. Made in Fritzing.

D:2
 Appendix E

Code

E.1 Robot code

This section contains the code on the Arduino in the robot to maintain balance and
receive steering commands.
1 /*
2 Arduino Two Wheeled Self - balancing Robot
3 Date : 2020 -05 -20
4 Written by : Fredrik Ihrfelt and William Marin
5 Examinor : Nihad Subasic
6 TRITA - nr : 2020:36
7 Course : MF133X
8
9 Bachelor ’s thesis at KTH in mechatronics
10
11 The code :
12 - Reads data from the MPU6050 and calculates the angle
13 - Calculates the motor speed based on the angle error with a PID
controller
14 - Reads the serial port and changes the angular set point or
rotational speed
15 - Changes the angular set point if the robot is moving when no
steering command is given
16
17 The code runs at 250 Hz on a Arduino UNO R3 with a clock speed of
16 MHz
18 */
19
20 // --- INCLUDED LIBRARIES - - -//
21 # include < Wire .h >
22
23 // --- DECLARING I2C ADDRESS OF MPU6050 - - -//
24 int gyro_address = 0 x68 ;
25 int a cc_c alib ratio n_va lue = 480;
26
27 // --- SETTING PID PARAMETERS - - -//
28 float pid_p_gain = 12;

E:1
 APPENDIX E. CODE

29 float pid_i_gain = 0.5;

30 float pid_d_gain = 22;
31
32 // --- DECLARING GLOBAL VARIABLES - - -//
33 byte start ;
34
35 int left_motor , throttle_left_motor , throttle_counter_left_motor ,
throttle_left_motor_memory ;
36 int right_motor , throttle_right_motor , throttle_counter_right_motor ,
throttle_right_motor_memory ;
37 int gyro_pitch_data_raw , gyro_yaw_data_raw , ac ce ler om et er_ da ta _r aw ;
38
39 long gyro_yaw_calibration_value , g y r o _ p i t c h _ c a l i b r a t i o n _ v a l u e ;
40
41 unsigned long loop_timer ;
42
43 // Variables for the angle and PID controller
44 float angle_gyro , angle_acc , angle , s e l f_ b a l a n c e _ p i d _ s e t p o i n t ;
45 float pid_error_temp , pid_i_mem , pid_setpoint , gyro_input , pid_output
, pid_last_d_error ;
46 float pid_output_left , pid_output_right ;
47
48 // Variables for steering
49 float spd = 1; // Stores the desired speed value
50 float rotation = 1; // Stores the rotation value
51 float turning_speed = 30;
52 float max_target_speed = 200;
53 float desired_rotation ;
54 float desired_speed ;
55
56 // Declaring constants for serial port reading
57 const byte buffSize = 40;
58 char inputBuffer [ buffSize ];
59 const char startMarker = ’ < ’;
60 const char endMarker = ’ > ’;
61 byte bytesRecvd = 0;
62 boolean readInProgress = false ;
63 boolean newDataFromEsp = false ;
64
65 char messageFromEsp [ buffSize ] = {0};
66
67
68 void setup () {
69 // Starting the serial port att 115200 kbps
70 Serial . begin (115200) ;
71
72 // Starting the I2C bus
73 Wire . begin () ;
74
75 // Setting the I2C clock speed to 400 kHz
76 TWBR = 12;
77
78
79 // Creating variable pulse for stepper motor control ,

E:2
 E.1. ROBOT CODE

TIMER2_COMPA_vect
80 TCCR2A = 0;
81 TCCR2B = 0;
82 TIMSK2 |= (1 << OCIE2A ) ;
83 TCCR2B |= (1 << CS21 ) ;
84 OCR2A = 39;
85 TCCR2A |= (1 << WGM21 ) ;
86
87 // Starting the MPU6050
88 Wire . beginTransmission ( gyro_address ) ;
89 Wire . write (0 x6B ) ;
90 Wire . write (0 x00 ) ;
91 Wire . endTransmission () ;
92
93 // Setting the scale of the gyro to +/ - 250 degrees per second
94 Wire . beginTransmission ( gyro_address ) ;
95 Wire . write (0 x1B ) ;
96 Wire . write (0 x00 ) ;
97 Wire . endTransmission () ;
98
99 // Setting the scale of the accelerometer to +/ - 4 g .
100 Wire . beginTransmission ( gyro_address ) ;
101 Wire . write (0 x1C ) ;
102 Wire . write (0 x08 ) ;
103 Wire . endTransmission () ;
104
105 // Setting a Low Pass Filter on MPU6050 to 43 Hz
106 Wire . beginTransmission ( gyro_address ) ;
107 Wire . write (0 x1A ) ;
108 Wire . write (0 x03 ) ;
109 Wire . endTransmission () ;
110
111 // Defining outputs
112 pinMode (2 , OUTPUT ) ;
113 pinMode (3 , OUTPUT ) ;
114 pinMode (4 , OUTPUT ) ;
115 pinMode (5 , OUTPUT ) ;
116 pinMode (13 , OUTPUT ) ;
117
118 // Calibrating the MPU6050 by reading the gyro offset 500 times and
calculating the mean value
119 for ( receive_counter = 0; receive_counter < 500; receive_counter ++) {
120 Wire . beginTransmission ( gyro_address ) ;
121 Wire . write (0 x43 ) ;
122 Wire . endTransmission () ;
123 Wire . requestFrom ( gyro_address , 4) ;
124 g y r o _ y a w _ c a l i b r a t i o n _ v a l u e += Wire . read () < <8| Wire . read () ;
125 g y r o _ p i t c h _ c a l i b r a t i o n _ v a l u e += Wire . read () < <8| Wire . read () ;
126 delayMicroseconds (3700) ;
127 }
128 g y r o _ p i t c h _ c a l i b r a t i o n _ v a l u e /= 500;
129 g y r o _ y a w _ c a l i b r a t i o n _ v a l u e /= 500;
130
131 // Creating loop timer to achieve 250 Hz frequency

E:3
 APPENDIX E. CODE

132 loop_timer = micros () + 4000;

133
134 }
135
136 // --- MAIN LOOP - - -//
137 void loop () {
138
139 // Reading data from the serial port
140 getDataFromEsp () ;
141
142 // Calculate forvard speed from serial reading
143 if ( spd > 1) {
144 desired_speed = max_target_speed *( spd - 1) ;
145 }
146
147 // Calculate backward speed from serial reading
148 if ( spd < 1) {
149 desired_speed = max_target_speed *(1 - spd ) ;
150 }
151
152 // Calculate clockwise rotation speed from serial reading
153 if ( rotation > 1) {
154 desired_rotation = turning_speed *( rotation - 1) ;
155 }
156
157 // Calculate counter clockwise rotation speed from serial reading
158 if ( rotation < 1) {
159 desired_rotation = turning_speed *(1 - rotation ) ;
160 }
161
162 // --- READ DATA FROM ACCELEROMETER - - -//
163 Wire . beginTransmission ( gyro_address ) ;
164 Wire . write (0 x3F ) ;
165 Wire . endTransmission () ;
166 Wire . requestFrom ( gyro_address , 2) ;
167 a c ce le rom et er _d ata _r aw = Wire . read () < <8| Wire . read () ;
168 a c ce le rom et er _d ata _r aw += acc _cali brat ion_ value ;
169 if ( a cc el ero me te r_ dat a_ ra w > 8200) acc el er ome te r_ da ta_ ra w = 8200;
170 if ( a cc el ero me te r_ dat a_ ra w < -8200) acc el er ome te r_ da ta_ ra w = -8200;
171
172 // Calculate angle from the accelerometer data
173 angle_acc = asin (( float ) a cce le ro met er _d at a_r aw /8200.0) * 57.296;
174
175 // Set the angle of the gyro to the angle of the accelerometer if
the robot is vertical
176 if ( start == 0 && angle_acc > -0.5&& angle_acc < 0.5) {
177 angle_gyro = angle_acc ;
178 start = 1;
179 }
180
181 // / - - - READING DATA FROM THE GYRO - - -///
182 Wire . beginTransmission ( gyro_address ) ;
183 Wire . write (0 x43 ) ;
184 Wire . endTransmission () ;

E:4
 E.1. ROBOT CODE

185 Wire . requestFrom ( gyro_address , 4) ;

186 gyro_yaw_data_raw = Wire . read () < <8| Wire . read () ;
187 g yro_pitch_data_raw = Wire . read () < <8| Wire . read () ;
188
189 // Compensate the angle data with the calibration value
190 g yro_pitch_data_raw -= g y r o _ p i t c h _ c a l i b r a t i o n _ v a l u e ;
191 angle_gyro += gyro_pitch_data_raw * 0.000031;
192
193 gyro_yaw_data_raw -= g y r o _ y a w _ c a l i b r a t i o n _ v a l u e ;
194
195 // Corecting gyro drift with complementary filter
196 angle_gyro = angle_gyro * 0.9996 + angle_acc * 0.0004;
197
198 // Print the angle of the gyro for experiments
199 Serial . println ( angle_gyro ) ;
200
201 // --- PID CONTROLLER - - -//
202 // Calculating the angular error
203 pid_error_temp = angle_gyro - s e l f _ b a l a n c e _ p i d _s e t p o i n t -
pid_setpoint ;
204 if ( pid_output > 10 || pid_output < -10) pid_error_temp += pid_output
* 0.015 ;
205
206 // Calculating the value on the I - part and add it to i_mem
207 pid_i_mem += pid_i_gain * pid_error_temp ;
208
209 // Limit the maximum I - part value
210 if ( pid_i_mem > 400) pid_i_mem = 400;
211 else if ( pid_i_mem < -400) pid_i_mem = -400;
212
213 // Calculating PID controller output
214 pid_output = pid_p_gain * pid_error_temp + pid_i_mem + pid_d_gain *
( pid_error_temp - pid_last_d_error ) ;
215 if ( pid_output > 400) pid_output = 400;
216 else if ( pid_output < -400) pid_output = -400;
217
218 // Storing the error for the next loop
219 pid_last_d_error = pid_error_temp ;
220
221 // Creating a dead band for small PID outputs
222 if ( pid_output < 5 && pid_output > -5) pid_output = 0;
223
224 // --- CONTROLLER OUTPUTS - - -//
225
226 // Copying the PID output to the right and left motors
227 pid_output_left = pid_output ;
228 pid_output_right = pid_output ;
229
230 // If a right rotation command is given , increase speed on the left
motor and decrease speed on the right motor
231 if ( rotation < 1) {
232 pid_output_left += desired_rotation ;
233 pid_output_right -= desired_rotation ;
234 }

E:5
 APPENDIX E. CODE

235
236 // If a left rotation command is given , increase speed on the right
motor and decrease speed on the left motor
237 if ( rotation > 1) {
238 pid_output_left -= desired_rotation ;
239 pid_output_right += desired_rotation ;
240 }
241
242 // If a forward command is given , increase the PID angle set point
243 if ( spd > 1) {
244 if ( pid_setpoint > -2.5) pid_setpoint -= 0.1;
245 if ( pid_output > desired_speed * -1) pid_setpoint -= 0.005;
246 }
247
248 // If a backward command is given , decrease the PID angle set point
249 if ( spd < 1) {
250 if ( pid_setpoint < 2.5) pid_setpoint += 0.1;
251 if ( pid_output < desired_speed ) pid_setpoint += 0.005;
252 }
253
254 // If no steering command is given , set the PID angle set point to
zero
255 if ( spd == 1 && rotation == 1) {
256 if ( pid_setpoint > 0.5) pid_setpoint -=0.05;
257 else if ( pid_setpoint < -0.5) pid_setpoint +=0.05;
258 else pid_setpoint = 0;
259 }
260
261 // Change the PID angle setpoint to compensate for off centered
weight
262 if ( pid_setpoint == 0) {
263 if ( pid_output < 0) s e l f _ b a l a n c e _ pi d _ s e t p o i n t += 0.0115;
264 if ( pid_output > 0) s e l f _ b a l a n c e _ pi d _ s e t p o i n t -= 0.0115;
265 }
266
267
268 // --- CALCULATING THE STEPPER MOTOR PULSE - - -//
269 // Linearize the stepper motors non - linear behavior
270 if ( pid_output_left > 0) pid_output_left = 405 - (1/( pid_output_left
+ 9) ) * 5500;
271 else if ( pid_output_left < 0) pid_output_left = -405 - (1/(
pid_output_left - 9) ) * 5500;
272
273 if ( pid_output_right > 0) pid_output_right = 405 - (1/(
pid_output_right + 9) ) * 5500;
274 else if ( pid_output_right < 0) pid_output_right = -405 - (1/(
pid_output_right - 9) ) * 5500;
275
276 // Calculate the pulse time for the stepper motors
277 if ( pid_output_left > 0) left_motor = 400 - pid_output_left ;
278 else if ( pid_output_left < 0) left_motor = -400 - pid_output_left ;
279 else left_motor = 0;
280
281 if ( pid_output_right > 0) right_motor = 400 - pid_output_right ;

E:6
 E.1. ROBOT CODE

282 else if ( pid_output_right < 0) right_motor = -400 - pid_output_right ;

283 else right_motor = 0;
284
285 // Copy the pulse time to the throttle variables so the interrupt
subroutine can use them
286 t hrottle_left_motor = left_motor ;
287 t h rottle_right_motor = right_motor ;
288
289
290 // Delay loop if the time is under 40 ms (250 Hz )
291 while ( loop_timer > micros () ) ;
292 loop_timer += 4000;
293 }
294
295 // --- INTERRUPT ROUTINE FOR TIMER2_COMPA_vect - - -//
296 ISR ( TIMER2_COMPA_vect ) {
297 // Left motor pulse calculations
298 t h r o t t l e _ c o u n t e r _ l e f t _ m o t o r ++;
299 if ( t h r o t t l e _ c o u n t e r _ l e f t _ m o t o r > t h r o t t l e _ l e f t _ m o t o r _ m e m o r y ) {
300 t h r o t t l e _ c o u n t e r _ l e f t _ m o t o r = 0;
301 t h r o t t l e _ l e f t _ m o t o r _ m e m o r y = throttle_left_motor ;
302 if ( t h r o t t l e _ l e f t _ m o t o r _ m e m o r y < 0) {
303 PORTD &= 0 b11110111 ;
304 t h r o t t l e _ l e f t _ m o t o r _ m e m o r y *= -1;
305 }
306 else PORTD |= 0 b00001000 ;
307 }
308 else if ( t h r o t t l e _ c o u n t e r _ l e f t _ m o t o r == 1) PORTD |= 0 b00000100 ;
309 else if ( t h r o t t l e _ c o u n t e r _ l e f t _ m o t o r == 2) PORTD &= 0 b11111011 ;
310
311 // right motor pulse calculations
312 t h r o t t l e _ c o u n t e r _ r i g h t _ m o t o r ++;
313 if ( t h r o t t l e _ c o u n t e r _ r i g h t _ m o t o r > t h r o t t l e _ r i g h t _ m o t o r _ m e m o r y ) {
314 t h r o t t l e _ c o u n t e r _ r i g h t _ m o t o r = 0;
315 t h r o t t l e _ r i g h t _ m o t o r _ m e m o r y = throttle_right_motor ;
316 if ( t h r o t t l e _ r i g h t _ m o t o r _ m e m o r y < 0) {
317 PORTD |= 0 b00100000 ;
318 t h r o t t l e _ r i g h t _ m o t o r _ m e m o r y *= -1;
319 }
320 else PORTD &= 0 b11011111 ;
321 }
322 else if ( t h r o t t l e _ c o u n t e r _ r i g h t _ m o t o r == 1) PORTD |= 0 b00010000 ;
323 else if ( t h r o t t l e _ c o u n t e r _ r i g h t _ m o t o r == 2) PORTD &= 0 b11101111 ;
324 }
325
326
327 // --- FUNTION TO READ SERIAL DATA FROM ESP8266 - - -//
328 void getDataFromEsp () {
329
330 // Only read serial data if available
331 if ( Serial . available () > 0) {
332
333 // Store read data in variable x
334 char x = Serial . read () ;

E:7
 APPENDIX E. CODE

335
336 // Stop reading when ’>’ is sent
337 if ( x == endMarker ) {
338 readInProgress = false ;
339 newDataFromEsp = true ;
340 inputBuffer [ bytesRecvd ] = 0;
341 parseData () ;
342 }
343
344 // Read the string between ’<’ and ’>’
345 if ( readInProgress ) {
346 inputBuffer [ bytesRecvd ] = x ;
347 bytesRecvd ++;
348 if ( bytesRecvd == buffSize ) {
349 bytesRecvd = buffSize - 1;
350 }
351 }
352
353 // Start reading when ’<’ is sent
354 if ( x == startMarker ) {
355 bytesRecvd = 0;
356 readInProgress = true ;
357 }
358 }
359 }
360
361 // --- FUNCTION TO SPLIT THE READ SERIAL DATA - - -//
362 void parseData ()
363
364 // Using strtoIndx as an index
365 char * strtokIndx ;
366
367 // Read the first part of the string and store as the speed command
368 strtokIndx = strtok ( inputBuffer , " ," ) ;
369 spd = atof ( strtokIndx ) ;
370
371 // Read the second part of the string and store as the rotation
command
372 strtokIndx = strtok ( NULL , " ," ) ;
373 rotation = atof ( strtokIndx ) ;
374
375 }

E:8
 E.2. REMOTE CODE

E.2 Remote code

E.2.1 Transmitter
This section contains the code on the ESP8266 in the remote controller.
1 /*
2 Remote Transmitter Code
3 Date : 2020 -05 -20
4 Written by : Fredrik Ihrfelt and William Marin
5 Examinor : Nihad Subasic
6 TRITA - nr : 2020:36
7 Course : MF133X
8
9 Bachelor ’s thesis at KTH in mechatronics
10
11 The code :
12 - Reads joystick positiom
13 - Sends data to reciever ESP8266 with TCP
14 - Register reciever ESP8266 response
15 - Prints data to 0.96 oled display
16 */
17 # include < ESP8266WiFi .h >
18 # include < WiFiClient .h >
19 # include < Wire .h >
20 # include < Adafruit_ADS1015 .h >
21 # include < Adafruit_GFX .h >
22 # include < Adafruit_SSD1306 .h >
23 # include < SPI .h >
24
25 const char * ssid = " robotwifi " ;
26 const char * password = " kthkex20 " ;
27
28 int k = 0;
29 int j = 0;
30 int i = 0;
31 int q = 0;
32 int state = 1;
33 int displayTime ;
34
35 float q2 = 0;
36 float a0 ;
37 float a1 ;
38 float a2 ;
39 float refreshTime ;
40 float refreshMean = 0;
41 float refreshMeanDisp ;
42 float refreshMax = 0;
43 float refreshMaxDisp = 0;
44 float minbat = 10.5;
45 float maxbat = 11.25;
46 float batper = 0;
47 float spd = 0;
48 float rot = 0;

E:9
 APPENDIX E. CODE

49 float spdp = 0;
50 float rotp = 0;
51 float num = 0;
52 float voltage = 0;
53 float voltage2 = 0;
54 float RssI = 0;
55
56 # define SCREEN_WIDTH 128 // OLED display width , in pixels
57 # define SCREEN_HEIGHT 64 // OLED display height , in pixels
58 // Declaration for SSD1306 display connected using software SPI
59 # define OLED_MOSI 12
60 # define OLED_CLK 14
61 # define OLED_DC 0
62 # define OLED_CS 2
63 # define OLED_RESET 13
64
65 Adafruit_SSD1306 display ( SCREEN_WIDTH , SCREEN_HEIGHT , OLED_MOSI ,
66 OLED_CLK , OLED_DC , OLED_RESET , OLED_CS ) ;
67 Adafruit_ADS1115 ads (0 x48 ) ;
68
69 void setup () {
70 Serial . begin (115200) ;
71 if (! display . begin ( SSD1306_SWITCHCAPVCC ) ) {
72 Serial . println ( F ( " SSD1306 allocation failed " ) ) ;
73 for (;;) ; // Don ’t proceed , loop forever
74 }
75
76 ads . begin () ; // Starts ADC
77
78 // Explicitly set the ESP8266 to be a WiFi - client
79 WiFi . mode ( WIFI_STA ) ;
80 WiFi . begin ( ssid , password ) ;
81
82 // Loops until connection with server is established
83 while ( WiFi . status () != WL_CONNECTED ) {
84 int16_t i ;
85 display . clearDisplay () ;
86 display . setTextSize (1) ; // Normal 1:1 pixel scale
87 display . setTextColor ( SSD1306_WHITE ) ; // Draw white text
88 display . setCursor (10 , 10) ; // Start at top - left corner
89 display . print ( F ( " Connecting " ) ) ; // Prints status to screen
90 display . display () ;
91 delay (600) ;
92 display . print ( F ( " . " ) ) ;
93 display . display () ;
94 delay (300) ;
95 display . print ( F ( " . " ) ) ;
96 display . display () ;
97 delay (300) ;
98 }
99 }
100
101 void loop () {
102 int16_t adc0 ; // Sixteen bit integer as a result

E:10
 E.2. REMOTE CODE

103 int16_t adc1 ;

104 int16_t adc2 ;
105 adc0 = ads . readADC_SingleEnded (0) ; // Reads data from ADC
106 adc1 = ads . readADC_SingleEnded (1) ;
107 adc2 = ads . readADC_SingleEnded (2) ;
108 a0 = adc0 ; // Converts from int to float
109 a1 = adc1 ;
110 a2 = adc2 ;
111 spd = 200 - round (200 * a1 / a2 ) ; // Scale data from sixteen bit to
0 -200 resulotion
112 rot = round (200 * a0 / a2 ) ;
113
114 // Makes sure that value 100 is sent when joystick idle
115 if ( spd <= 110) {
116 if ( spd >= 90) {
117 spd = 100;
118 }
119 }
120 if ( rot <= 110) {
121 if ( rot >= 90) {
122 rot = 100;
123 }
124 }
125
126 // Initize wifi characteristics
127 WiFiClient client ;
128 const char * host = " 192.168.4.1 " ;
129 const int httpPort = 80;
130
131 // In case of connection disruption : Prints status and resets the
ESP
132 if (! client . connect ( host , httpPort ) ) {
133 display . clearDisplay () ;
134 display . setCursor (10 , 10) ;
135 display . println ( F ( " connection failed , " ) ) ;
136 display . setCursor (10 , 20) ;
137 display . println ( F ( " resetting " ) ) ;
138 display . display () ;
139 delay (4000) ;
140 ESP . reset () ;
141 return ;
142 }
143
144 // Creates a URI for the request
145 String url = " / data / " ;
146 url += " ? sensor_reading = " ;
147 url += spd ;
148 url += " / " ;
149 url += " & sensor_reading2 = " ;
150 url += rot ;
151 url += " / " ;
152 url += " & sensor_reading3 = " ;
153 url += state ;
154

E:11
 APPENDIX E. CODE

155 refreshTime = millis () ; // Saves the current millis () value

156
157 // This will send the request to the server
158 client . print ( String ( " GET " ) + url + " HTTP /1.1\ r \ n " +
159 " Host : " + host + " \ r \ n " +
160 " Connection : close \ r \ n \ r \ n " ) ;
161 unsigned long timeout = millis () ;
162
163 // Loops until connection is established
164 while (! client . available () ) {
165 // If there is no contact with reciever after 5000 ms or more ,
connection is lost . Prints status to screen .
166 if ( millis () - timeout > 5000) {
167 display . clearDisplay () ;
168 display . print ( F ( " Connection timeout " ) ) ;
169 display . display () ;
170 client . stop () ;
171 return ;
172 }
173 }
174
175 refreshTime = millis () - refreshTime ; // Saves the time for contact
with reciever
176 String line = client . readStringUntil ( ’\ r ’) ; // Reads first line of
server response
177
178 // removes 9 first characters of string
179 while ( k <= 8) {
180 line [ k ] = 0;
181 k = k + 1;
182 }
183 k = 0;
184
185 // Moves every character 9 positions forward
186 while ( j <= 20) {
187 line [ j ] = line [ j + 9];
188 j = j + 1;
189 }
190 j = 0;
191
192 q = line . toInt () ; // Converts string to int
193
194 // Sets RssI status
195 if ( state == 1 ) {
196 RssI = q ;
197
198 }
199 // Sets Vin for battery
200 if ( state == 2) {
201 voltage = q / 10;
202 state = 0;
203 }
204
205 spd = spd - 100; // Scales to -100 to 100

E:12
 E.2. REMOTE CODE

206
207 display . clearDisplay () ; // Clears Display
208 display . setTextSize (1) ; // Normal 1:1 pixel scale
209 display . setTextColor ( SSD1306_WHITE ) ; // Draw white text
210 display . setCursor (0 , 0) ; // Start at top - left corner
211 display . print ( F ( " SPD " ) ) ;
212 display . print ( spd , 0) ;
213 display . print ( F ( " % " ) ) ;
214 display . setCursor (64 , 0) ;
215 display . print ( F ( " ROT " ) ) ;
216
217 // Prints R if joystick x - axis position is Right
218 if ( rot > 100) {
219 rot = rot - 100;
220 display . print ( F ( " R " ) ) ;
221 display . print ( rot , 0) ;
222 display . println ( F ( " % " ) ) ;
223 }
224
225 // Prints L if joystick x - axis position is Right
226 else if ( rot < 100) {
227 rot = 100 - rot ;
228 display . print ( F ( " L " ) ) ;
229 display . print ( rot , 0) ;
230 display . println ( F ( " % " ) ) ;
231 }
232
233 // Prints Zero if joystick x - axis is 0
234 else {
235 display . print ( " 0 " ) ;
236 display . println ( F ( " % " ) ) ;
237 }
238
239 refreshMean = refreshMean + refreshTime ; // Creates a 20 - loop
refresh mean
240 // Saves the maximum response time
241 if ( refreshTime >= refreshMax ) {
242 refreshMax = refreshTime ;
243 }
244
245 // Saves refresh times to display float
246 if ( i == 20) {
247 refreshMeanDisp = refreshMean / i ;
248 refreshMean = 0;
249 refreshMaxDisp = refreshMax ;
250 refreshMax = 0;
251 i = 0;
252 }
253 i = i + 1;
254
255 // Prints response time
256 display . setCursor (0 , 10) ;
257 display . print ( F ( " refreshTime " ) ) ;
258 display . print ( refreshMeanDisp , 0) ;

E:13
 APPENDIX E. CODE

259 display . println ( F ( " ms " ) ) ;

260 display . setCursor (0 , 20) ;
261 display . print ( F ( " refreshMax " ) ) ;
262 display . print ( refreshMaxDisp , 0) ;
263 display . println ( F ( " ms " ) ) ;
264
265
266 // Scales and print robot battery voltage
267 voltage = voltage / 10000;
268 display . setCursor (0 , 30) ;
269 display . print ( F ( " Vin " ) ) ;
270 display . print ( voltage , 2) ;
271 display . println ( F ( " V " ) ) ;
272
273 // Calculates and prints battery percentage for robot
274 batper = ( voltage2 - minbat ) / ( maxbat - minbat ) ;
275 display . setCursor (64 , 30) ;
276 display . print ( F ( " Bat " ) ) ;
277 display . print ( batper , 0) ;
278 display . println ( F ( " % " ) ) ;
279 display . display () ; // prints the display
280 }

E.2.2 Reciever
This section contains the code on the ESP8266 in the robot.
1 /*
2 Remote Transmitter Code
3 Date : 2020 -05 -20
4 Written by : Fredrik Ihrfelt and William Marin
5 Examinor : Nihad Subasic
6 TRITA - nr : 2020:36
7 Course : MF133X
8
9 Bachelor ’s thesis at KTH in mechatronics
10
11 The code :
12 - Reads request from client
13 - Processes request and sends to Arduino
14 - Measure battery voltage
15 - Sends battery voltage to remote controller
16 */
17 # include < ESP8266WiFi .h >
18 # include < ESP8266WebServer .h >
19
20 const char * ssid = " robotwifi " ;
21 const char * password = " kthkex20 " ;
22 const int analogInPin = A0 ; // ESP8266 Analog Pin ADC0 = A0
23
24 int sensorValue = 0; // value read from the pot

E:14
 E.2. REMOTE CODE

25 int sendToEsp = 0;
26 float voltage = 12;
27 float spdA = 1;
28 float rotA = 1;
29
30 ESP8266WebServer server (80) ; // Opens the 80 port
31
32 // This handle is triggerd every 10 ms
33 void handleSentVar () {
34 if ( server . hasArg ( " sensor_reading " ) ) { // this is the variable sent
from the client
35
36 float spd = server . arg ( " sensor_reading " ) . toInt () ;
37 float rot = server . arg ( " sensor_reading2 " ) . toInt () ;
38 float state = server . arg ( " sensor_reading3 " ) . toInt () ;
39
40 spd = spd / 100;
41 rot = rot / 100;
42
43
44 // Sends data to Arduino if diffrent from previous or start value
45 if ( spd >= 0) {
46 if ( spd <= 2) {
47 if ( rot >= 0) {
48 if ( rot <= 2) {
49 if ( spd != spdA ) {
50 Serial . print ( " <" ) ;
51 Serial . print ( spd ) ;
52 Serial . print ( " ," ) ;
53 Serial . print ( rot ) ;
54 Serial . println ( " >" ) ;
55 }
56
57 else if ( rot != rotA ) {
58 Serial . print ( " <" ) ;
59 Serial . print ( spd ) ;
60 Serial . print ( " ," ) ;
61 Serial . print ( rot ) ;
62 Serial . println ( " >" ) ;
63 }
64
65 spdA = spd ;
66 rotA = rot ;
67
68 // Reads battery voltage , scale and convert to int
69 sensorValue = analogRead ( analogInPin ) ;
70 voltage = ( sensorValue * 1800000) / (1023) ;
71 round ( voltage ) ;
72 sendToEsp = voltage ;
73
74 server . send ( sendToEsp , " " , " " ) ; // Sends data to
transmitter
75 }
76 }

E:15
 APPENDIX E. CODE

77 }
78 }
79 }
80 }
81
82
83
84 void setup () {
85 Serial . begin (115200) ;
86 delay (50) ;
87 WiFi . softAP ( ssid , password ) ;
88 IPAddress myIP = WiFi . softAPIP () ;
89 server . on ( " / data / " , HTTP_GET , handleSentVar ) ;
90 server . begin () ;
91 }
92
93 void loop () {
94 server . handleClient () ;
95 delay (10) ;
96 }

E:16
TRITA ITM-EX 2020:36

www.kth.se