DEGREE PROJECT IN TECHNOLOGY,

FIRST CYCLE, 15 CREDITS

STOCKHOLM, SWEDEN 2019

Reaction Wheel Stabilized

Stick
PONTUS GRÄSBERG

BILL LAVEBRATT

KTH ROYAL INSTITUTE OF TECHNOLOGY

SCHOOL OF INDUSTRIAL ENGINEERING AND MANAGEMENT
Reaction Wheel Stabilized Stick

Bachelor’s Thesis in Mechatronics

PONTUS GRÄSBERG
BILL LAVEBRATT

Supervisor: Nihad Subasic

Examiner: Nihad Subasic

TRITA-ITM-EX 2019:38
 Abstract
Control theory can be used to make an unstable system
stable. This thesis seeks to do this, where the system is a
two DOF inverted pendulum with reaction wheels for sta-
bilisation. The thesis also seeks to answer what is most im-
portant for making it stabilize for a longer period of time.
It was decided that a state space controller was to be used
with various sensors measuring the states. To be able to de-
sign a functioning demonstrator, a mathematical model of
the system dynamics was developed. In the end the demon-
strator proved to function as desired, being able to balance
indefinitely. It was found that it is absolutely necessary to
either give the controller a perfect set point or to implement
an automatic set point.

Keywords: Mechatronics, Reaction wheel, Inverted pen-

dulum, State space, Control theory.
 Referat
Reaktionshjuls stabiliserad pinne
Reglerteknik kan användas för att göra ostabila system sta-
bila. Målet med detta projekt var att göra detta med ett
system i form av en inverterad pendel med två frihetsgrader
som balanseras med hjälp av två svänghjul. Projektet söker
att besvara frågan om vad som är de viktigaste faktorerna
för att få systemet att vara stabilt över en längre tid. En
tillståndsåterkoppling användes som regulator vilket inne-
bar att flera olika sensorer behövdes för att mäta de olika
tillstånden. För att kunna konstruera en fungerande proto-
typ utvecklades en matematisk modell av systemet vilken
användes för simulering av systemet. Till slut konstruerades
en fungerade prototyp som till synes kunde balansera över
oöverskådlig tid. En av de faktorer som visade sig påver-
ka huruvida systemet uppnår stabilitet över längre tid var
hur bra referenspunkt som gavs till regulatorn, det vill sä-
ga det tillstånd som regulatorn reglerar systemet mot. Det
visade sig vara möjligt att implementera en självjusterande
referenspunkt som gjorde systemet stabilt över tid.
Acknowledgements

First of all we would like to thank KTH prototype center and Tomas Östberg for
helping us make the reaction wheels. Also a big thanks to Staffan Qvarnström
for helping us order parts and finding motors for the project as well as to the lab
assistants for helping us and answering various questions.
Contents

Contents

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

2 Theory 5
2.1 System dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.1.1 Dynamics of the system . . . . . . . . . . . . . . . . . . . . . 5
2.1.2 Motor dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.1.3 State space model . . . . . . . . . . . . . . . . . . . . . . . . 6
2.2 Control design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2.1 Simulation and Pole Placement . . . . . . . . . . . . . . . . . 8
2.3 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3.1 Accelerometer . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3.2 Gyroscope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3.3 Hall effect sensor . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.4 Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.5 Controlling motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3 Design and implementation 13

3.1 Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.1.1 Motor driver . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.2 Measuring motor speed . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.3 Inertial measurement unit . . . . . . . . . . . . . . . . . . . . . . . . 15
3.4 Construction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.5 Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.6 Tuning of the controller . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.6.1 Automatic set point . . . . . . . . . . . . . . . . . . . . . . . 18

4 Results 21
5 Discussion and conclusion 23
5.1 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
5.2 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

6 Recommendations and future work 25

6.1 Recommendations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
6.2 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

Bibliography 27

Appendices 27

A Datasheets 29

B Schematics 32

C Code 33
C.1 MATLAB code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
C.2 Arduino code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
List of Figures

2.1 Force analysis of inverted pendulum. Drawn in draw.io. . . . . . . . . . 6

2.2 Simulation of angular deviation of the pendulum. Made in MATLAB. . 8
2.3 Simulation of corresponing input voltage. Made in MATLAB. . . . . . . 9
2.4 MEMS accelerometer structure [3]. . . . . . . . . . . . . . . . . . . . . . 9
2.5 Two examples of PWM signals [6] . . . . . . . . . . . . . . . . . . . . . 11
2.6 A simple H-bridge circuit [7] . . . . . . . . . . . . . . . . . . . . . . . . 11

3.1 Illustration of how to measure direction. Drawn in draw.io. . . . . . . . 14

3.2 Plot of measured angular velocity of reaction wheel. Made in MATLAB. 14
3.3 The angle as given by the accelerometer, gyroscope and Kalman filter.
Made in MATLAB. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.4 Cad model. Made in Solid Edge ST10. . . . . . . . . . . . . . . . . . . . 16
3.5 The finished demonstrator while balancing. . . . . . . . . . . . . . . . . 17
3.6 Flowchart. Drawn in draw.io. . . . . . . . . . . . . . . . . . . . . . . . . 18
3.7 Shows how the set point moves compered to the angle while stabilising.
Made in MATLAB. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4.1 Shows the angle and angular velocity of the IMU while stabilising. Made
in MATLAB. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
4.2 Shows how the set point and the angle moved when the center of mass
was changed while stabilizing. Made in MATLAB. . . . . . . . . . . . . 22

A.1 Datasheet for the motor [12]. . . . . . . . . . . . . . . . . . . . . . . . . 29

A.2 General specifications for pololu H-bridge [13]. . . . . . . . . . . . . . . 30

B.1 Schematic of all the electrical components. Made using Fritzing. . . . . 32

List of Figures

Nomenclature

Name Description
m The mass of the structure
I The moment of inertia of the structure
θ Angular diviation from equilibrium point
l Height of the center of mass
Mmotor torque applied by motor
ω Angular velocity of reaction wheel
k2 φ Torque constant of motor
RA Terminal resistance of motor
U Voltage to the motors
θIM U Angle of IMU
dx Set point
T Time period between the updates of PWM signal
L Feedback gain vector
k Gain for automatic set point

1
 List of Figures

List of Abbreviations

Abbreviations Description
DOF Degrees of Freedom
IMU Inertial measurement unit
MEMS microelectromechanical systems
PWM Pulse Width Modulation
ODE Ordinary differential equation
RPM Revolutions per minute

2
Chapter 1

Introduction

1.1 Background
The problem of balancing an inverted pendulum is a classical problem in the field
of dynamics and control theory. The problem arises for example when dealing with
rockets or self balancing robots. One method to balance the inverted pendulum is
with reaction wheels.

A reaction wheel is a flywheel that is designed to give a reaction torque when

torque is applied to it. Reaction wheels are primarily used in spacecrafts for atti-
tude control, eliminating the need for thrusters. The advantage of reaction wheels
is that they can apply a torque to a system in an environment where there is no
external structure or medium to provide a torque.

1.2 Purpose
The Purpose of the project is to use reaction wheels to stabilize an inherently
unstable system. In this case the system is a two DOF inverted pendulum, when
left alone the pendulum will fall. The objective is to keep the pendulum standing.
By using motors to apply torque on the reaction wheels reaction forces will arise
which could keep the pendulum standing. This thesis seeks to answer:

• In what way is it possible to construct a functioning demonstrator with the

resources available?
• What are the most important parameters in order to keep the system stable
over time?

1.3 Scope
Even though the dynamics of the system is nonlinear this thesis only considers linear
control methods. There are several types of controllers that could work, but this

3
 CHAPTER 1. INTRODUCTION

thesis investigates only how a state space controller performs. A micro controller
will be used to control the system. The demonstrator shall not be of a height of
more than 0,4 m and shall carry all components needed except the power supply
for the motors.

1.4 Method
First the dynamics of the system was analysed and a mathematical model was
developed. With the mathematical model of the system, simulations were made in
MATLAB. The simulations were used to test various designs and controllers. With
constrains given by the simulations, components were chosen and gathered. The
demonstrator was built and tests were conducted to determine which parameters
are of the most importance to balance the pendulum.

4
Chapter 2

Theory

2.1 System dynamics

In order to understand the characteristics of the system and to be able to design
an effective control algorithm, a simplified mathematical model of the system was
developed.

2.1.1 Dynamics of the system

The dynamics of the pendulum can be simplified by analysing its dynamics in only
one plane. If the chosen plane is parallel with one of the reaction wheels the system
simplifies to that of an inverted pendulum in one DOF with a single reaction wheel
attached to it. This pendulum can only rotate around one axis. This simplification
can be made since the action of the second reaction wheel, perpendicular to the
plane, should not greatly affect the dynamics of the pendulum in the plane being
analysed.

According to figure 2.1 and Newtons second law, the rotational dynamics of the
pendulum around O can be described as

ez : I θ̈ = mglsin(θ) − Mmotor , (2.1)

where I is the moment of inertia in the z-direction of the pendulum at O, θ is the
angular deviation from the equilibrium point when the pendulum is upright, m is
the mass of the system, l is the distance to the center of gravity and Mmotor is the
torque applied by the motor on the reaction wheel. Mmotor can be thought of as
the reaction torque between the pendulum and the reaction wheel. Therefore the
rotational dynamics of the reaction wheel can be described as

ez : Ir ω̇ = Mmotor , (2.2)
where Ir is the combined moment of inertia of the reaction wheel and the rotor of
the motor and ω is the angular velocity of the reaction wheel and the motor.

5
 CHAPTER 2. THEORY

Figure 2.1. Force analysis of inverted pendulum. Drawn in draw.io.

Knowing that the same equations applies in the yz-plane the model should be
sufficiently accurate for the purpose to stabilize the pendulum.

2.1.2 Motor dynamics

The motors used in this project were a pair of brushed DC motors. A relation
between the angular velocity of a motor, the voltage applied on the motor and the
motor torque can be stated as

k2 φ k2 φ2 ω
Mmotor = U− , (2.3)
RA RA
where k2 φ is the torque constant, RA is the terminal resistance and U is the voltage
applied on the motor[1].

2.1.3 State space model

To simplify the analysis of the system and the design of the controller, the system
dynamics can be written as a set of first order linear differential equations. Since
θ can be assumed to be small and sin(θ) ≈ θ for small θ, equation 2.1 can be
linearised as

I θ̈ = mglθ − Mmotor . (2.4)

6
2.2. CONTROL DESIGN

The state variables x1 , x2 and x3 are introduced as x1 = θ, x2 = θ̇ and x3 = ω.

With equation 2.2, 2.3 and 2.4 the dynamics can now be written in the following
form

ẋ(t) = Ax(t) + Bu(t)) (2.5)

y(t) = Cx(t) + Du(t) (2.6)
as
 
0 1 0
    
ẋ1 x1 0
mgR k2φ2     k2 φ 
RA I  x2  + − RA I  u(t)
   0

ẋ2  = 
 I (2.7)
2 k2 φ
ẋ3 0 0 − Rk2AφIr x3 R A Ir
 
h i x1
y(t) = 1 0 0 x2  (2.8)
 
x3
which is the state space model of the system. Here, u(t) is the input variable, or
the voltage in this case, and y(t) is the output variable, or the state that is to be
controlled, which in this case is θ.

2.2 Control design

The system itself is not asymptotically stable, which can be verified by computing
the eigenvalues of matrix A. If the real part of the eigenvalues are strictly negative
the system is asymptotically stable [2]. To change the dynamics of the system and
make it asymptotically stable, active feedback can be used by letting the voltage U
depend on the state variables.

In this project a state space feedback controller with integral action was chosen
as the controller. Integral action makes the system more robust to modelling errors
and disturbances [2]. A new state variable x4 is introduced to denote the integrator
state which is the integral of the output error. Let r denote the reference or the
wanted output, then the following holds:

ẋ4 = r − y = r − Cx. (2.9)

The system becomes a closed-loop system as the states are fed back to form the
input as  
x1
h i x 
u(t) = −Lx = − L1 L2 L3 L4  2  , (2.10)
 
x3 
x4
where the vector L contains the feedback gain. All states can be measured. The
resulting system now becomes

7
 CHAPTER 2. THEORY

" # " #" # " # " #

ẋ A 0 x B 0
= + u+ r (2.11)
ẋ4 −C 0 x4 0 1

and with the feedback

" # " # " # !" # " #

ẋ A 0 B x 0
= − L + r. (2.12)
ẋ4 −C 0 0 x4 1

By choosing L, the poles of the system can be placed as desired. Since the system
is asymptotically stable, it will converge to a static state that satisfies r − Cx = 0.
In other words x1 = r. In this case r = 0 since the pendulum shall be balancing
around the equilibrium point where θ = 0.

2.2.1 Simulation and Pole Placement

Simulation and pole placement were done in MATLAB. The code can be seen in
appendix C.1. The system dynamics is given by equation 2.12. The system of
differential equations can for example be solved numerically with a Runge-Kutta
method. A fourth ordered Runge-Kutta method was implemented in MATLAB. By
building the ODE solver and not using one of MATLABs predifined ODE solvers,
things like maximum voltage and anti windup could more easily be accounted for.
The simulation gives the behaviour of all the states, the torque applied by the
motors and the input voltage. Figure 2.2 and 2.3 show the angular deviation of
the pendulum and the input voltage when some poles with corresponding L were
chosen. Here the initial conditions of the states were chosen to be 0,1 rad for the
angular deviation of the pendulum and zero for the rest.

Figure 2.2. Simulation of angular deviation of the pendulum. Made in MATLAB.

8
2.3. SENSORS

Figure 2.3. Simulation of corresponing input voltage. Made in MATLAB.

2.3 Sensors
In order to use a state space controller it is necessary to somehow measure or
estimate the states. In this project the states were measured by using a few different
sensors. The accelerometer and the gyroscope were used to measure the angle and
the angular velocity of the system, and the the Hall effect sensors were used to
measure the angular velocity of the reaction wheels.

2.3.1 Accelerometer
Accelerometers measure the acceleration. To measure the acceleration accelerom-
eters typically use a mass spring system which measures the capacitance. There
might be vibrations in the structure which for the accelerometer in this case are
unwanted noise. The measurement also has a bias which will be constant and can
therefore be mostly removed[3]. The principle of one type of MEMS accelerometer
is shown in figure 2.4.

Figure 2.4. MEMS accelerometer structure [3].

2.3.2 Gyroscope
A gyroscope measures the angular velocity. It normally does this by having a
spinning disc and using the Coriolis force. A cheaper and often a more practical

9
 CHAPTER 2. THEORY

alternative is to use a microelectromechanical system (MEMS) gyroscope. However

because the calculation of the angle requires integration of the angular velocity
measurement, the error of the gyroscope drifts with time [4]. The drifting of the
error is caused by multiple factors and has complex non-linear characteristics [4].

2.3.3 Hall effect sensor

A Hall effect sensor is a device that responds to a magnetic field by generating an

output voltage whose magnitude depends on that of the magnetic field. There are
Hall effect sensors with a built in Schmitt trigger making the output digital. Such
sensors were used to measure the angular velocity of the reaction wheels.

2.4 Kalman filter

Because of the noise that occurs in the MEMS sensors as described in section 2.3
it is of importance to filter the data gathered from the sensors. To achieve this a
Kalman filter was used. The name implies that the Kalman filter works as a filter,
however it works more like an observer. An observer is a system that estimates
unknown states with measured states. The idea of a Kalman filter is to use current
and past sensor readings and to use probability and statistics to minimise the mean
squared errors to estimate a state [5]. It was of importance to not simply filter
the data with for instance a butterworth low-pass filter, since such a filter would
delay the data signal in time, which can cause the system to become unstable or
overshoot when changing rapidly. If the measurements are very delayed the state
space controller reacts too slow to the change in angle and thereby making the
system more unstable. For a more exact mathematical description on Kalman filter
see [5].

2.5 Controlling motors

To control the motors it was necessary to be able to control the voltage given to the
motors. This can be achieved by sending a Pulse Width Modulation (PWM) signal
to an H-bridge. A PWM signal is, as the name implies, a signal consisting of pulses
of high and low. By changing the pulse width, or the portion of the time where
the signal is high, the mean value of the signal changes. The PWM signal opens
and closes switches in the H-bridge that control if current from the external power
supply can flow through. Using this technique the output voltage to the motors can
be controlled. Figure 2.5 shows 2 different PWM signals and the resulting average
voltage.

10
2.5. CONTROLLING MOTORS

Figure 2.5. Two examples of PWM signals [6]

An H-bridge is a circuit with transistors making it possible to switch the polarity

of the output voltage. This way the motors could be controlled in both directions.
Figure 2.6 shows a circuit of an H-bridge.

Figure 2.6. A simple H-bridge circuit [7]

11
Chapter 3

Design and implementation

This chapter describes the design of the demonstrator, the relevant hardware that
was used and the reasoning behind them. It also considers the software implementa-
tion. The simulation model in MATLAB with data from the CAD model indicated
how the demonstrator should be designed to function as desired. Due to cost limi-
tations and components available our ideal design could not be achieved and some
compromises had to be made.

3.1 Motors
Simulations were done in order to find approximate requirements for the motors.
The system performance is largely dependent on its mass and the power that the
motors can deliver, and since the power a motor can deliver is proportional to its
mass, there was no easy way to calculate the exact requirements on the specifications
of the motors. Here the mathematical model of the system dynamics that was built
was crucial. It made it possible to simulate the system behaviour with various
parameter values. A motor with high power to weight ratio could improve the
system performance. It was estimated that a motor that can deliver around 0,2
Nm of torque and with the mass of about 0,4 kg could stabilize the system. One
of the motors available with similar specifications was a motor manufactured by
Dunkermotoren of model GR 42x40. The mass of this model is 0,49 kg and it
can deliver 0,33 Nm during stall torque. It was found in the simulations that the
pendulum should be able to balance using this motor. The data sheet for the GR
42x40 motor can be seen in appendix A, figure A.1.

3.1.1 Motor driver

Since the Arduino is not powerful enough to drive the motors, an external power
supply had to be used. To control the polarity and the size of the voltage supplied
to the motors, a dual H-bridge was used. The aim was to be able to supply the
motors with 24 V which could draw as much as 4 A for short periods of time. From

13
 CHAPTER 3. DESIGN AND IMPLEMENTATION

these conditions the H-bridge Pololu Dual VNH5019 Motor Driver was chosen. This
H-bridge can operate at 24 V and deliver a continuous current of 12 A per channel,
which is sufficient for the purpose. The VNH5019 also features PWM operation up
to 20 kHz which enables quieter and smoother running of the motors[8]. General
specifications for the H-bridge taken from the Pololu website can be seen in appendix
A figure A.2.

3.2 Measuring motor speed

The angular velocity of the motors can be measured using Hall effect sensors that
detect magnets which are attached on the reaction wheels. In order to tell which
direction the motors are rotating, two Hall effect sensors per motor were used. The
figure below illustrates how the direction is determined.

Figure 3.1. Illustration of how to measure direction. Drawn in draw.io.

When sensor 1 detects a magnet the system checks if sensor 2 is detecting a magnet.
If so, the magnets are moving to the right, otherwise the magnets are moving to
the left. By counting detections and measuring the time difference between the
detections the angular velocity can be calculated.

Figure 3.2. Plot of measured angular velocity of reaction wheel. Made in MATLAB.

14
3.3. INERTIAL MEASUREMENT UNIT

Figure 3.2 shows the measured angular velocity of one of the reaction wheels where
the motor was given 24 volts for seven seconds, -24 volts for seven seconds and then
zero volts until the angular velocity became zero.

3.3 Inertial measurement unit

For measuring the angle a MEMS gyroscope and accelerometer was used. The
chosen sensor was the MPU-6050. As described in chapter 2 these measurements
have both noise and drift, therefore a Kalman filter was used. To implement the
Kalman filter a library and example code made by Kristian Sloth Lauszus was used
[9].

Figure 3.3. The angle as given by the accelerometer, gyroscope and Kalman filter.
Made in MATLAB.

Figure 3.3 shows the angle as given by the accelerometer, gyroscope and Kalman
filter. It can be seen that the accelerometer had a lot of noise and that the gyroscope
drifted with time as stated in section 2.3. The Kalman filtered data has low noise
and is not time delayed as some filters can cause, making it suitable for use in the
controller. Note that figure 3.3 shows two 90◦ turns of the system. In reality the
the demonstrator will only move a few degrees making the value given directly by
the accelerometer useless because of the noise to signal ratio.

3.4 Construction
It was decided that a platform to hold the components and the stick together would
be 3D-printed. The aim of the platform was to get the system as compact as possi-
ble and the weight close to the stick to get the center of mass approximately above
it. It was also of importance to fasten the motors to reduce vibrations caused by

15
 CHAPTER 3. DESIGN AND IMPLEMENTATION

them. The design also features an adjustable height of the platform since there was
uncertainties about which height that would work. The stick chosen was a beam of
steel with a radius of 2 mm.

The reaction wheels were designed to have low mass while simultaneously keeping a
high moment of inertia. They were designed in Solid Edge and then manufactured
by using a water cutter to cut out the silhouette out of a sheet of steel, and then
milling material away from the inner parts to lower the mass while keeping a high
moment of inertia. The whole structure was then put together with screws and set
screw hubs as shown in figure 3.4 where the CAD model is seen. All of the electrical
components were also put on the platform and connected as the schematics figure
B.1 illustrates. Figure 3.5 shows the finished demonstrator while balancing. Here
the lower side of the platform is placed on a height of 10 cm and the diameter of
the reaction wheels is 15 cm.

Figure 3.4. Cad model. Made in Solid Edge ST10.

16
3.5. CODE

Figure 3.5. The finished demonstrator while balancing.

3.5 Code
An Arduino Uno was used as the controller unit due to its ease of use and in-
expensiveness. The code was written in Arduinos IDE and then uploaded to the
Arduino Uno. A simplification of the logical flow of the program is illustrated in the
flowchart in figure 3.6. In the main loop the data from the IMU is updated every
iteration, while the angular velocity of the reaction wheel is only recalculated and
updated when the hall sensors have detected more than three magnets going in the
same direction. This means the calculated angular velocity is the mean value of the
angular velocity during the last three detections whenever the angular velocity is
updated. Accordingly there are delays in this data and it could be more precise by
adding more magnets.

The PWM signal is updated when the time T has elapsed since the last time the
PWM signal was updated. When the PWM signal is updated a function is called
that takes the states as input and returns the voltage that should be sent to the
motor. The function basically uses equation 2.10 (u = −Lx) to calculate the volt-
age, but with the restriction of setting the voltage to 24 V if equation 2.10 gives a
voltage that exceeds 24 V. It also integrates the error to form the integrator state
as according to equation 2.9. Though if the voltage exceeds 24 V the input signal is
saturated and no integration is done. This is refereed to as an anti windup method
and it keeps the integrator from growing when the actuator is saturated and can not

17
 CHAPTER 3. DESIGN AND IMPLEMENTATION

produce the wanted output. Anti windup prevents large overshoot and oscillations
in the output signal of the system [10].

Figure 3.6. Flowchart. Drawn in draw.io.

3.6 Tuning of the controller

In the simulation several different poles where tried with corresponding feedback
gain L. These L where implemented in the code. After several tests it became clear
which L worked best.

3.6.1 Automatic set point

The state space controller as described in section 2.1.3 relies on that the center of
mass is located at θ = 0 and that the IMU is attached to the structure correctly.
Since the center of mass is not exactly at θ = 0 and the IMU is a not completely

18
3.6. TUNING OF THE CONTROLLER

aligned to the line of center of mass, the data has to be compensated for this. This
is done by subtracting a small angle dx to the angle read by the IMU θIM U before
it goes into the controller. This means the controller will try to get θIM U = dx, and
therefore dx can be interpreted as the set point. dx compensates for the slanting
IMU and the center of mass not being precisely above the stick. If this compensation
is not exactly correct the stabilisation will fail after a short time. The system
will try to regulate itself to a state where the center of mass is not above the
interface between the ground and the stick. This will result in a small torque
constantly pulling in one direction and the reaction wheels will constantly have to
accelerate to keep up for this. The motors will build up speed until they can no
longer apply any torque. This problem can be solved by changing dx over time as
dxn −θIM U
dxn+1 = dxn + k |dx n −θIM U |
, where k is some positive gain. This means dx or the set
point will increase if dx > θIM U and decrease if dx < θIM U . If k is chosen so that
the set point change a bit slower than θIM U it solves the problem of not knowing
the correct set point. The set point will automatically go towards a good value. See
figure 3.7 where the stabilisation starts at time=0.

Figure 3.7. Shows how the set point moves compered to the angle while stabilising.
Made in MATLAB.

A mathematical proof for that this should work could not be found. But there are
some similar work where similar solutions were briefly discussed [11].

19
Chapter 4

Results

When implementing the state space controller on its own the system never seemed
to be able to balance over time. The pendulum could stand up for a while but
always fell after tens of seconds or at best after some minutes. We strongly suspect
that the problem was because of the set point, which is discussed in chapter 3.6.
After the implementation of the automatic set point, the system became seemingly
stable. There was no sign that the system could not balance indefinitely when no
disturbances were applied. Figure 4.1 shows the angle and angular velocity of the
IMU while stabilising.

Figure 4.1. Shows the angle and angular velocity of the IMU while stabilising.
Made in MATLAB.

The demonstrator could handle disturbances such as someone pushing slightly on its
platform. Also the automatic set point made the system able to handle changes to
the center of mass while balancing. Figure 4.2 shows the behaviour when a mass is
put near one of the platforms edges. It illustrates how the system automatically finds
the new angle it should stabilize around. In the end L = [−550 −60 −0, 0585 1000],
k = 0, 00003 and T = 0, 001 seconds were considered to give the best performance.
This is while the platform was placed on a height of 10 cm. Large vibrations started

21
 CHAPTER 4. RESULTS

to occur when the platform was placed a couple of centimetres higher.

Figure 4.2. Shows how the set point and the angle moved when the center of mass
was changed while stabilizing. Made in MATLAB.

22
Chapter 5

Discussion and conclusion

5.1 Discussion

In order to acquire data to make the graphs in this report the arduino had to send
data to the computer. Since the arduino did not have wireless transmission built in
nor was this added, this data transfer went through a cable. Because of the rather
thick cable the system was not optimal when collecting data. The cable caused dis-
turbances by pulling on the system. This was however not the greatest disturbance
to the system while sending data. When the Arduino sent data there were delays
causing the code to slow down. This made the system more unstable and caused
an increased swaying movement that can be seen in figure 3.7, 4.1 and 4.2. To deal
with this effect data was sent less often to the computer. It made the system more
stable but reduced the resolution of the data shown in figure 3.7, 4.1 and 4.2.

The height of the platform proved to be an important factor to make the sys-
tem stable. This was predicted with the mathematical model of the system. What
was not as anticipated were the large vibrations that occurred when the the height
of the platform was increased. The objective was to put the platform a bit higher,
but it turned out that the vibrations made this very difficult. Apart from the stick
being to weak, the reaction wheels were not perfectly symmetrical and this might
have caused unnecessary excitement to the stick making it vibrate.

The first feedback gain L that was implemented in the micro controller was accord-
ing to the simulations enough to make the system stable. It was then discovered
that an L that was predicted to give a much faster system was superior. This was
probably due to the delays that occurred in the real system that had not been ac-
counted for in the simulations. Also the mathematical model was an approximation
and was not expected to describe the system perfectly.

23
 CHAPTER 5. DISCUSSION AND CONCLUSION

5.2 Conclusion
Several parameters such as the mass, the height of the center of mass and the
moment of inertia of the reaction wheels are of importance to get the system stable.
As expected it is important to implement a good controller and especially a correct
set point so that the system does not try to stabilize towards a wrong angle. While
methods for finding a good set point can be developed, the system is still very
sensitive to changes in the center of mass and may still fail to balance after some
time. In order to make the system stable over time an automatic set point was
implemented as described in 3.6.1, which makes the system stable indefinitely.

24
Chapter 6

Recommendations and future work

6.1 Recommendations
Even if the demonstrator worked it had some flaws and there are room for im-
provements. There were resonance frequencies in the system when the height of
the platform was increased. For future work it would be of importance to use a
stiffer stick for the demonstrator. It would also be recommended to use reaction
wheels that are symmetrical as to not cause any vibrations. Finally it would be rec-
ommended to use a rotary encoder to measure the angular velocity of the reaction
wheels with higher precision.

6.2 Future work

Future work can for example be to investigate how high the system could be made
and still be stable. One could also look at how noisy or biased state measurements
effect the system. It would be interesting to investigate more profoundly how the
system handles disturbances and see how robust the system is. It would also be of
interest to attempt to integrate the power supply on the pendulum and make the
system independent of outer facilities.

25
Bibliography

[1] H Johansson. Elektroteknik. KTH, Department of Machine Design, Mecha-

tronic.
[2] L Glad T. Ljung. Reglerteknik. Studentlitteratur AB.
[3] O.J. Woodman. An introduction to inertial navigation. Tech. rep. University
of Cambridge.
[4] C.-X. Shiau J.-K. Huang and M.-Y Chang. “Noise Characteristics of MEMS
Gyro’s Null Drift and Temperature Compensation.” In: Journal of applied
Science and Engineering 15.3 (2012), pp. 239–246.
[5] G Welch G. Bishop. An Introduction to the Kalman Filter. Tech. rep. Univer-
sity of North Carolina at Chapel Hill, Department of computer Science.
[6] Pulse Width Modulation. url: https://www.electronics-tutorials.ws/
blog/pulse-width-modulation.html (visited on 05/01/2019).
[7] What is an H-Bridge? url: https://www.build- electronic- circuits.
com/h-bridge/ (visited on 05/01/2019).
[8] H Amlinger. Reduction of Audible Noise of a Traction Motor at PWM Oper-
ation. Tech. rep. KTH.
[9] K Lauszus. Kalman Filter. url: https : / / github . com / TKJElectronics /
KalmanFilter (visited on 05/01/2019).
[10] Erik Torstensson. Comparison of Schemes for Windup Protection. Tech. rep.
Lund University Department of Automatic Control.
[11] P Brevik. Two-Axis Reaction Wheel Inverted Pendulum. Tech. rep. Norwegian
University of Science and Technology.
[12] GR 42x40. url: https://www.dunkermotoren.com/uploads/tx_products/
downloads/MKS/gr-42x40-8842702010.pdf (visited on 05/07/2019).
[13] Pololu Dual VNH5019 Motor Driver Shield for Arduino. url: https://www.
pololu.com/product/2507/specs (visited on 05/07/2019).

27
Appendix A

Datasheets

Figure A.1. Datasheet for the motor [12].

29
 APPENDIX A. DATASHEETS

Figure A.2. General specifications for pololu H-bridge [13].

30
31
 APPENDIX B. SCHEMATICS

Appendix B

Schematics

Figure B.1. Schematic of all the electrical

32 components. Made using Fritzing.
Appendix C

Code

C.1 MATLAB code

33
statespace_motor_integrator.m
% Project name: Reaction Wheel Stabilized Stick
% Bachelor thesis in mechatronics at KTH, spring 2019
% Bill Lavebratt, Pontus Gräsberg
% Description of code:
% Matlab program for simulation of the system. Needs the function
RK4systk.m
% It predicts how the system will behave when implementing a State
space
% controller

clear all, clc, close all

m=1.75; % 1.734; % Mass of the system, from cad model
R=0.14; % Distance from interface between stick and
ground to the center of mass, from cad model

I= 350/100^2; % Moment of inertia of the system, at the

interface between stick and ground, from Cad model
Ihh=10.196/100^2; % Moment of inertia of the reaction wheel,
from Cad model
Im=110/(1000*100^2); % Moment of inertia of the rotor in motor,
from datasheet of motor
Ih=Ihh+Im; % Combined moment of inertia of the reaction
wheel and motor
g=9.82; % Gravitational acceleration

k2fi=5.84/100; % Torque constant of motor, from datasheet of

motor
Ra=4.2; % Terminal resistance, from datasheet of motor

% State space model

A=[0 1 0;
m*g*R/I 0 k2fi^2/(I*Ra);
0 0 -k2fi^2/(Ih*Ra)];

B=[0; -k2fi/(I*Ra); k2fi/(Ih*Ra)];

C=[1 0 0];

% State space model when imlpementing the integrator

Any=[A zeros(size(A,1),1);-C 0];
Bny=[B;0];
Cny=[C 0];
ref=0; % Set point
D=[zeros(size(A,1),1);1];

Lny = place(Any, Bny, [-2,0,-1+0.5i -1-0.5i]); % Gives the feedback

gain constants in L as a function of the poles of the system.

Amax=[A zeros(size(A,1),1); zeros(1,4)]; % Is used when desired

voltage is larger than the maximum allowed.

1
Umax=24; % Maximum voltage

% System of differential equations of the system with controller and

anti windup, accounts for Umax.
f=@(tvec,yvec) ((((Any-Bny*Lny)*yvec' +ref*D)*(abs(-
yvec(end,:)*Lny')<=Umax))+(Amax*yvec'+Bny*Umax*abs(-yvec(end,:)*Lny')/
(-yvec(end,:)*Lny'))*(abs(-yvec(end,:)*Lny')>Umax)')';

y0=[0.125 0 0 0]; % Initial conditions of the states [Agle, Angular

velocity, Angular velocity of recation wheel, Integraion of the
error]
tspan=[0 10]; % Simulation spans over this time.
h=0.001; % Step length in ODE solver.
[tv,yv]=RK4systk(f,tspan,y0,h); % Solution of differential equation
with implementation of RK4, See code "RK4systk.m "
figure
plot(tv,yv(:,1))
grid on, title('Angle of pendulum \theta'), xlabel('t [s]'),
ylabel('rad');

figure
plot(tv,yv(:,2))
grid on, title('Angular velocity of pendulum'), xlabel('t [s]'),
ylabel('rad/s');

figure
plot(tv,yv(:,4))
grid on, title('Integral(ref-angle)'), xlabel('t [s]');

figure
plot(tv,yv(:,3))
grid on, title('Angular velocity of reaction wheel'), xlabel('t [s]'),
ylabel('rad/s');

% The voltage that was sent to the motor

U=-yv*Lny';
for ii=1:length(U)
if abs(U(ii))>Umax;

U(ii)=Umax*U(ii)/abs(U(ii));
end
end

M=@(w,Ua) Ua*k2fi/Ra-(k2fi)^2/Ra*w; % Equation of the motor

Mmotor=M(yv(:,3),U); % The torque the motor gave

figure
plot(tv,U)
grid on, title('Voltage'), xlabel('t [s]'), ylabel('V');

figure
plot(tv,Mmotor)
grid on, title('Torque from motor'), xlabel('t [s]'), ylabel('Nm');

2
RK4systk.m
% Project name: Reaction Wheel Stabilized Stick
% Bachelor thesis in mechatronics at KTH, spring 2019
% Bill Lavebratt, Pontus Gräsberg
% Description of code:
% Matlab function for simulation of the system.
% Implementation of a Runge-Kutta 4th Order method for System of
differential equations.
% The input is f: function of system of differential equations, tspan:
% start time and end time of simulation, y0: Initial conditions, h:
% steplength.
% The output is tv: the time vector, yv: matrix of the solution to the
% system of differential equations.

function [tv,yv]=RK4systk(f,tspan,y0,h)

n=(tspan(2)-tspan(1))/h;
tv=(tspan(1)+h*(0:n))';
p=length(y0);
yv=zeros(n+1,p);
yv(1,:)=y0;

for ii=1:n
k1=feval(f,tv(ii),yv(ii,:));
k2=feval(f,tv(ii)+h/2,yv(ii,:)+h/2*k1);
k3=feval(f,tv(ii)+h/2,yv(ii,:)+h/2*k2);
k4=feval(f,tv(ii)+h,yv(ii,:)+h*k3);
yv(ii+1,:)=yv(ii,:)+h/6*(k1+2*k2+2*k3+k4);
end
end

Published with MATLAB® R2017b

1
C.2. ARDUINO CODE

C.2 Arduino code

/∗
∗ University : Royal I n s t i t u t e o f Technology , KTH
∗ TRITA number : TRITA−ITM−EX 2 0 1 9 : 3 8
∗ Authors : B i l l L a v e b r a t t and Pontus G r s b e r g
∗ Name o f p r o j e c t : R e a c t i o n wheel s t a b a l i z e d s t i c k
∗
∗ D e s c r i p t i o n o f code :
∗ Main program used t o b a l a n c e t h e r e a c t i o n wheel s t a b a l i z e d s t i c k .
∗ F i l t e r e d s t a t e measurements g o e s i n t o a c o n t r o l e r / r e g u l a t o r f u n c t i o n
t h a t c a l c u l a t e s t h e d e s i r e d v o l t a g e t o t h e motors .
∗ Then t h e c o r e s p o n d i n g PWM s i g n a l s i s s e n t t o an H−b r i d g e .
∗/

#i n c l u d e <PWM. h> // l i b r a r y f o r change i n pwm f r e q

// S o u r c e : h t t p s : / / g i t h u b . com/RCS101/PWM

#i n c l u d e < f i l t e r s . h> // L i b r a r y f o r Butterworth f i l t e r

#i n c l u d e < f i l t e r s _ d e f s . h>
// S o u r c e : h t t p s : / / g i t h u b . com/ MartinBloedorn / l i b F i l t e r

#i n c l u d e <Wire . h>
#i n c l u d e <Kalman . h> // S o u r c e : h t t p s : / / g i t h u b . com/ T K J E l e c t r o n i c s /
KalmanFilter

#d e f i n e RESTRICT_PITCH // Comment out t o r e s t r i c t r o l l to 90deg

instead

Kalman kalmanX ; // C r e a t e t h e Kalman i n s t a n c e s

Kalman kalmanY ;

f l o a t BY2 ; // C r e a t e v a r i b l e f o r Butterworth f i l t e r o f g y r o r a t e
f l o a t BX2 ;

int i ;
int i i ;
f l o a t dx1 ;
f l o a t dy1 ;

const f l o a t cutoff_freq = 5 . 0 ; // C u t o f f f r e q u e n c y i n Hz f o r
Butterworth f i l t e r
c o n s t f l o a t sampling_time = 0 . 0 0 5 5 ; // Sampling time i n s e c o n d s .
IIR : : ORDER o r d e r = IIR : : ORDER : : OD3 ; // Order (OD1 t o OD4)

// Low−p a s s f i l t e r
F i l t e r f ( c u t o f f _ f r e q , sampling_time , o r d e r ) ; // Function f o r Butterworth
filter
F i l t e r g ( c u t o f f _ f r e q , sampling_time , o r d e r ) ;

f l o a t g y r o X r a t e 3 ; // V a r i b l e f o r g y r o r a t e g i v e n by MPU
f l o a t gyroYrate3 ;

37
 APPENDIX C. CODE

f l o a t dx = 0 . 0 3 ; // S t a r t i n g s e t p o i n t
f l o a t dy = 0 . 0 0 6 ;

/∗ IMU Data ∗/
d o u b l e accX , accY , accZ ;
d o u b l e gyroX , gyroY , gyroZ ;
i n t 1 6 _ t tempRaw ;

d o u b l e gyroXangle , gyroYangle ; // Angle c a l c u l a t e u s i n g t h e gyro o n l y

d o u b l e kalAngleX , kalAngleY ; // C a l c u l a t e d a n g l e u s i n g a Kalman f i l t e r

uint32_t timer ;
u i n t 8 _ t i 2 c D a t a [ 1 4 ] ; // B u f f e r f o r I2C data

// V a r i b l e s f o c o n t r o l l e r
f l o a t L1=−550; // S t a t e s p a c e f e e d b a c k g a i n
f l o a t L2=−60;
f l o a t L3= −0.0585;
f l o a t L4 =1000;
f l o a t v_ref =0;
f l o a t DT= 0 . 0 0 1 ; //Time p e r i o d between t h e u p d a t e s o f PWM s i g n a l (
Helpful f o r i n t e g r a t o r part )
u n s i g n e d l o n g oldTime =0;

// S t a t e v a r i a b l e s f o r d i r e c t i o n 1
f l o a t v_vink1 ;
f l o a t v_hast1 ;
f l o a t radps1 ;
f l o a t x41 =0;

// S t a t e v a r i a b l e s f o r d i r e c t i o n 2
f l o a t v_vink2 ;
f l o a t v_hast2 ;
f l o a t radps2 ;
f l o a t x42 =0;

// v a r i b l e f o r c o n t r o l l e r
f l o a t Umax=24; // Maximum v o l t a g e
f l o a t U1 ;
f l o a t U2 ;

// P i n s f o r H−b r i d g e
#d e f i n e M1PWM 9
#d e f i n e M1INA 13
#d e f i n e M1INB 12
#d e f i n e M1CS A0
i n t M1EN = 1 1 ;

#d e f i n e M2PWM 10
#d e f i n e M2INA 8
#d e f i n e M2INB 7

38
C.2. ARDUINO CODE

i n t M2EN = 4 ;

i n t 3 2 _ t f r e q =20000; //PWM f r e q

f l o a t pwmOutput1=0; // PWM s i g n a l v a r i b l e s
f l o a t pwmOutput2=0;

float half_revolutions1 ; // Magnet d e t e c t i n g c o u n t e r s

float half_revolutions2 ;

// V a r i a b l e s f o r H a l l s e n s o r
u n s i g n e d l o n g DTH1;
u n s i g n e d l o n g DTH2;
unsigned long timeold1 ;
unsigned long timeold2 ;
i n t i n t e r r u p t P i n 1 1 =2;
i n t i n t e r r u p t P i n 2 1 =3;
i n t drPin12=A0 ;
i n t drPin22=A1 ;

void setup ( ) {
InitTimersSafe () ;
S e t P i n F r e q u e n c y S a f e (M1PWM, f r e q ) ;

// For motor
pinMode (M1PWM, OUTPUT) ;
pinMode (M1INA, OUTPUT) ;
pinMode (M1INB, OUTPUT) ;
pinMode (M1EN, OUTPUT) ;
pinMode (M2PWM, OUTPUT) ;
pinMode (M2INA, OUTPUT) ;
pinMode (M2INB, OUTPUT) ;
pinMode (M2EN, OUTPUT) ;

// S e t i n i t i a l r o t a t i o n d i r e c t i o n
d i g i t a l W r i t e (M1INA, HIGH) ;
d i g i t a l W r i t e (M1INB, LOW) ;
d i g i t a l W r i t e (M1EN, HIGH) ;
d i g i t a l W r i t e (M2INA, HIGH) ;
d i g i t a l W r i t e (M2INB, LOW) ;
d i g i t a l W r i t e (M2EN, HIGH) ;

// For H a l l s e n s o r
// I n i t i a l i z e t h e i n t t e r r u p t p i n ( Arduino d i g i t a l p i n 2 )
attachInterrupt ( digitalPinToInterrupt ( interruptPin11 ) ,
magnet_detect1 , FALLING) ;
// I n i t i a l i z e t h e i n t t e r r u p t p i n ( Arduino d i g i t a l p i n 3 )
attachInterrupt ( digitalPinToInterrupt ( interruptPin21 ) ,
magnet_detect2 , FALLING) ;
pinMode ( drPin12 , INPUT) ;
pinMode ( drPin22 , INPUT) ;

39
 APPENDIX C. CODE

S e r i a l . begin (115200) ;
Wire . b e g i n ( ) ;
#i f ARDUINO >= 157
Wire . s e t C l o c k ( 4 0 0 0 0 0UL) ; // S e t I2C f r e q u e n c y t o 400kHz
#e l s e
TWBR = ( (F_CPU / 400000UL) − 1 6 ) / 2 ; // S e t I2C f r e q u e n c y t o 400
kHz
#e n d i f

i 2 c D a t a [ 0 ] = 7 ; // S e t t h e sample r a t e t o 1000Hz − 8kHz/(7+1) = 1000

Hz
i 2 c D a t a [ 1 ] = 0 x00 ; // D i s a b l e FSYNC and s e t 260 Hz Acc f i l t e r i n g , 256
Hz Gyro f i l t e r i n g , 8 KHz s a m p l i n g
i 2 c D a t a [ 2 ] = 0 x00 ; // S e t Gyro F u l l S c a l e Range t o 250deg / s
i 2 c D a t a [ 3 ] = 0 x00 ; // S e t A c c e l e r o m e t e r F u l l S c a l e Range t o 2g
w h i l e ( i 2 c W r i t e ( 0 x19 , i2cData , 4 , f a l s e ) ) ; // Write t o a l l f o u r
r e g i s t e r s a t once
w h i l e ( i 2 c W r i t e ( 0 x6B , 0 x01 , t r u e ) ) ; // PLL with X a x i s g y r o s c o p e
r e f e r e n c e and d i s a b l e s l e e p mode
w h i l e ( i2cRead ( 0 x75 , i2cData , 1 ) ) ;
i f ( i 2 c D a t a [ 0 ] != 1 1 4 ) { // Read "WHO_AM_I" r e g i s t e r
S e r i a l . p r i n t (F( " E r r o r r e a d i n g s e n s o r " ) ) ;
while (1) ;
}

d e l a y ( 1 0 0 ) ; // Wait f o r s e n s o r t o s t a b i l i z e

/∗ S e t kalman and gyro s t a r t i n g a n g l e ∗/

w h i l e ( i2cRead ( 0 x3B , i2cData , 6 ) ) ;
accX = ( i n t 1 6 _ t ) ( ( i 2 c D a t a [ 0 ] << 8 ) | i 2 c D a t a [ 1 ] ) ;
accY = ( i n t 1 6 _ t ) ( ( i 2 c D a t a [ 2 ] << 8 ) | i 2 c D a t a [ 3 ] ) ;
accZ = ( i n t 1 6 _ t ) ( ( i 2 c D a t a [ 4 ] << 8 ) | i 2 c D a t a [ 5 ] ) ;

// S o u r c e : h t t p : / /www. f r e e s c a l e . com/ f i l e s / s e n s o r s / doc / app_note /AN3461

. pdf eq . 25 and eq . 26
// atan2 o u t p u t s t h e v a l u e o f − to ( r a d i a n s ) − s e e h t t p : / / en .
w i k i p e d i a . o r g / w i k i / Atan2
// I t i s then c o n v e r t e d from r a d i a n s t o d e g r e e s
#i f d e f RESTRICT_PITCH // Eq . 25 and 26
d o u b l e r o l l = atan2 ( accY , accZ ) ∗ RAD_TO_DEG;
d o u b l e p i t c h = atan (−accX / s q r t ( accY ∗ accY + accZ ∗ accZ ) ) ∗
RAD_TO_DEG;
#e l s e // Eq . 28 and 29
d o u b l e r o l l = atan ( accY / s q r t ( accX ∗ accX + accZ ∗ accZ ) ) ∗
RAD_TO_DEG;
d o u b l e p i t c h = atan2 (−accX , accZ ) ∗ RAD_TO_DEG;
#e n d i f

kalmanX . s e t A n g l e ( r o l l ) ; // S e t s t a r t i n g a n g l e
kalmanY . s e t A n g l e ( p i t c h ) ;
gyroXangle = r o l l ;
gyroYangle = p i t c h ;

40
C.2. ARDUINO CODE

timer = micros ( ) ;

//MAIN LOOP
void loop ( ) {

MPU_Kalman( ) ; // Read data g i v e n by Kalman f i l t e r

// C a l c u l a t e s a n g u l a r v e l o c i t y [ rad / s ] f o r H a l l e f f e c t s e n s o r s
i f ( abs ( h a l f _ r e v o l u t i o n s 1 ) >3) {
DTH1 = ( m i c r o s ( ) − t i m e o l d 1 ) ;
r a d p s 1 = −3.1416∗1000000∗( h a l f _ r e v o l u t i o n s 1 /DTH1) / 2 . 0 ;
half_revolutions1 = 0;
timeold1 = micros () ;

}
i f ( abs ( h a l f _ r e v o l u t i o n s 2 ) >3) {
DTH2 = ( m i c r o s ( ) − t i m e o l d 2 ) ;
r a d p s 2 = −3.1416∗1000000∗( h a l f _ r e v o l u t i o n s 2 /DTH2) / 2 . 0 ;
half_revolutions2 = 0;
timeold2 = micros () ;

// C a l c u l a t e s new v o l t a g e when t h e time T has e l a p s e d s i n c e l a s t time

t h e v o l t a g e was c a l c u l a t e d
i f ( ( m i c r o s ( )−oldTime ) >(DT∗ 1 0 0 0 0 0 0 ) ) {

dx1=(−v_vink1 ) / abs ( v_vink1 ) ; // Change s e t p o i n t

dx=dx +0.00003∗ dx1 ;

dy1=(−v_vink2 ) / abs ( v_vink2 ) ;

dy=dy +0.00003∗ dy1 ;

// C a l c u l a t e new v o l t a g e with t h e c o n t r o l l e r / r e g u l a t o r f u n c t i o n t h a t
t a k e s t h e s t a t e s and r e t u r n s t h e d e s i r e d v o l t a g e .
U1=r e g u l a t o r ( v_vink1 , v_hast1 , radps1 ,& x41 ) ; // V o l t a g e f o r motor1
U2=r e g u l a t o r ( v_vink2 , v_hast2 , radps2 ,& x42 ) ; // V o l t a g e f o r motor2
oldTime=m i c r o s ( ) ;

// S e t v o l t a g e t o z e r o i f some x o r y a n g l e i s more than 20

degrees ,
// s i n c e t h e system c a n o t be s t a b i l i z e d i n t h a t c a s e .
i f ( abs ( v_vink1 ∗ 1 8 0 . 0 / 3 . 1 4 1 5 ) >20 | | abs ( v_vink2 ∗ 1 8 0 . 0 / 3 . 1 4 1 5 ) >20){
U1=0;
U2=0;
}

// Sens d e s i r e d v o l t a g e t o f u n c t i o n s t h a t s e t s t h e PWM s i g n a l
corresponding to the d e s i r e d vol tage
motor1pwm (U1) ;

41
 APPENDIX C. CODE

motor2pwm (U2) ;

}
}

//FUNKCTIONS BELOW

// R e g u l a t o r f u n c t i o n t h a t t a k e s t h e s t a t e s and r e t u r n s t h e v o l t a g e
f l o a t r e g u l a t o r ( f l o a t v_vink , f l o a t v_hast , f l o a t s_hast , f l o a t ∗ x4 ) {
f l o a t temp=∗x4 ;
( ∗ x4 ) =(∗x4 ) +(v_ref−v_vink ) ∗DT; // i n t e g r a t o r p a r t o f c o n t r o l l e r
f l o a t UU;

UU=−(L1∗ v_vink+L2∗ v_hast+L3∗ s_hast+L4 ∗ ( ∗ x4 ) ) ;

// When t h e d e s i r e d v o l t a g e i s t o l a r g e
i f ( abs (UU)>Umax) {
UU=Umax∗ abs (UU) /UU;
( ∗ x4 )=temp ; // a n t i windup
}

r e t u r n UU;
}

// Function t h a t s e t s t h e PWM s i g n a l c o r r e s p o n d i n g t o t h e d e s i r e d
voltage
v o i d motor2pwm ( f l o a t s p a n n i n g 1 ) {

i f ( spanning1 >=0){ // Decide t h e d i r e c t i o n o f t h e

current
d i g i t a l W r i t e (M1INA, HIGH) ;
d i g i t a l W r i t e (M1INB, LOW) ;
d i g i t a l W r i t e (M1EN, HIGH) ;
}
else{
d i g i t a l W r i t e (M1INA, LOW) ;
d i g i t a l W r i t e (M1INB, HIGH) ;
d i g i t a l W r i t e (M1EN, HIGH) ;
s p a n n i n g 1=abs ( s p a n n i n g 1 ) ;
}
pwmOutput1 = map( spanning1 , 0 , Umax, 0 , 2 5 5 ) ; // Map t h e
p o t e n t i o m e t e r v a l u e from 0 t o 255
pwmWrite (M1PWM, pwmOutput1 ) ;
}

// Function t h a t s e t s t h e PWM s i g n a l c o r r e s p o n d i n g t o t h e d e s i r e d
voltage
v o i d motor1pwm ( f l o a t s p a n n i n g 2 ) {

i f ( spanning2 >=0){ // Decide t h e d i r e c t i o n o f t h e

current
d i g i t a l W r i t e (M2INA, HIGH) ;
d i g i t a l W r i t e (M2INB, LOW) ;
d i g i t a l W r i t e (M2EN, HIGH) ;
}

42
C.2. ARDUINO CODE

else{
d i g i t a l W r i t e (M2INA, LOW) ;
d i g i t a l W r i t e (M2INB, HIGH) ;
d i g i t a l W r i t e (M2EN, HIGH) ;
s p a n n i n g 2=abs ( s p a n n i n g 2 ) ;
}
pwmOutput2 = map( spanning2 , 0 , Umax, 0 , 2 5 5 ) ; // Map t h e
p o t e n t i o m e t e r v a l u e from 0 t o 255
pwmWrite (M2PWM, pwmOutput2 ) ;
}

// This f u n c t i o n i s c a l l e d whenever a magnet / i n t e r r u p t i s d e t e c t e d by

H a l l s e n s o r 1 , c o u n t s t h e d e t e c t i o n s o f magnets
v o i d magnet_detect1 ( )
{
i f ( d i g i t a l R e a d ( drPin12 )==LOW) {
h a l f _ r e v o l u t i o n s 1 ++;
}
else {
h a l f _ r e v o l u t i o n s 1 −−;
}
}

// This f u n c t i o n i s c a l l e d whenever a magnet / i n t e r r u p t i s d e t e c t e d by

H a l l s e n s o r 2 , c o u n t s t h e d e t e c t i o n s o f magnets
v o i d magnet_detect2 ( )
{
i f ( d i g i t a l R e a d ( drPin22 )==LOW) {
h a l f _ r e v o l u t i o n s 2 ++;
}
else {
h a l f _ r e v o l u t i o n s 2 −−;
}
}

//Kalman f u c t i o n made by Lauszus

v o i d MPU_Kalman( ) {
/∗ Update a l l t h e v a l u e s ∗/
w h i l e ( i2cRead ( 0 x3B , i2cData , 1 4 ) ) ;
accX = ( i n t 1 6 _ t ) ( ( i 2 c D a t a [ 0 ] << 8 ) | i 2 c D a t a [ 1 ] ) ;
accY = ( i n t 1 6 _ t ) ( ( i 2 c D a t a [ 2 ] << 8 ) | i 2 c D a t a [ 3 ] ) ;
accZ = ( i n t 1 6 _ t ) ( ( i 2 c D a t a [ 4 ] << 8 ) | i 2 c D a t a [ 5 ] ) ;
tempRaw = ( i n t 1 6 _ t ) ( ( i 2 c D a t a [ 6 ] << 8 ) | i 2 c D a t a [ 7 ] ) ;
gyroX = ( i n t 1 6 _ t ) ( ( i 2 c D a t a [ 8 ] << 8 ) | i 2 c D a t a [ 9 ] ) ;
gyroY = ( i n t 1 6 _ t ) ( ( i 2 c D a t a [ 1 0 ] << 8 ) | i 2 c D a t a [ 1 1 ] ) ;
gyroZ = ( i n t 1 6 _ t ) ( ( i 2 c D a t a [ 1 2 ] << 8 ) | i 2 c D a t a [ 1 3 ] ) ; ;

d o u b l e dt = ( d o u b l e ) ( m i c r o s ( ) − t i m e r ) / 1 0 0 0 0 0 0 ; // C a l c u l a t e d e l t a
time
timer = micros ( ) ;

// S o u r c e : h t t p : / /www. f r e e s c a l e . com/ f i l e s / s e n s o r s / doc / app_note /AN3461

. pdf eq . 25 and eq . 26

43
 APPENDIX C. CODE

// atan2 o u t p u t s t h e v a l u e o f − to ( r a d i a n s ) − s e e h t t p : / / en .
w i k i p e d i a . o r g / w i k i / Atan2
// I t i s then c o n v e r t e d from r a d i a n s t o d e g r e e s
#i f d e f RESTRICT_PITCH // Eq . 25 and 26
d o u b l e r o l l = atan2 ( accY , accZ ) ∗ RAD_TO_DEG;
d o u b l e p i t c h = atan (−accX / s q r t ( accY ∗ accY + accZ ∗ accZ ) ) ∗
RAD_TO_DEG;
#e l s e // Eq . 28 and 29
d o u b l e r o l l = atan ( accY / s q r t ( accX ∗ accX + accZ ∗ accZ ) ) ∗
RAD_TO_DEG;
d o u b l e p i t c h = atan2 (−accX , accZ ) ∗ RAD_TO_DEG;
#e n d i f

d o u b l e g y r o X r a t e = gyroX / 1 3 1 . 0 ; // Convert t o deg / s

d o u b l e g y r o Y r a t e = gyroY / 1 3 1 . 0 ; // Convert t o deg / s

#i f d e f RESTRICT_PITCH
// This f i x e s t h e t r a n s i t i o n problem when t h e a c c e l e r o m e t e r a n g l e
jumps between −180 and 180 d e g r e e s
i f ( ( r o l l < −90 && kalAngleX > 9 0 ) | | ( r o l l > 90 && kalAngleX < −90) )
{
kalmanX . s e t A n g l e ( r o l l ) ;
kalAngleX = r o l l ;
gyroXangle = r o l l ;
} else
kalAngleX = kalmanX . g e t A n g l e ( r o l l , gyroXrate , dt ) ; // C a l c u l a t e t h e
a n g l e u s i n g a Kalman f i l t e r

i f ( abs ( kalAngleX ) > 9 0 )

g y r o Y r a t e = −g y r o Y r a t e ; // I n v e r t r a t e , s o i t f i t s t h e r e s t r i c e d
accelerometer reading
kalAngleY = kalmanY . g e t A n g l e ( p i t c h , gyroYrate , dt ) ;
#e l s e
// This f i x e s t h e t r a n s i t i o n problem when t h e a c c e l e r o m e t e r a n g l e
jumps between −180 and 180 d e g r e e s
i f ( ( p i t c h < −90 && kalAngleY > 9 0 ) | | ( p i t c h > 90 && kalAngleY <
−90) ) {
kalmanY . s e t A n g l e ( p i t c h ) ;
kalAngleY = p i t c h ;
gyroYangle = p i t c h ;
} else
kalAngleY = kalmanY . g e t A n g l e ( p i t c h , gyroYrate , dt ) ; // C a l c u l a t e
t h e a n g l e u s i n g a Kalman f i l t e r

i f ( abs ( kalAngleY ) > 9 0 )

g y r o X r a t e = −g y r o X r a t e ; // I n v e r t r a t e , s o i t f i t s t h e r e s t r i c e d
accelerometer reading
kalAngleX = kalmanX . g e t A n g l e ( r o l l , gyroXrate , dt ) ; // C a l c u l a t e t h e
a n g l e u s i n g a Kalman f i l t e r
#e n d i f

gyroXangle += g y r o X r a t e ∗ dt ; // C a l c u l a t e gyro a n g l e w i t h o u t any

filter
gyroYangle += g y r o Y r a t e ∗ dt ;

44
C.2. ARDUINO CODE

g y r o X r a t e 3 = kalmanX . g e t R a t e ( ) ; // C a l c u l a t e gyro a n g l e u s i n g t h e
unbiased rate
g y r o Y r a t e 3 = kalmanY . g e t R a t e ( ) ;

// R e s e t t h e gyro a n g l e when i t has d r i f t e d t o o much

i f ( gyroXangle < −180 | | gyroXangle > 1 8 0 )
gyroXangle = kalAngleX ;
i f ( gyroYangle < −180 | | gyroYangle > 1 8 0 )
gyroYangle = kalAngleY ;

float filterFrequency = 1000.0;

BX2 = f . f i l t e r I n ( g y r o X r a t e 3 ) ; // Butterworth f i l t e r i n g
BY2 = g . f i l t e r I n ( g y r o Y r a t e 3 ) ;

v_vink1 = 3 . 1 4 1 5 / 1 8 0 . 0 ∗ kalAngleX−dx ; // c o n v e r t t o Radians

v_vink2 = 3 . 1 4 1 5 / 1 8 0 . 0 ∗ kalAngleY−dy ;

v_hast1 = 3 . 1 4 1 5 / 1 8 0 . 0 ∗BX2 ;
v_hast2 = 3 . 1 4 1 5 / 1 8 0 . 0 ∗BY2 ;

/∗ C o py r i g h t (C) 2012 K r i s t i a n Lauszus , TKJ E l e c t r o n i c s . A l l r i g h t s

reserved .

This s o f t w a r e may be d i s t r i b u t e d and m o d i f i e d under t h e terms o f t h e

GNU
G e n e r a l P u b l i c L i c e n s e v e r s i o n 2 (GPL2) a s p u b l i s h e d by t h e Free
Software
Foundation and a p p e a r i n g i n t h e f i l e GPL2 .TXT i n c l u d e d i n t h e
packaging of
t h i s f i l e . P l e a s e n o t e t h a t GPL2 S e c t i o n 2 [ b ] r e q u i r e s t h a t a l l works
based
on t h i s s o f t w a r e must a l s o be made p u b l i c l y a v a i l a b l e under t h e terms
of
t h e GPL2 ( " C o p y l e f t " ) .

Contact i n f o r m a t i o n
−−−−−−−−−−−−−−−−−−−
K r i s t i a n Lauszus , TKJ E l e c t r o n i c s
Web : h t t p : / /www. t k j e l e c t r o n i c s . com
e−m a i l : k r i s t i a n l @ t k j e l e c t r o n i c s . com
∗/

c o n s t u in t 8_ t IMUAddress = 0 x68 ; // AD0 i s l o g i c low on t h e PCB

c o n s t u i n t 1 6 _t I2C_TIMEOUT = 1 0 0 0 ; // Used t o check f o r e r r o r s i n I2C
communication

u i n t 8 _ t i 2 c W r i t e ( u i nt 8 _t r e g i s t e r A d d r e s s , u i nt 8 _t data , b o o l sendStop )
{

45
 APPENDIX C. CODE

r e t u r n i 2 c W r i t e ( r e g i s t e r A d d r e s s , &data , 1 , sendStop ) ; // Returns 0 on

success
}

u i n t 8 _ t i 2 c W r i t e ( u i nt 8 _t r e g i s t e r A d d r e s s , u i nt 8 _t ∗ data , ui n t8 _t l e n g t h
, b o o l sendStop ) {
Wire . b e g i n T r a n s m i s s i o n ( IMUAddress ) ;
Wire . w r i t e ( r e g i s t e r A d d r e s s ) ;
Wire . w r i t e ( data , l e n g t h ) ;
u i n t 8_ t r c o d e = Wire . e n d T r a n s m i s s i o n ( sendStop ) ; // Returns 0 on
success
i f ( rcode ) {
S e r i a l . p r i n t (F( " i 2 c W r i t e f a i l e d : " ) ) ;
S e r i a l . p r i n t l n ( rcode ) ;
}
r e t u r n r c o d e ; // See : h t t p : / / a r d u i n o . c c / en / R e f e r e n c e /
WireEndTransmission
}
u i n t 8 _ t i2cRead ( ui n t8 _ t r e g i s t e r A d d r e s s , ui n t8 _ t ∗ data , ui n t8 _t n b y t e s )
{
u i n t3 2 _ t timeOutTimer ;
Wire . b e g i n T r a n s m i s s i o n ( IMUAddress ) ;
Wire . w r i t e ( r e g i s t e r A d d r e s s ) ;
u i n t 8_ t r c o d e = Wire . e n d T r a n s m i s s i o n ( f a l s e ) ; // Don ’ t r e l e a s e t h e bus
i f ( rcode ) {
S e r i a l . p r i n t (F( " i2cRead f a i l e d : " ) ) ;
S e r i a l . p r i n t l n ( rcode ) ;
r e t u r n r c o d e ; // See : h t t p : / / a r d u i n o . c c / en / R e f e r e n c e /
WireEndTransmission
}
Wire . requestFrom ( IMUAddress , nbytes , ( u in t 8_ t ) t r u e ) ; // Send a
r e p e a t e d s t a r t and then r e l e a s e t h e bus a f t e r r e a d i n g
f o r ( u i nt 8 _t i = 0 ; i < n b y t e s ; i ++) {
i f ( Wire . a v a i l a b l e ( ) )
data [ i ] = Wire . r e a d ( ) ;
else {
timeOutTimer = m i c r o s ( ) ;
w h i l e ( ( ( m i c r o s ( ) − timeOutTimer ) < I2C_TIMEOUT) && ! Wire .
available () ) ;
i f ( Wire . a v a i l a b l e ( ) )
data [ i ] = Wire . r e a d ( ) ;
else {
S e r i a l . p r i n t l n (F( " i2cRead t i m e o u t " ) ) ;
r e t u r n 5 ; // This e r r o r v a l u e i s not a l r e a d y taken by
endTransmission
}
}
}
r e t u r n 0 ; // S u c c e s s
}

46
TRITA TRITA-ITM-EX 2019:38

www.kth.se