Balancing Cube

Thesis WIP

TIM GIDLÖF
CARL GRUNEAU

Bachelor’s Thesis at ITM

Supervisor: Nihad Subasic
Examiner: Nihad Subasic

TRITA-ITM-EX 2020:35
 Abstract
In todays society there is microprocessors in almost ev-
ery new item that is produced for home use. They are all
being connected and smart, and by that micro controllers
is playing an increasingly important role in peoples private
life.
In this thesis in mechatronics a micro controller will
be implemented on an Arduino to make it possible for a
cube to balance in upright position. The cube is in theory
an inverted pendulum with one degrees of freedom, and is
intended to balance using an inertia wheel at the top of the
structure. A PID regulator was used, and at the time this
report was written, the right parameter values for PID was
not found. The cube is able to shift its position back and
forth over the setpoint with support on each side to prevent
it from falling. A bit more tuning is required to make it
balance on its own.

Keywords: Mechatronics, Reaction wheel, Balancing robot, Control theory, PID

 Referat
Balanserande Kub
I dagens samhälle är microprocessorer i nästan alla nya
produkter skapade för hemma användande. De är alla sam-
mankopplade och smarta. I och med det får mikrokontroller
en allt större roll i människors privata liv.
I den här rapporten inom mekatronik implementeras
en microkontroller til en arduino för att balansera en kub
stående på en kant. I teorin är en kub en inverterad pendel
med en frihetsgrad och är tänkt att balansera med hjälp av
ett reaktionshjul monterat överst på prototypen. En PID
regulator användes och då denna rapporten skrevs hade
rätt parametrar inte funnits. Kuben klarar av att ändra
position fram och tillbaka över referensläget då den blocke-
ras från att ramla. För att den ska klara av att balansera
av sig själv behövs regulatorn ställas in bättre.
Acknowledgements

We would like to thank our course supervisor and examiner Nihad Subasic for always
answering questions and emails at times they appeared and giving us an oppertunity to
complete this project despite the effects of COVID-19. A special appreciation goes to the
course assistant Seshagopalan Thorapalli Muralidharan for always taking the time to help
and meeting up with us when we got completely stuck.
Contents

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

2 Theory 3
2.1 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.1.1 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.1.2 Hall effect-sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.2 Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.2.1 Energy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.2.2 Lagrangian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.3 Control theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.3.1 State space model . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.3.2 State feedback . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.3.3 PID-controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3 Construction and implementation 9

3.1 Components . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3.1.1 Motor and motor driver . . . . . . . . . . . . . . . . . . . . . . . . 9
3.1.2 Reading crucial values . . . . . . . . . . . . . . . . . . . . . . . . . 9
3.2 Construction and software . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.2.1 Test Rig . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.2.2 Reaction wheel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.2.3 Tuning the controller . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.2.4 Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

4 Results 13

5 Discussion 15
5.1 Parasitic inductance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
5.2 Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
5.3 Improvements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
5.4 COVID-19 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

Bibliography 17
A Dunkermotoren G42X40 datasheet 19

B Circuitdiagram 21

C Arduino code 23

Appendices 30
List of Figures

2.1 A visualization of θc , θw , Center of mass, Lm and L (Created using GeoGebra). 4

3.1 The design of the test rig made (Created using Solid Edge 2019). . . . . . . . . 10
3.2 Flow chart of the system (Created using draw.io) . . . . . . . . . . . . . . . . 11

4.1 Picture of finished product seen from the front . . . . . . . . . . . . . . . . . . 13

4.2 Picture of finished product seen from the side . . . . . . . . . . . . . . . . . . . 14

A.1 Datasheet for the Dunkermotoren G42X40 . . . . . . . . . . . . . . . . . . . . 19

B.1 Circuitdiagram over all electric components. (Created using Fritzing) . . . . . 21

List of Tables
Chapter 1

Introduction

1.1 Background
Considering today’s generation of robotics, it is important for a mechatronics student to
learn control theory and how it is applied to balancing machines and objects. Similar
objects have already been created by mechatronic institutions which leaves great amounts
of information in the subject to be found.
For many years inverted pendulums have been used to study control theory in labora-
tories[1]. First to implement linear control on a system that is unstable to then go more
in depth of nonlinear control due to an inverted pendulums nonlinear motion. A cube that
would balance on one corner shows the same control challenges as an inverted pendulum
however instead of a 2D-space the balancing cube operate in the 3D-space.
The cube in this project will use inertia wheels to be able to balance. Inertia wheels
can for instance be used to stabilise various kinds of spacecraft, as an example satellites[2].

1.2 Purpose
The purpose of this bachelor thesis is to construct and program a cube that is able to
balance on one of the edges without any external help. It should also be able to handle
minor disturbances. The second purpose of this bachelor thesis is to further develop the
knowledge in control system engineering and electronics.

1.3 Goals
The first goal is to make the cube balance on its edge and with minor disturbances keep it
balanced.
The second goal will be to make it jump up from laying with the flat side down, to the
edge and then go into the balancing phase.
With time and resources, the third goal will be to make the cube safely fall down to the
resting position.

1
 CHAPTER 1. INTRODUCTION

1.4 Scope
All electronics and mechanical parts shall be fitted into a 150x150x150mm cube apart from
the power source which will stand beside the cube. The robot needs to have a mechanical,
an electrical and a programmable part.

1.5 Method
The course of action in this project started with modelling the dynamics of the cube to
formulate mathematical equations that could be used in the control theory. The control
theory was then tested in matlab to after be implemented into a test rig.

2
Chapter 2

Theory

This chapter will describe the theory behind the mechanics of the cube. The first sections
will describe the hardware needed to make the cube balance and how they work. The second
part will describe the mathematics that will be needed for the control theory and the last
part will bring up the control theory that will be implemented.

2.1 Sensors
To be able to control the system and make a closed loop system, different sensors is needed
to read the most crucial variables. The accelerometer and gyroscope sensor are measuring
the tilt angle and angle velocity of the whole system, and the hall effect sensor is used to
read the angular velocity of the reaction wheel that will stabilize the system.

2.1.1 IMU
An IMU is an abbrevation for Inertial Measurement Unit[3]. The IMU sensor can measure
from 2 to 6 degrees of freedom which translates to be able to measure movement in the
3D space. In this project the IMU sensor will be a gyroscope sensor and accelerometer
combined. The gyroscope sensor is measuring the angular velocity, and by math and Coriolis
effect, it is possible to calculate the angle the sensor is situated in. The accelerometer
sensor measures the linear acceleration with the help of a moving mass inside the sensor.
The mass is moving in a linear motion, and is coupled with small arms which acts as
capacitors together with the small elements on the board. When the mass is moving, the
small capacitance emerging can be read and the acceleration be calculated.

2.1.2 Hall effect-sensor

A hall effect sensor is detecting magnetic fields near the sensors tip. By reading the small
voltage appearing when a magnetic field is applied, the sensor is for instance able to read
the revolutions per minute of a wheel if the dimensions from the wheel is known.

3
 CHAPTER 2. THEORY

2.2 Modeling
For the cube to be able to balance on its edge, a model of the dynamics and a state-space
model has to be designed.

2.2.1 Energy
To model the dynamics of the cube and reaction wheel, equations of potential- and kinetic
energies has to be set up [4][5]. To begin with, the potential energy equation will be formed.
The system is divided between the mass of the reaction wheel and the remaining mass
of the cube. Since the mass of the reaction wheel affects the system from the center of the
cube the equation is set up as

V = (mw L + mc Lm )g cos(θc ) (2.1)

where mw and mc is the mass of the wheel and cube respectively. L is the length from the
balancing edge to the center of the cube and Lm is the length to center of mass. As seen in
Figure 2.1, θc is the angle the cube has in respect to the center point.

Figure 2.1. A visualization of θc , θw , Center of mass, Lm and L (Created using

GeoGebra).

Next the equation for the kinetic energy will be modeled. The equations will be based
of inertia around the balancing point of the cube. Due to the reaction wheel affecting the
whole cubes inertia, it has to be considered forming the equation. Same goes for the angle
velocity of the cube affecting the energy of the reaction wheel. From this the equation of
the kinetic energy can be stated as

1 1
T = (Ic + mw L2 )θ̇c2 + Iw (θ̇c + θ̇w )2 (2.2)
2 2
where the first part is the energy from the cube and the second part is the energy of the
reaction wheel. Ic and Iw is the inertia from the cube and wheel respectively.

4
2.2. MODELING

2.2.2 Lagrangian
To formulate the dynamics of the system, a method can be used called the Lagrangian-
equations [6]. The Lagrangian-functions are based on degrees of freedom, which describes
particles independent movement in the 3D-space, and their generalized coordinates which
is their positions. The difference between the cubes kinetic- and potential energies is what
formulates the Lagrangian-function as L = T − V . Inserting equations 2.1 and 2.2 gives
1 1
L= (Ic + mw L2 )θ̇c2 + Iw (θ̇c + θ̇w )2 − (mw L + mc Lm )g cos(θc ). (2.3)
2 2
Now to describe the system and its movements, differential equations based on the Lagrangian-
function will be formed. The Lagrangian-equation is formulated as
d ∂L ∂L ∂R
( )− + = pk (2.4)
dt ∂ q̇k ∂qk ∂ q̇k
where qk is the generalized coordinates as described above and q̇k is their change over time.
Added to the equation is the Hamilton-equations ∂∂R q̇k , which are the forces that make the
system lose energy in form of friction, called the dissipative forces, and pk which is the
generalized torque.
Derivatives of the Lagrangian-function with respect of angle velocity and time will give
the first part of the the Lagrangian-equation,
d ∂L
( ) = (Ic + mw L2 )θ̈c + Iw (θ̈c + θ̈w ) (2.5)
dt ∂ θ̇c

d ∂L
( ) = Iw (θ̈c + θ̈w ) (2.6)
dt ∂ θ̇w
where equation 2.5 is based on the cubes angular velocity and 2.6 is based on the wheels
angular velocity. Next
∂L
= (mw L + mc Lm )g sin(θc ) (2.7)
∂θc
forms the derivative of the Lagrangian-function with respect to the cubes angle. The partial
derivative with respect to the wheels angle is based of the momentum transferred from the
motor Mm ,
∂L
= K2 φu (2.8)
∂θw
where K2 φu = Mm [7]. K2 φ is the motor constant and u is the power input to the motor.
The last part of the Lagrangian-equation is to formulate the losses from friction. To
express the friction losses
1 1
R = fc θ̇c2 + fw θ̇w
2
(2.9)
2 2
is used, where fc is the friction constant between the cube and surface and fw is the friction
constant between the reaction wheel and motor axle. The derivatives of equation 2.9 with
respect to the angle velocities is expressed as
∂R
= fc θ̇c (2.10)
∂ θ̇c

∂R
= fw θ̇w (2.11)
∂ θ̇w

5
 CHAPTER 2. THEORY

which forms the last part of the Lagrangian-equation. By inserting equations 2.5-2.11 in
equation 2.4 an expression for the the dynamics of the system can be formulated as

(mw L + mc Lm )g sin(θc ) − K2 φu + fw θ̇w − fc θ̇c

θ̈c = (2.12)
Ic + mw L2

(Ic + Iw + mw L2 )(K2 φu − fw θ̇w ) − (mc Lm + mw L)g sin(θc )Iw + fc θ̇c Iw

θ̈w = (2.13)
Iw (Ic + mw L2 )

which can be used to design the control theory behind the cube.

2.3 Control theory

To be able to control the system, a controller will be implemented. There are various
kinds of potential regulators. Depending on the actual system, an evaluation is needed
to determine the most suitable controller. In our model there are three crucial values the
whole system is relying on, and they are all readable.

2.3.1 State space model

The state space model is on the form

ẋ = A~x(t) + B~u(t) (2.14)

y = C~x(t) + D~u(t), (2.15)

where ẋ represents the derivative of the states with respect to time, the y represents the
output value, the ~x is a state-vector and ~u is a vector of the input values. With this model,
and a given an input signal, it is possible to predict how the system will react and what
output value you will get at a specific point of time. To make the state space model much
more easy to handle, it is preferred if the model is linear. If not so, a wisely selected point
to linearize about must be stated. In this case, because the action for the system will
occur while the cube is balancing on the edge, the point along the vertical axis with all the
parameters set to zero is chosen as the point to linearize around.
 
0 1 0
 (Lmw +Lm mc )g −fc fw
A= (2.16)

Ic +mw L2 Ic +mw L2 Ic +mw L2 
−(Lmw +Lm mc )g fc −(Ic +Iw +mw L2 )cw
Ic +mw L2 Ic +mw L2 Iw (Ic +mw L2 )
 
0
−K2 φ
B= (2.17)
 
Ic +mw L2 
2
(Ic +Iw +mw L )K2 φ
2
Iw (Ic +mw L )
 
1 0 0
C = 0 1 0 (2.18)
0 0 1
 
D= 0 (2.19)

6
2.3. CONTROL THEORY

2.3.2 State feedback

When the correlation between the input and output is known, it would be advantageous to
base your input value on your states [8]. To do so it is needed to implement a state feedback
controller. When using a feedback controller, the location of the poles must be known so
the system can act most optimally. But finding these placements for the poles can be quite
hard, and a few compromises might be needed regarding the robustness and speed of the
system. By implementing a Linear Quadratic Regulator (LQR), these compromises can be
optimized by finding an ”L” that minimizes the criteria
Z ∞
(xT (t)Qx(t) + u2 (t))dt (2.20)
0

where Q typically is set to be a diagonal matrix where the elements indicates how fast the
corresponding component of x should behave.

2.3.3 PID-controller
Another often used controller in the industry is the PID controller[8], and it has been around
since the 1700s in various forms. It is a low cost controller, and usually not requires any
models for the system to be tuned correctly. The controller is easy implemented, but requires
fine tuning to get the perfect parameters P, I and D which is the three gain parameters that
calculates the output with respect to the error, the integral of error and derivative of error.
Z t
1 d
u(t) = KP (e(t) + (e(t))dt + KD e(t)) (2.21)
KI 0 dt

where the u(t) is the output value, and

e(t) = r(t) − y(t) (2.22)

where r(t) is the reference value, and the y(t) is the system output value which is fed back
in to the controller, hence a feedback loop. The KP in the formula is the proportional
gain, and is always proportional to the current error e(t). With a big KP gain, the system
will react fast to small changes. The KD gain is preventing overshoot of the system by
looking at the derivative of the error, and KI gain is mostly reducing the steady state error.
Combining these three parameters and tuning them to perfection can sometimes be hard,
which is the biggest disadvantage of the PID controller.

7
Chapter 3

Construction and implementation

This chapter will cover the components used in this project, and how the construction is
build up. A CAD-model will be presented to visualize the concept. Due to the lack of
financial means the outcome of the project is not what was first planned, but it fulfills its
purpose. This is after all just meant to be a prototype, so the cosmetics of the construction
is not the biggest priority.

3.1 Components
The most crucial components used in this project is briefly presented below.

3.1.1 Motor and motor driver

The motor for this project is a Dunkermotoren GR42X40, which was available in the labora-
tory at the institution. At first the plan was to use a three phased brushless motor, mostly
because the compact design which makes the whole construction more compact. But after
consulting with the assistant for the course, the conclusion was to skip the brushless motor,
mostly due to the complications in selecting a motor driver that is properly configured. The
actual motor is 24V and 21W, with a torque constant of 0.0584 Nm/A, which is enough
according to the Matlab simulations that has been executed.
The driver chosen is a VNH3SP30 motor driver, and it is capable of delivering up to 30
amps with a range of 5.5 to 36 volt. Since the Arduino used in this project only is capable
of delivering 5 volt, an external power source was required to provide power to the motor.

3.1.2 Reading crucial values

The LQR controller is relying on correct values from the parameters discussed in section
2.2. To provide the controller with the right values an MPU 6050 sensor was bought. The
sensor is capable of detecting both acceleration and angles, thanks to its onboard sensors.
A crucial step in mounting the sensor was to get it lined up with the vertical axis, to
get the right values from the gyroscope sensor. The MPU 6050 sensor is providing the
controller with both angle acceleration of the cube and the angle the cube is tilting in.
To read the angle acceleration of the wheel two hall effect sensors was bought, one for
reading clockwise rotation and the other for reading counterclockwise rotation. They are
spaced out at two different radii from the motor axle, with magnets mounted so they have

9
 CHAPTER 3. CONSTRUCTION AND IMPLEMENTATION

a lead when the wheel turns in a specific direction. Meaning if the wheel is spinning in
counterclockwise direction, the inner magnet have a lead, and the inner hall effect sensor
will be detecting the magnet before the outer one, thereby the Arduino knows the wheel is
turning in counterclockwise direction.
To make the readings more accurate and cancel out the noise from the sensor a Kalman
filter was implemented, which make values of the states more stable over time and the
controller can act more accurately. The Kalman filter was introduced 1960 by R.E Kalman
[9]. The purpose of the filter is to predict how the system will behave at different times and
it can do so even though the nature of said system is unknown. It works by implementing
equations and gives a solution of the least-square method that the controller can compute.

3.2 Construction and software

The construction will be discussed in the following section. Starting off by explaining the
test rig and different parts that went into creating it and followed up by the code that was
implemented to make it work.

3.2.1 Test Rig

A test rig was built to simulate balancing on the cubes edge in the form of one side and two
cutouts designed to give the rig structure and hold the motor as can be seen in figure 3.1
below. Two spacers were 3D-printed to hold everything together and a rod going through
all of the parts. The rod was then connected to a base plate and for the cube to be able to
rotate around the axis two wheel bearings were connected on each end.

Figure 3.1. The design of the test rig made (Created using Solid Edge 2019).

10
3.2. CONSTRUCTION AND SOFTWARE

3.2.2 Reaction wheel

The reaction wheel was first drafted up in Solid edge, setting the dimension so the weight
would be the right. The wheel was then waterjet cut and lathed so the hub and spokes were
thinner than the outer ring. Doing so the wheel has the most of its mass concentrated at
the periphery giving it a higher moment of inertia. The outcome was a wheel with diameter
of 15 cm and 5 mm thick.
An axle hub was created out of aluminium to be able to mount the reaction wheel on
the 5 mm motor axle.

3.2.3 Tuning the controller

After discussions with the course assistant it was decided to implement a PID controller
instead of using LQR due to communication between Matlab and Arduino will cause a delay
in the system and Matlab was required to solve the equations for the LQR.

3.2.4 Code
An Arduino was used to control the system, reading the values and provide the right voltage
to the motor. As stated above LQR would require calculations to be performed by Matlab,
which would receive the parameter values from the Arduino by USB cable, and then sending
back the correct voltage to supply the motor with when the calculations would have been
executed. Instead a PID controller was chosen. The PID controller only need one input
value which in this case was the angle of the cube picked up by the accelerometer and the
Arduino will do the rest of the calculations.

Figure 3.2. Flow chart of the system (Created using draw.io)

The code for the Arduino was written in Arduino IDE and then uploaded to the mi-
croprocessor, whilst the code for matlab is written in matlab and the program is running

11
 CHAPTER 3. CONSTRUCTION AND IMPLEMENTATION

parallel with the arduino code. As can be seen in figure 3.2 the PID is implemented with
the angle of the cube as setpoint.

12
Chapter 4

Results

As can be seen in figures 4.1 and 4.2 below the intended cube ended up in the prototype
state, since the focus was based at making the prototype balance. The prototype was never
able to balance on its own. With the PID parameter values at KP = 7.5, KI = 0.6 and
KD = 0.9 the prototype was rocking back and forth in between 2 fingers that was blocking
it from falling over. Due to the first goal not being fulfilled the second and third goal could
not be tested.

Figure 4.1. Picture of finished product seen from the front

13
 CHAPTER 4. RESULTS

Figure 4.2. Picture of finished product seen from the side

14
Chapter 5

Discussion

The following chapter will bring up the different obstacles, thoughts and improvements that
was encountered during the process.

5.1 Parasitic inductance

The first obstacle that was encountered building the system was parasitic inductance[10].
When the motor was supplied with an alternating voltage an undesired inductance was
formed called parasitic inductance. The parasitic inductance caused a magnetic field that
disturbed the MPU 6050 which initially gave off readings but after a short while interrupted
the signal and stopped giving readings completely. To solve the encountered obstacle a large
number of capacitors in varying sizes were used, connected between Vcc and ground both
to the MPU 6050 and the motordriver. The reason behind using capacitors was to have
a more stable current by reducing the voltage spikes given off the motor. The parasitic
inductance was not removed completely but the system became a lot more stable and the
MPU 6050 was able to give better readings.

5.2 Controller
The PID controller that was implemented never received the correct parameters to make
the cube balance. The controller was able to counter falling over on one side however doing
so the overshoot put the system in a too large angle on the opposite side and the cube fell
over. A lot of time was spent trying to find the best parameter values and more time will
be spent before the final deadline.

5.3 Improvements
The inertia wheel was not properly balanced, mainly the dynamic balance. One reason
could be the hub not completely flat on the side mated with the wheel. To improve this a
new hub would be created and after properly dynamically balance the wheel, reason for it
not being done was not having the proper equipment. The ball bearings had a small play
on the axle the test rig was mounted on. Which could also be a factor as to why the rig

15
 CHAPTER 5. DISCUSSION

was not balancing. Future work would be to implement everything into a cube as was the
original idea.

5.4 COVID-19
During the time of this project the pandemic COVID-19 was in full effect. The Swedish
government decided to close off all of the higher education which meant all studies were
to be performed from distance, meaning no access to the Royal Institute of Technology’s
facilities. The communication with the course assistants was mostly through e-mail and
workshops online through video conferences. Due to this when an obstacle was encountered
a lot of time was spent trying to solve it while receiving suggestions from prior mentioned
course assistant. Because of this changes was made to the design, first plan was to create a
cube that was able to balance but instead the test rig became the final product. Some parts
that was needed but was difficult to create at home was made at KTH and then picked up,
the rest was solved by implementing certain parts that was accessible at home.

16
Bibliography

[1] K.J. Åström and K. Furuta. “Swinging up a pendulum by energy control”. In:
Automatica 36.2 (2000), pp. 287–295. url: https://doi.org/10.1016/
S0005-1098(99)00140-5.
[2] Aage Skullestad and James M. Gilbert. “H control of a gravity gradient sta-
bilised satellite”. In: Control Engineering Practice 8.9 (2000), pp. 975–983.
url: https://doi.org/10.1016/S0967-0661(00)00044-7.
[3] C.Pao. What is an IMU sensor? 2018. url: https://www.ceva- dsp.
com/ourblog/what-is-an-imu-sensor/ (visited on 05/13/2020).
[4] Erik Bjerke and Björn Pehrsson. “Development of a Nonlinear Mechatronic
Cube”. Master Thesis. Chalmers University of Technology, 2016. url: http:
//publications.lib.chalmers.se/records/fulltext/233543/
233543.pdf.
[5] M. Gajamohan et al. “The Cubli: A cube that can jump up and balance”. In:
2012 IEEE/RSJ International Conference on Intelligent Robots and Systems.
Vilamoura: 2012 IEEE/RSJ International Conference on Intelligent Robots
and Systems, 2012, pp. 3722–3727. url: http://ieeexplore.ieee.org.
focus.lib.kth.se/stamp/stamp.jsp?tp=%7B%5C&%7Darnumber=
6385896%7B%5C&%7Disnumber=6385431.
[6] Nicholas Apazidis. Mekanik II - Partikelsystem, stel kropp och analytisk mekanik.
1:3. Lund: Studentlitteratur AB, 2017, pp. 271–277.
[7] Hans B. Johansson. Elektroteknik. 2013års. Stockholm: Institutionen för Maskinkon-
struktion, KTH, 2013, p. 298.
[8] Torkel Glad and Lennart Ljung. Reglerteknik - Grundläggande teori. 4:16.
Lund: Studentlitteratur AB, 2018.
[9] Greg Welch, Gary Bishop, et al. “An introduction to the Kalman filter”. In:
(1995). url: http://citeseerx.ist.psu.edu/viewdoc/download?
doi=10.1.1.336.5576&rep=rep1&type=pdf.
[10] R. Bayerer. “Parasitic inductance hindering utilization of power devices”. In:
CIPS 2016; 9th International Conference on Integrated Power Electronics Sys-
tems. 2016, pp. 1–8. url: https : / / ieeexplore - ieee - org . focus .
lib.kth.se/stamp/stamp.jsp?tp=&arnumber=7736790.

17
Appendix A

Dunkermotoren G42X40 datasheet

Figure A.1. Datasheet for the Dunkermotoren G42X40

19
Appendix B

Circuitdiagram

Figure B.1. Circuitdiagram over all electric components. (Created using Fritzing)

21
 Appendix C

Arduino code

1 /*
2 Bachelor thesis by Tim Gidlof and Carl Gruneau, KTH 2020
3 TRITA number: TRITA_ITM_EX 2020:35
4 Name of project: Balancing Cube
5
6 Description of the code:
7 This program is intended to make an one DOF cube balance on its
edge. It’s doing so by
8 read angle measurement from IMU mpu6050 sensor, filter the angle
through a Kalmanfilter
9 and then use the angle to calculate the right voltage for the
motor. A PID is implemented
10 which takes the filtered angle and gives an output.
11 */
12
13 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All
rights reserved.
14
15 This software may be distributed and modified under the terms of
the GNU
16 General Public License version 2 (GPL2) as published by the Free
Software
17 Foundation and appearing in the file GPL2.TXT included in the
packaging of
18 this file. Please note that GPL2 Section 2[b] requires that all
works based
19 on this software must also be made publicly available under the
terms of
20 the GPL2 ("Copyleft").
21
22 Contact information
23 -------------------
24
25 Kristian Lauszus, TKJ Electronics

23
 APPENDIX C. ARDUINO CODE

26 Web : http://www.tkjelectronics.com
27 e-mail : kristianl@tkjelectronics.com
28 */
29
30 #include <PID_v1.h> // installed library for PID-controller
31 #include <Wire.h>
32 #include <Kalman.h> // Source: https://github.com/TKJElectronics/
KalmanFilter
33
34 #define RESTRICT_PITCH // Comment out to restrict roll to 90deg
instead - please read: http://www.freescale.com/files/sensors
/doc/app_note/AN3461.pdf
35
36 Kalman kalmanX; // Create the Kalman instances
37 Kalman kalmanY;
38
39 /* IMU Data */
40 double accX, accY, accZ;
41 double gyroX, gyroY, gyroZ;
42 int16_t tempRaw;
43
44
45 double gyroXangle, gyroYangle; // Angle calculate using the gyro
only
46 double compAngleX, compAngleY; // Calculated angle using a
complementary filter
47 double kalAngleX, kalAngleY; // Calculated angle using a Kalman
filter
48 double offsetAngle = 2.7;
49
50 uint32_t timer;
51 uint8_t i2cData[14]; // Buffer for I2C data
52
53 //motor constants
54 const int inaPin = 13;
55 const int inbPin = 9;
56 const int pwmPin = 6;
57 const int trimPin = A0;
58 int i;
59
60 // PID variables
61 double Setpoint, Input, Output;
62 double Kp = 15, Ki = 0, Kd = 5;
63 PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
64
65
66 void setup() {
67
68 Serial.begin(115200);

24
 69 Wire.begin();
70 #if ARDUINO >= 157
71 Wire.setClock(400000UL); // Set I2C frequency to 400kHz
72 #else
73 TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to
400kHz
74 #endif
75
76 i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) =
1000Hz
77 i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc
filtering, 256 Hz Gyro filtering, 8 KHz sampling
78 i2cData[2] = 0x00; // Set Gyro Full Scale Range to 250deg /s
79 i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to
2g
80 while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four
registers at once
81 while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis
gyroscope reference and disable sleep mode
82
83 while (i2cRead(0x75, i2cData, 1));
84 if (i2cData[0] != 0x68) { // 0x68 Read "WHO_AM_I" register
85 Serial.print(F("Error reading sensor"));
86 while (1);
87 }
88
89 delay(100); // Wait for sensor to stabilize
90
91 /* Set kalman and gyro starting angle */
92 while (i2cRead(0x3B, i2cData, 6));
93 accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
94 accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
95 accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
96
97 // Source: http://www.freescale.com/files/sensors/doc/app_note/
AN3461.pdf eq. 25 and eq. 26
98 // atan2 outputs the value of - to (radians) - see http://
en.wikipedia.org/wiki/Atan2
99 // It is then converted from radians to degrees
100 #ifdef RESTRICT_PITCH // Eq. 25 and 26
101 double roll = atan2(accY, accZ) * RAD_TO_DEG;
102 double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) *
RAD_TO_DEG;
103 #else // Eq. 28 and 29
104 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) *
RAD_TO_DEG;
105 double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
106 #endif
107

25
 APPENDIX C. ARDUINO CODE

108 kalmanX.setAngle(roll); // Set starting angle

109 kalmanY.setAngle(pitch);
110 gyroXangle = roll;
111 gyroYangle = pitch;
112 compAngleX = roll;
113 compAngleY = pitch;
114
115 //MOTOR parameters
116 pinMode(inaPin, OUTPUT);
117 pinMode(inbPin, OUTPUT);
118 pinMode(pwmPin, OUTPUT);
119 pinMode(trimPin, INPUT);
120
121 // PID parameters
122 Setpoint = 0;
123 myPID.SetMode(AUTOMATIC);
124 timer = micros();
125 }
126
127 // Main loop where all the magic happends
128 void loop(){
129 mpu_kalman(); // reads the values
from the sensor, and filters them
130 Input =( kalAngleY-offsetAngle); // a sligt offset is
subtracted from the angle to get the
131 // correct input for
the PID
132 Serial.println(kalAngleY-offsetAngle);
133
134
135 // if the angle is bigger than 10 degrees, the cube will do
nothing
136 if (abs(Input) > 10){
137 digitalWrite(inaPin, LOW);
138 digitalWrite(inbPin, LOW);
139 analogWrite(pwmPin, 0);
140 //Serial.println("STOP");
141 }
142
143 // If the angle is between 0 and -10 degrees
144 if (Input < 0 && Input > -10){
145 digitalWrite(inaPin, LOW);
146 digitalWrite(inbPin, HIGH);
147 myPID.Compute();
148 analogWrite(pwmPin, Output);
149 }
150 // If the angle is between 0 and 10 degrees
151 else if (Input >= 0 && Input < 10) {
152 digitalWrite(inaPin, HIGH);

26
153 digitalWrite(inbPin, LOW);
154 Input = Input * (-1);
155 myPID.Compute();
156 analogWrite(pwmPin, Output);
157 }
158
159
160 }
161
162 // void that reads all the values from the MPU650 sensor,
calculates the angles
163 // and then filters them
164 void mpu_kalman() {
165 /* Update all the values */
166 while (i2cRead(0x3B, i2cData, 14));
167 accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
168 accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
169 accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
170 //tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
171 gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
172 gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
173 gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;
174
175 double dt = (double)(micros() - timer) / 1000000; // Calculate
delta time
176 timer = micros();
177
178 // Source: http://www.freescale.com/files/sensors/doc/app_note/
AN3461.pdf eq. 25 and eq. 26
179 // atan2 outputs the value of - to (radians) - see http://
en.wikipedia.org/wiki/Atan2
180 // It is then converted from radians to degrees
181 #ifdef RESTRICT_PITCH // Eq. 25 and 26
182 double roll = atan2(accY, accZ) * RAD_TO_DEG;
183 double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) *
RAD_TO_DEG;
184 #else // Eq. 28 and 29
185 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) *
RAD_TO_DEG;
186 double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
187 #endif
188
189 double gyroXrate = gyroX / 131.0; // Convert to deg/s
190 double gyroYrate = gyroY / 131.0; // Convert to deg/s
191
192 #ifdef RESTRICT_PITCH
193 // This fixes the transition problem when the accelerometer
angle jumps between -180 and 180 degrees

27
 APPENDIX C. ARDUINO CODE

194 if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX <
-90)) {
195 kalmanX.setAngle(roll);
196 compAngleX = roll;
197 kalAngleX = roll;
198 gyroXangle = roll;
199 } else
200 kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); //
Calculate the angle using a Kalman filter
201
202 if (abs(kalAngleX) > 90)
203 gyroYrate = -gyroYrate; // Invert rate, so it fits the
restriced accelerometer reading
204 kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
205 #else
206 // This fixes the transition problem when the accelerometer
angle jumps between -180 and 180 degrees
207 if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY
< -90)) {
208 kalmanY.setAngle(pitch);
209 compAngleY = pitch;
210 kalAngleY = pitch;
211 gyroYangle = pitch;
212 } else
213 kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); //
Calculate the angle using a Kalman filter
214
215 if (abs(kalAngleY) > 90)
216 gyroXrate = -gyroXrate; // Invert rate, so it fits the
restriced accelerometer reading
217 kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate
the angle using a Kalman filter
218 #endif
219
220 gyroXangle += gyroXrate * dt; // Calculate gyro angle without
any filter
221 gyroYangle += gyroYrate * dt;
222 //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle
using the unbiased rate
223 //gyroYangle += kalmanY.getRate() * dt;
224
225 compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll
; // Calculate the angle using a Complimentary filter
226 compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 *
pitch;
227
228 // Reset the gyro angle when it has drifted too much
229 if (gyroXangle < -180 || gyroXangle > 180)
230 gyroXangle = kalAngleX;

28
231 if (gyroYangle < -180 || gyroYangle > 180)
232 gyroYangle = kalAngleY;
233
234 Serial.print("\r\n");
235 delay(2);
236
237 }

1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All

rights reserved.
2
3 This software may be distributed and modified under the terms of
the GNU
4 General Public License version 2 (GPL2) as published by the Free
Software
5 Foundation and appearing in the file GPL2.TXT included in the
packaging of
6 this file. Please note that GPL2 Section 2[b] requires that all
works based
7 on this software must also be made publicly available under the
terms of
8 the GPL2 ("Copyleft").
9
10 Contact information
11 -------------------
12
13 Kristian Lauszus, TKJ Electronics
14 Web : http://www.tkjelectronics.com
15 e-mail : kristianl@tkjelectronics.com
16 */
17
18 const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
19 const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in
I2C communication
20
21 uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool
sendStop) {
22 return i2cWrite(registerAddress, &data, 1, sendStop); //
Returns 0 on success
23 }
24
25 uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t
length, bool sendStop) {
26 Wire.beginTransmission(IMUAddress);
27 Wire.write(registerAddress);
28 Wire.write(data, length);
29 uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on
success
30 if (rcode) {
31 Serial.print(F("i2cWrite failed: "));

29
 APPENDIX C. ARDUINO CODE

32 Serial.println(rcode);
33 }
34 return rcode; // See: http://arduino.cc/en/Reference/
WireEndTransmission
35 }
36
37 uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t
nbytes) {
38 uint32_t timeOutTimer;
39 Wire.beginTransmission(IMUAddress);
40 Wire.write(registerAddress);
41 uint8_t rcode = Wire.endTransmission(false); // Don’t release
the bus
42 if (rcode) {
43 Wire.endTransmission(true);
44 Wire.beginTransmission(IMUAddress);
45 Wire.write(registerAddress);
46 //Serial.print(F("i2cRead failed: "));
47 //Serial.println(rcode);
48 //return rcode; // See: http://arduino.cc/en/Reference/
WireEndTransmission
49 }
50 Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a
repeated start and then release the bus after reading
51 for (uint8_t i = 0; i < nbytes; i++) {
52 if (Wire.available())
53 data[i] = Wire.read();
54 else {
55 timeOutTimer = micros();
56 while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.
available());
57 if (Wire.available())
58 data[i] = Wire.read();
59 else {
60 Serial.println(F("i2cRead timeout"));
61 return 0; // This error value is not already taken by
endTransmission
62 }
63 }
64 }
65 return 0; // Success
66 }

30