Balancing Cube

Thesis WIP

TIM GIDLOFCARL GRUNEAU

Bachelor’s Thesis at ITMSupervisor: Nihad SubasicExaminer: 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 allbeing connected and smart, and by that micro controllersis playing an increasingly important role in peoples privatelife.

In this thesis in mechatronics a micro controller willbe implemented on an Arduino to make it possible for acube to balance in upright position. The cube is in theoryan inverted pendulum with one degrees of freedom, and isintended to balance using an inertia wheel at the top of thestructure. A PID regulator was used, and at the time thisreport was written, the right parameter values for PID wasnot found. The cube is able to shift its position back andforth over the setpoint with support on each side to preventit from falling. A bit more tuning is required to make itbalance on its own.

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

ReferatBalanserande Kub

I dagens samhalle ar microprocessorer i nastan alla nyaprodukter skapade for hemma anvandande. De ar alla sam-mankopplade och smarta. I och med det far mikrokontrolleren allt storre roll i manniskors privata liv.

I den har rapporten inom mekatronik implementerasen microkontroller til en arduino for att balansera en kubstaende pa en kant. I teorin ar en kub en inverterad pendelmed en frihetsgrad och ar tankt att balansera med hjalp avett reaktionshjul monterat overst pa prototypen. En PIDregulator anvandes och da denna rapporten skrevs haderatt parametrar inte funnits. Kuben klarar av att andraposition fram och tillbaka over referenslaget da den blocke-ras fran att ramla. For att den ska klara av att balanseraav sig sjalv behovs regulatorn stallas in battre.

Acknowledgements

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

Contents

1 Introduction 11.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Purpose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.3 Goals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.4 Scope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.5 Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2 Theory 32.1 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1.1 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.1.2 Hall effect-sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.2 Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.2.1 Energy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.2.2 Lagrangian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.3 Control theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.3.1 State space model . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.3.2 State feedback . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.3.3 PID-controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3 Construction and implementation 93.1 Components . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.1.1 Motor and motor driver . . . . . . . . . . . . . . . . . . . . . . . . 93.1.2 Reading crucial values . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.2 Construction and software . . . . . . . . . . . . . . . . . . . . . . . . . . . 103.2.1 Test Rig . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103.2.2 Reaction wheel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.2.3 Tuning the controller . . . . . . . . . . . . . . . . . . . . . . . . . . 113.2.4 Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

4 Results 13

5 Discussion 155.1 Parasitic inductance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155.2 Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155.3 Improvements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155.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). . . . . . . . . 103.2 Flow chart of the system (Created using draw.io) . . . . . . . . . . . . . . . . 11

4.1 Picture of finished product seen from the front . . . . . . . . . . . . . . . . . . 134.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 tolearn control theory and how it is applied to balancing machines and objects. Similarobjects have already been created by mechatronic institutions which leaves great amountsof 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 morein depth of nonlinear control due to an inverted pendulums nonlinear motion. A cube thatwould balance on one corner shows the same control challenges as an inverted pendulumhowever 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 wheelscan 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 tobalance on one of the edges without any external help. It should also be able to handleminor disturbances. The second purpose of this bachelor thesis is to further develop theknowledge 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 itbalanced.

The second goal will be to make it jump up from laying with the flat side down, to theedge and then go into the balancing phase.

With time and resources, the third goal will be to make the cube safely fall down to theresting position.

1

CHAPTER 1. INTRODUCTION

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

1.5 MethodThe course of action in this project started with modelling the dynamics of the cube toformulate mathematical equations that could be used in the control theory. The controltheory 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 sectionswill describe the hardware needed to make the cube balance and how they work. The secondpart will describe the mathematics that will be needed for the control theory and the lastpart 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 neededto read the most crucial variables. The accelerometer and gyroscope sensor are measuringthe tilt angle and angle velocity of the whole system, and the hall effect sensor is used toread 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 measurefrom 2 to 6 degrees of freedom which translates to be able to measure movement in the3D space. In this project the IMU sensor will be a gyroscope sensor and accelerometercombined. The gyroscope sensor is measuring the angular velocity, and by math and Corioliseffect, it is possible to calculate the angle the sensor is situated in. The accelerometersensor 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 ascapacitors together with the small elements on the board. When the mass is moving, thesmall 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 smallvoltage appearing when a magnetic field is applied, the sensor is for instance able to readthe revolutions per minute of a wheel if the dimensions from the wheel is known.

3

CHAPTER 2. THEORY

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

2.2.1 EnergyTo model the dynamics of the cube and reaction wheel, equations of potential- and kineticenergies 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 massof the cube. Since the mass of the reaction wheel affects the system from the center of thecube the equation is set up as

V = (mwL+mcLm)g cos(θc) (2.1)

where mw and mc is the mass of the wheel and cube respectively. L is the length from thebalancing edge to the center of the cube and Lm is the length to center of mass. As seen inFigure 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 usingGeoGebra).

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

T = 12(Ic +mwL

2)θ2c + 1

2Iw(θc + θw)2 (2.2)

where the first part is the energy from the cube and the second part is the energy of thereaction wheel. Ic and Iw is the inertia from the cube and wheel respectively.

4

2.2. MODELING

2.2.2 LagrangianTo 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 describesparticles independent movement in the 3D-space, and their generalized coordinates whichis their positions. The difference between the cubes kinetic- and potential energies is whatformulates the Lagrangian-function as L = T − V . Inserting equations 2.1 and 2.2 gives

L = 12(Ic +mwL

2)θ2c + 1

2Iw(θc + θw)2 − (mwL+mcLm)g cos(θc). (2.3)

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

dt( ∂L∂qk

) − ∂L

∂qk+ ∂R

∂qk= pk (2.4)

where qk is the generalized coordinates as described above and qk is their change over time.Added to the equation is the Hamilton-equations ∂R

∂qk, which are the forces that make the

system lose energy in form of friction, called the dissipative forces, and pk which is thegeneralized torque.

Derivatives of the Lagrangian-function with respect of angle velocity and time will givethe first part of the the Lagrangian-equation,

d

dt( ∂L∂θc

) = (Ic +mwL2)θc + Iw(θc + θw) (2.5)

d

dt( ∂L∂θw

) = Iw(θc + θw) (2.6)

where equation 2.5 is based on the cubes angular velocity and 2.6 is based on the wheelsangular velocity. Next

∂L

∂θc= (mwL+mcLm)g sin(θc) (2.7)

forms the derivative of the Lagrangian-function with respect to the cubes angle. The partialderivative with respect to the wheels angle is based of the momentum transferred from themotor Mm,

∂L

∂θw= K2φu (2.8)

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 lossesR = 1

2fcθ2c + 1

2fwθ2w (2.9)

is used, where fc is the friction constant between the cube and surface and fw is the frictionconstant between the reaction wheel and motor axle. The derivatives of equation 2.9 withrespect to the angle velocities is expressed as

∂R

∂θc= fcθc (2.10)

∂R

∂θw= fwθw (2.11)

5

CHAPTER 2. THEORY

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

θc = (mwL+mcLm)g sin(θc) −K2φu+ fwθw − fcθcIc +mwL2 (2.12)

θw = (Ic + Iw +mwL2)(K2φu− fwθw) − (mcLm +mwL)g sin(θc)Iw + fcθcIw

Iw(Ic +mwL2) (2.13)

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

2.3 Control theoryTo be able to control the system, a controller will be implemented. There are variouskinds of potential regulators. Depending on the actual system, an evaluation is neededto determine the most suitable controller. In our model there are three crucial values thewhole system is relying on, and they are all readable.

2.3.1 State space modelThe state space model is on the form

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

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

where x represents the derivative of the states with respect to time, the y represents theoutput 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 whatoutput value you will get at a specific point of time. To make the state space model muchmore easy to handle, it is preferred if the model is linear. If not so, a wisely selected pointto linearize about must be stated. In this case, because the action for the system willoccur while the cube is balancing on the edge, the point along the vertical axis with all theparameters set to zero is chosen as the point to linearize around.

A =

0 1 0(Lmw+Lmmc)g

Ic+mwL2−fc

Ic+mwL2fw

Ic+mwL2

−(Lmw+Lmmc)gIc+mwL2

fc

Ic+mwL2−(Ic+Iw+mwL

2)cw

Iw(Ic+mwL2)

(2.16)

B =

0−K2φ

Ic+mwL2

(Ic+Iw+mwL2)K2φ

Iw(Ic+mwL2)

(2.17)

C =

1 0 00 1 00 0 1

(2.18)

D =[0]

(2.19)

6

2.3. CONTROL THEORY

2.3.2 State feedbackWhen the correlation between the input and output is known, it would be advantageous tobase your input value on your states [8]. To do so it is needed to implement a state feedbackcontroller. When using a feedback controller, the location of the poles must be known sothe system can act most optimally. But finding these placements for the poles can be quitehard, and a few compromises might be needed regarding the robustness and speed of thesystem. By implementing a Linear Quadratic Regulator (LQR), these compromises can beoptimized by finding an ”L” that minimizes the criteria∫ ∞

0(xT (t)Qx(t) + u2(t))dt (2.20)

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

2.3.3 PID-controllerAnother often used controller in the industry is the PID controller[8], and it has been aroundsince the 1700s in various forms. It is a low cost controller, and usually not requires anymodels for the system to be tuned correctly. The controller is easy implemented, but requiresfine tuning to get the perfect parameters P, I and D which is the three gain parameters thatcalculates the output with respect to the error, the integral of error and derivative of error.

u(t) = KP (e(t) + 1KI

∫ t

0(e(t))dt+KD

d

dte(t)) (2.21)

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 backin to the controller, hence a feedback loop. The KP in the formula is the proportionalgain, and is always proportional to the current error e(t). With a big KP gain, the systemwill react fast to small changes. The KD gain is preventing overshoot of the system bylooking 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 isbuild up. A CAD-model will be presented to visualize the concept. Due to the lack offinancial means the outcome of the project is not what was first planned, but it fulfills itspurpose. This is after all just meant to be a prototype, so the cosmetics of the constructionis not the biggest priority.

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

3.1.1 Motor and motor driverThe 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, mostlybecause the compact design which makes the whole construction more compact. But afterconsulting 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. Theactual motor is 24V and 21W, with a torque constant of 0.0584 Nm/A, which is enoughaccording to the Matlab simulations that has been executed.

The driver chosen is a VNH3SP30 motor driver, and it is capable of delivering up to 30amps with a range of 5.5 to 36 volt. Since the Arduino used in this project only is capableof delivering 5 volt, an external power source was required to provide power to the motor.

3.1.2 Reading crucial valuesThe LQR controller is relying on correct values from the parameters discussed in section2.2. To provide the controller with the right values an MPU 6050 sensor was bought. Thesensor 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, toget the right values from the gyroscope sensor. The MPU 6050 sensor is providing thecontroller 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 forreading clockwise rotation and the other for reading counterclockwise rotation. They arespaced 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 incounterclockwise direction, the inner magnet have a lead, and the inner hall effect sensorwill be detecting the magnet before the outer one, thereby the Arduino knows the wheel isturning in counterclockwise direction.

To make the readings more accurate and cancel out the noise from the sensor a Kalmanfilter was implemented, which make values of the states more stable over time and thecontroller 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 andit can do so even though the nature of said system is unknown. It works by implementingequations and gives a solution of the least-square method that the controller can compute.

3.2 Construction and softwareThe construction will be discussed in the following section. Starting off by explaining thetest rig and different parts that went into creating it and followed up by the code that wasimplemented 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 twocutouts designed to give the rig structure and hold the motor as can be seen in figure 3.1below. Two spacers were 3D-printed to hold everything together and a rod going throughall of the parts. The rod was then connected to a base plate and for the cube to be able torotate 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 wheelThe reaction wheel was first drafted up in Solid edge, setting the dimension so the weightwould be the right. The wheel was then waterjet cut and lathed so the hub and spokes werethinner than the outer ring. Doing so the wheel has the most of its mass concentrated atthe periphery giving it a higher moment of inertia. The outcome was a wheel with diameterof 15 cm and 5 mm thick.

An axle hub was created out of aluminium to be able to mount the reaction wheel onthe 5 mm motor axle.

3.2.3 Tuning the controllerAfter discussions with the course assistant it was decided to implement a PID controllerinstead of using LQR due to communication between Matlab and Arduino will cause a delayin the system and Matlab was required to solve the equations for the LQR.

3.2.4 CodeAn Arduino was used to control the system, reading the values and provide the right voltageto 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 sendingback the correct voltage to supply the motor with when the calculations would have beenexecuted. Instead a PID controller was chosen. The PID controller only need one inputvalue which in this case was the angle of the cube picked up by the accelerometer and theArduino 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 withthe 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 prototypestate, since the focus was based at making the prototype balance. The prototype was neverable to balance on its own. With the PID parameter values at KP = 7.5, KI = 0.6 andKD = 0.9 the prototype was rocking back and forth in between 2 fingers that was blockingit from falling over. Due to the first goal not being fulfilled the second and third goal couldnot 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 thatwas encountered during the process.

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

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

5.3 ImprovementsThe inertia wheel was not properly balanced, mainly the dynamic balance. One reasoncould be the hub not completely flat on the side mated with the wheel. To improve this anew hub would be created and after properly dynamically balance the wheel, reason for itnot being done was not having the proper equipment. The ball bearings had a small playon 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 theoriginal idea.

5.4 COVID-19During the time of this project the pandemic COVID-19 was in full effect. The Swedishgovernment decided to close off all of the higher education which meant all studies wereto be performed from distance, meaning no access to the Royal Institute of Technology’sfacilities. The communication with the course assistants was mostly through e-mail andworkshops online through video conferences. Due to this when an obstacle was encountereda lot of time was spent trying to solve it while receiving suggestions from prior mentionedcourse assistant. Because of this changes was made to the design, first plan was to create acube that was able to balance but instead the test rig became the final product. Some partsthat 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. Astrom 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 Bjorn Pehrsson. “Development of a Nonlinear MechatronicCube”. 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 Robotsand 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 ars. Stockholm: Institutionen for Maskinkon-struktion, KTH, 2013, p. 298.

[8] Torkel Glad and Lennart Ljung. Reglerteknik - Grundlaggande 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 20203 TRITA number: TRITA_ITM_EX 2020:354 Name of project: Balancing Cube56 Description of the code:7 This program is intended to make an one DOF cube balance on its

edge. It’s doing so by8 read angle measurement from IMU mpu6050 sensor, filter the angle

through a Kalmanfilter9 and then use the angle to calculate the right voltage for the

motor. A PID is implemented10 which takes the filtered angle and gives an output.11 */1213 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All

rights reserved.1415 This software may be distributed and modified under the terms of

the GNU16 General Public License version 2 (GPL2) as published by the Free

Software17 Foundation and appearing in the file GPL2.TXT included in the

packaging of18 this file. Please note that GPL2 Section 2[b] requires that all

works based19 on this software must also be made publicly available under the

terms of20 the GPL2 ("Copyleft").2122 Contact information23 -------------------2425 Kristian Lauszus, TKJ Electronics

23

APPENDIX C. ARDUINO CODE

26 Web : http://www.tkjelectronics.com27 e-mail : kristianl@tkjelectronics.com28 */2930 #include <PID_v1.h> // installed library for PID-controller31 #include <Wire.h>32 #include <Kalman.h> // Source: https://github.com/TKJElectronics/

KalmanFilter3334 #define RESTRICT_PITCH // Comment out to restrict roll to 90deg

instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf

3536 Kalman kalmanX; // Create the Kalman instances37 Kalman kalmanY;3839 /* IMU Data */40 double accX, accY, accZ;41 double gyroX, gyroY, gyroZ;42 int16_t tempRaw;434445 double gyroXangle, gyroYangle; // Angle calculate using the gyro

only46 double compAngleX, compAngleY; // Calculated angle using a

complementary filter47 double kalAngleX, kalAngleY; // Calculated angle using a Kalman

filter48 double offsetAngle = 2.7;4950 uint32_t timer;51 uint8_t i2cData[14]; // Buffer for I2C data5253 //motor constants54 const int inaPin = 13;55 const int inbPin = 9;56 const int pwmPin = 6;57 const int trimPin = A0;58 int i;5960 // PID variables61 double Setpoint, Input, Output;62 double Kp = 15, Ki = 0, Kd = 5;63 PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);646566 void setup() {6768 Serial.begin(115200);

24

69 Wire.begin();70 #if ARDUINO >= 15771 Wire.setClock(400000UL); // Set I2C frequency to 400kHz72 #else73 TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to

400kHz74 #endif7576 i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) =

1000Hz77 i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc

filtering, 256 Hz Gyro filtering, 8 KHz sampling78 i2cData[2] = 0x00; // Set Gyro Full Scale Range to 250deg /s79 i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to

2g80 while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four

registers at once81 while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis

gyroscope reference and disable sleep mode8283 while (i2cRead(0x75, i2cData, 1));84 if (i2cData[0] != 0x68) { // 0x68 Read "WHO_AM_I" register85 Serial.print(F("Error reading sensor"));86 while (1);87 }8889 delay(100); // Wait for sensor to stabilize9091 /* 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]);9697 // Source: http://www.freescale.com/files/sensors/doc/app_note/

AN3461.pdf eq. 25 and eq. 2698 // atan2 outputs the value of - to (radians) - see http://

en.wikipedia.org/wiki/Atan299 // It is then converted from radians to degrees

100 #ifdef RESTRICT_PITCH // Eq. 25 and 26101 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 29104 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) *

RAD_TO_DEG;105 double pitch = atan2(-accX, accZ) * RAD_TO_DEG;106 #endif107

25

APPENDIX C. ARDUINO CODE

108 kalmanX.setAngle(roll); // Set starting angle109 kalmanY.setAngle(pitch);110 gyroXangle = roll;111 gyroYangle = pitch;112 compAngleX = roll;113 compAngleY = pitch;114115 //MOTOR parameters116 pinMode(inaPin, OUTPUT);117 pinMode(inbPin, OUTPUT);118 pinMode(pwmPin, OUTPUT);119 pinMode(trimPin, INPUT);120121 // PID parameters122 Setpoint = 0;123 myPID.SetMode(AUTOMATIC);124 timer = micros();125 }126127 // Main loop where all the magic happends128 void loop(){129 mpu_kalman(); // reads the values

from the sensor, and filters them130 Input =( kalAngleY-offsetAngle); // a sligt offset is

subtracted from the angle to get the131 // correct input for

the PID132 Serial.println(kalAngleY-offsetAngle);133134135 // if the angle is bigger than 10 degrees, the cube will do

nothing136 if (abs(Input) > 10){137 digitalWrite(inaPin, LOW);138 digitalWrite(inbPin, LOW);139 analogWrite(pwmPin, 0);140 //Serial.println("STOP");141 }142143 // If the angle is between 0 and -10 degrees144 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 degrees151 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 }158159160 }161162 // void that reads all the values from the MPU650 sensor,

calculates the angles163 // and then filters them164 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]);;174175 double dt = (double)(micros() - timer) / 1000000; // Calculate

delta time176 timer = micros();177178 // Source: http://www.freescale.com/files/sensors/doc/app_note/

AN3461.pdf eq. 25 and eq. 26179 // atan2 outputs the value of - to (radians) - see http://

en.wikipedia.org/wiki/Atan2180 // It is then converted from radians to degrees181 #ifdef RESTRICT_PITCH // Eq. 25 and 26182 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 29185 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) *

RAD_TO_DEG;186 double pitch = atan2(-accX, accZ) * RAD_TO_DEG;187 #endif188189 double gyroXrate = gyroX / 131.0; // Convert to deg/s190 double gyroYrate = gyroY / 131.0; // Convert to deg/s191192 #ifdef RESTRICT_PITCH193 // 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 } else200 kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); //

Calculate the angle using a Kalman filter201202 if (abs(kalAngleX) > 90)203 gyroYrate = -gyroYrate; // Invert rate, so it fits the

restriced accelerometer reading204 kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);205 #else206 // This fixes the transition problem when the accelerometer

angle jumps between -180 and 180 degrees207 if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY

< -90)) {208 kalmanY.setAngle(pitch);209 compAngleY = pitch;210 kalAngleY = pitch;211 gyroYangle = pitch;212 } else213 kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); //

Calculate the angle using a Kalman filter214215 if (abs(kalAngleY) > 90)216 gyroXrate = -gyroXrate; // Invert rate, so it fits the

restriced accelerometer reading217 kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate

the angle using a Kalman filter218 #endif219220 gyroXangle += gyroXrate * dt; // Calculate gyro angle without

any filter221 gyroYangle += gyroYrate * dt;222 //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle

using the unbiased rate223 //gyroYangle += kalmanY.getRate() * dt;224225 compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll

; // Calculate the angle using a Complimentary filter226 compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 *

pitch;227228 // Reset the gyro angle when it has drifted too much229 if (gyroXangle < -180 || gyroXangle > 180)230 gyroXangle = kalAngleX;

28

231 if (gyroYangle < -180 || gyroYangle > 180)232 gyroYangle = kalAngleY;233234 Serial.print("\r\n");235 delay(2);236237 }

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

23 This software may be distributed and modified under the terms of

the GNU4 General Public License version 2 (GPL2) as published by the Free

Software5 Foundation and appearing in the file GPL2.TXT included in the

packaging of6 this file. Please note that GPL2 Section 2[b] requires that all

works based7 on this software must also be made publicly available under the

terms of8 the GPL2 ("Copyleft").9

10 Contact information11 -------------------1213 Kristian Lauszus, TKJ Electronics14 Web : http://www.tkjelectronics.com15 e-mail : kristianl@tkjelectronics.com16 */1718 const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB19 const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in

I2C communication2021 uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool

sendStop) {22 return i2cWrite(registerAddress, &data, 1, sendStop); //

Returns 0 on success23 }2425 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

success30 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/

WireEndTransmission35 }3637 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 bus42 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/

WireEndTransmission49 }50 Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a

repeated start and then release the bus after reading51 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

endTransmission62 }63 }64 }65 return 0; // Success66 }

30