INOM
EXAMENSARBETE TEKNIK, GRUNDNIVÅ, 15 HP
STOCKHOLM SVERIGE 2020,
Balancing Cube Balanserande Kub
TIM GIDLÖF
CARL GRUNEAU
KTH
SKOLAN FÖR INDUSTRIELL TEKNIK OCH MANAGEMENT
Balancing Cube
TIM GIDL ¨ OF CARL GRUNEAU
Bachelor’s Thesis at ITM Supervisor: Nihad Subasic
Examiner: Nihad Subasic
TRITA-ITM-EX 2020:35
Abstract
In todays society there are microprocessors in almost every new item that is produced for home use. They are all be- ing connected and smart, and by that microcontrollers are playing an increasingly important role in peoples private life.
In this thesis in mechatronics a controller will be imple- mented on an Arduino to make it possible for a cube to balance in upright position. The cube is in theory an in- verted pendulum with one degree of freedom, and is in- tended to balance using an inertia wheel at the top of the structure. A PID regulator was used, and at the time this report was written, the right parameter values for the PID was not found. The cube is able to shift its position back and forth over the setpoint with support on each side to prevent it from falling. A bit more tuning is required to make it balance on its own.
Keywords: Mechatronics, Reaction wheel, Balancing robot, Control theory, PID
Referat
Balanserande Kub
I dagens samh¨alle ¨ar det microprocessorer i n¨astan alla nya produkter som skapas f¨or den privata marknaden. De ¨ar alla sammankopplade och smarta. I och med det spelar mikro- kontrollers en allt st¨orre roll i m¨anniskors dagliga liv.
I den h¨ar rapporten inom mekatronik implementeras en re- gulator i en arduino f¨or att balansera en kub st˚aende p˚a en kant. I teorin ¨ar en kub en inverterad pendel med en frihetsgrad och ¨ar t¨ankt att balansera med hj¨alp av ett reaktionshjul monterat ¨overst p˚a prototypen. En PID re- gulator anv¨andes och d˚a denna rapporten skrevs hade r¨att parametrar inte p˚atr¨affats. Kuben klarar av att ¨andra po- sition fram och tillbaka ¨over referensl¨aget d˚a den blockeras fr˚an att ramla. F¨or att den ska klara av att balansera av sig sj¨alv beh¨over regulatorn st¨allas in b¨attre.
Nyckelord: Mekatronik, Reaktionshjul, Balanserande robot, Reglerteknik, PID
Acknowledgements
We would like to thank our course supervisor and examiner Nihad Subasic for always answering questions and emails at times we needed help and giving us an opportunity to complete this project despite the effects of COVID-19. A special appreciation goes to the course assistant Seshagopalan Thorapalli Muralidharan for always taking the time to help and meeting up with us when we got completely stuck.
Contents
1 Introduction 1
1.1 Background . . . 1
1.2 Purpose . . . 1
1.3 Goals . . . 1
1.4 Scope . . . 2
1.5 Method . . . 2
2 Theory 3 2.1 Sensors . . . 3
2.1.1 IMU . . . 3
2.2 Modeling . . . 3
2.2.1 Energy . . . 3
2.2.2 Lagrangian . . . 4
2.3 Control theory . . . 6
2.3.1 State space model . . . 6
2.3.2 PID-controller . . . 6
2.3.3 Tuning the controller . . . 7
2.3.4 Ziegler-Nichols Method . . . 7
2.3.5 Trial and error . . . 8
2.3.6 Kalman filter . . . 8
3 Construction and implementation 9 3.1 Components . . . 9
3.1.1 Motor and motor driver . . . 9
3.1.2 Reading crucial values . . . 9
3.2 Construction and software . . . 10
3.2.1 Test Rig . . . 10
3.2.2 Reaction wheel . . . 10
3.2.3 Code . . . 11
4 Results 13 5 Discussion 15 5.1 Construction . . . 15
5.2 Parasitic inductance . . . 15
5.3 Controller . . . 15
5.4 Improvements . . . 16
5.5 COVID-19 . . . 16
5.6 Conclusion . . . 16
Bibliography 17
A Dunkermotoren G42X40 datasheet 19
B Circuitdiagram 21
C Arduino code 23
C.1 Main code . . . 23 C.2 I2C code for Kalman filter . . . 29
D Matlab-code 33
Appendices 33
List of Figures
2.1 A visualization of θc, θw, Center of mass, Lm and L (Created using GeoGebra). 4 3.1 The design of the test rig made (Created using Solid Edge 2019). . . 10 3.2 Flow chart of the system (Created using draw.io) . . . 11 4.1 Picture of finished product seen from the side . . . 13 4.2 Graph showing the angle of the cube while balancing with support, created with
Matlab. . . 14 4.3 Graph showing the voltage sent to the motor in the same duration as figure 4.2,
created with Matlab. . . 14 A.1 Datasheet for the Dunkermotoren G42X40 . . . 19 B.1 Circuitdiagram over all electric components. (Created using Fritzing) . . . 21
List of Tables
2.1 Table showing how change in PID parameters affects the system, taken from [9]. 7 2.2 Table showing setup rules for Ziegler-Nichols method, taken from [8]. . . 7
Abbreviations
IMU Inertial Measurements Unit
PID Proportional-Integral-Derivative Controller
Chapter 1
Introduction
In this introduction the background, purpose, goals, scope and method will be discussed.
1.1 Background
Considering today’s generation of robotics, it is important for a mechatronics student to learn control theory and how it is applied to balancing machines and objects. Similar objects have already been created by mechatronic institutions which leaves great amounts of information in the subject to be found.
For many years inverted pendulums have been used to study control theory in laborato- ries[1]. First to implement linear control on a system that is unstable to then go more in depth of nonlinear control due to an inverted pendulums nonlinear motion. A cube that would balance on one corner shows the same control challenges as an inverted pendulum however instead of a 2D-space the balancing cube operate in the 3D-space.
The cube in this project is using an inertia wheel to be able to balance. Inertia wheels can for instance be used to stabilise various kinds of spacecrafts, as an example satellites[2].
1.2 Purpose
The purpose of this bachelor’s thesis is to construct and program a cube that is able to balance on one of the edges without any external help. The following resarch questions will be answered in this thesis:
• What methods could be used to tune a PID regulator?
• What method is best suited for a model that is nonlinear?
The second purpose of this bachelor thesis is to further develop the knowledge in control system engineering and electronics.
1.3 Goals
The first goal is to make the cube balance on its edge and with minor disturbances keep it balanced.
1
CHAPTER 1. INTRODUCTION
The second goal will be to make it jump up from laying with the flat side down, to the edge and then go into the balancing phase.
With time and resources, the third goal will be to make the cube safely fall down to the resting position.
1.4 Scope
All electronics and mechanical parts shall be fitted into a 150x150x150mm cube apart from the power source which will stand beside the cube. The robot needs to have a mechanical, an electrical and a programmable part.
1.5 Method
The course of action in this project started with modelling the dynamics of the cube to formulate mathematical equations that could be used in the control theory. The control theory was then tested in matlab to after be implemented into a test rig.
2
Chapter 2
Theory
This chapter will describe the theory behind the mechanics of the cube. The first sections will describe the hardware needed to make the cube balance and how they work. The second part will describe the mathematics that will be needed for the control theory and the last part will bring up the control theory that will be implemented.
2.1 Sensors
To be able to control the system and make a closed loop system, a sensor is needed to read the most crucial variables. The accelerometer and gyroscope sensor are measuring the tilt angle and angle velocity of the whole system.
2.1.1 IMU
The IMU sensor can measure from 2 to 6 degrees of freedom which translates to be able to measure movement in the 3D space. In this project the IMU sensor will be a gyroscope sensor and accelerometer combined. The gyroscope sensor is measuring the angular velocity, and by math and Coriolis effect, it is possible to calculate the angle the sensor is situated in.
The accelerometer sensor measures the linear acceleration with the help of a moving mass inside the sensor. The mass is moving in a linear motion, and is coupled with small arms which acts as capacitors together with the small elements on the board. When the mass is moving, the small capacitance emerging can be read and the acceleration be calculated[3].
2.2 Modeling
For the cube to be able to balance on its edge, a model of the dynamics and a state-space model was designed.
2.2.1 Energy
To model the dynamics of the cube and reaction wheel, equations of potential- and kinetic energies have to be set up [4][5]. To begin with, the potential energy equation will be formed.
3
CHAPTER 2. THEORY
The system is divided between the mass of the reaction wheel and the remaining mass of the cube. Since the mass of the reaction wheel affects the system from the center of the cube the equation is set up as
V = (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 the balancing edge to the center of the cube and Lmis the length to center of mass. As seen in Figure 2.1, θc is the angle the cube has in respect to the center point.
Figure 2.1. A visualization of θc, θw, Center of mass, Lm and L (Created using GeoGebra).
Next the equation for the kinetic energy will be modeled. The equations will be based of inertia around the balancing point of the cube. Due to the reaction wheel affecting the whole cubes inertia, it has to be considered forming the equation. Same goes for the angle velocity of the cube affecting the energy of the reaction wheel. From this the equation of the kinetic energy can be stated as
T =1
2(Ic+ mwL2) ˙θ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 the reaction wheel. Ic and Iwis the inertia from the cube and wheel respectively.
2.2.2 Lagrangian
To formulate the dynamics of the system, a method can be used called the Lagrangian- equations [6]. The Lagrangian-functions are based on degrees of freedom, which describes particles independent movement in the 3D-space, and their generalized coordinates which are their positions. The difference between the cubes kinetic- and potential energies are what formulates the Lagrangian-function as L = T − V . Inserting equations 2.1 and 2.2 gives
L=1
2(Ic+ mwL2) ˙θ2c+1
2Iw( ˙θc+ ˙θw)2−(mwL+ mcLm)g cos(θc). (2.3) 4
2.2. MODELING
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 ˙qkis their change over time.
Added to the equation is the Hamilton-equations ∂ ˙∂Rqk, which are the forces that make the system lose energy in form of friction, called the dissipative forces, and pk which is the generalized torque.
Derivatives of the Lagrangian-function with respect of angle velocity and time will give the first part of the the Lagrangian-equation,
d dt(∂L
∂ ˙θc
) = (Ic+ mwL2)¨θc+ Iw(¨θc+ ¨θw) (2.5)
and 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 wheels angular 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 partial derivative with respect to the wheels angle is based of the momentum transferred from the motor 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 losses
R=1
2fc˙θc2+1
2fw˙θ2w (2.9)
is used, where fcis the friction constant between the cube and surface and fwis the friction constant between the reaction wheel and motor axle. The derivatives of equation 2.9 with respect to the angle velocities is expressed as
∂R
∂ ˙θc
= fc˙θc, (2.10)
∂R
∂ ˙θw = fw˙θw (2.11)
which forms the last part of the Lagrangian-equation. By inserting equations 2.5-2.11 in equation 2.4 an expression for the the dynamics of the system can be formulated as
¨θc= (mwL+ mcLm)g sin(θc) − K2φu+ fw˙θw− fc˙θc
Ic+ 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.
5
CHAPTER 2. THEORY
2.3 Control theory
To be able to control the system, a controller will be implemented. There are various kinds of potential regulators. Depending on the actual system, an evaluation is needed to determine the most suitable controller. In our model there are three crucial values the whole system is relying on, and they are all readable.
2.3.1 State space model
The state space model is on the form
˙x = A~x(t) + B~u(t) (2.14)
and
y= C~x(t) + D~u(t), (2.15)
where ˙x represents the derivative of the states with respect to time, the y represents the output value of system, the ~x is a state-vector and ~u is a vector of the output values of the controller[8]. With this model, and a given input signal, it is possible to predict how the system will react and what output value you will get at a specific point of time. To make the state space model much more easy to handle, it is preferred if the model is linear. If not so, a wisely selected point to linearize about must be stated. In this case, because the action for the system will occur while the cube is balancing on the edge, the point along the vertical axis with all the parameters set to zero is chosen as the point to linearize around.
Linearizing the equations (2.12) and (2.13) around ~x = (0, 0, 0) yields
A=
0 1 0
(Lmw+Lmmc)g Ic+mwL2
−fc
Ic+mwL2
fw
Ic+mwL2
−(Lmw+Lmmc)g Ic+mwL2
fc
Ic+mwL2
−(Ic+Iw+mwL2)cw Iw(Ic+mwL2)
, (2.16)
B =
0
−K2φ Ic+mwL2 (Ic+Iw+mwL2)K2φ
Iw(Ic+mwL2)
, (2.17)
C=
1 0 0 0 1 0 0 0 1
(2.18)
and
D= 0 . (2.19)
2.3.2 PID-controller
The most common controller in the industry is the PID controller[8], and it has been around since the 1700s in various forms. It is a low cost controller that usually does not require any models for the system to be tuned correctly. The controller is easy to implement, but requires fine tuning to get the correct parameters which makes it difficult to tune in some cases. The parameters P , I and D is the three gain parameters that calculates the output with respect to the error, the integral of the error and derivative of the error. The output is calculated through
u(t) = KP(e(t) + 1 KI
Z t 0
(e(t))dt + KD
d
dte(t)) (2.20)
6
2.3. CONTROL THEORY
where the u(t) is the output value, and
e(t) = r(t) − y(t) (2.21)
where r(t) is the reference value, and the y(t) is the system output value which is fed back in to the controller, hence a feedback loop. The KP in the formula is the proportional gain, and is always proportional to the current error e(t). With a big KP gain, the system will react fast to small changes. The KDgain is preventing overshoot of the system by looking at the derivative of the error, and KI gain is mostly reducing the steady state error. For a clearer view how each parameter affects the system see table 2.1 below. Combining these three parameters and tuning them to perfection can sometimes be difficult, which is the biggest disadvantage of the PID controller.
Table 2.1. Table showing how change in PID parameters affects the system, taken from [9].
Parameter Rise time Overshoot Settling time Steady-state
error Stability
KPDecrease Increase Small change Decrease Degrade
KIDecrease Increase Increase Eliminate Degrade
KD
Minor
change Decrease Decrease No effect in
theory Improve if
KDsmall
2.3.3 Tuning the controller
There are multiple approaches that can be made in order to tune a PID controller. If the modelling of the system is unknown, which it commonly can be, it is difficult find the correct values for the regulator[8]. In the following sections commonly used methods will be discussed.
2.3.4 Ziegler-Nichols Method
The Ziegler-Nichols method of tuning PID controllers is often first to conduct an experiment to get basic knowledge of the system. The experiment begins with setting the integer (KI) and derivative (KD) parts to infinity and zero respectively. Doing so would remove their impact on the system which only leaves the proportional gain. Now the proportional part (KP) would be raised until the system has a constant oscillation where the amplitude stays the same. When this state is achieved the KP value is recorded as K0 and the period time is recorded to T0. These values are then used to determine the PID parameters as can be seen in table 2.2.
Table 2.2. Table showing setup rules for Ziegler-Nichols method, taken from [8].
Regulator
KP KI KDP 0.5K
0PI 0.45K
0 T0/1.2
PID 0.6K
0 T0/2
T0/8
7
CHAPTER 2. THEORY
The Ziegler-Nichols tuning method has proven to not always give a good enough regulator however it is a good starting point to later fine tune the regulator [8].
2.3.5 Trial and error
For this method to work one first needs to have an understanding how each parameter affects the system. As previously written in the section about PID controllers each parameter affects the system differently. The method approached in this project was to first put all values quite low and after changing one parameter running the robot to see if and how the system was reacting differently. By observing the robot a decision had to be made on what parameter needed to be changed next depending on whether the robot had enough power to balance itself up or if the oscillations were to large. It is a fine line between the robot being fast enough or too strong which makes this method quite unreliable.
2.3.6 Kalman filter
The Kalman filter was introduced 1960 by R.E Kalman [10]. In a system noise will disrupt the readings of various sensors. The purpose of the filter is to predict how the system will behave at different times and it can do so even though the nature of said system is unknown.
It works by implementing equations and gives a solution of the least-square method that the controller can compute.
8
Chapter 3
Construction and implementation
This chapter will cover the components used in this project, and how the construction is build up. A CAD-model will be presented to visualize the concept. Due to the lack of financial means the outcome of the project is not what was first planned, but it fulfills its purpose. This is after all just meant to be a prototype, so the cosmetics of the construction is not the highest priority.
3.1 Components
The most crucial components used in this project is presented below.
3.1.1 Motor and motor driver
The motor for this project is a Dunkermotoren GR42X40, which was available in the lab- oratory at the institution. The actual motor is 24V and 21W, with a torque constant of 0.0584 Nm/A, which is enough according to the Matlab simulations that has been executed.
The driver chosen is a VNH3SP30 motor driver, and it is capable of delivering up to 30 amps with a range of 5.5 to 36 volt. Since the Arduino used in this project only is capable of delivering 5 volt, an external power source was required to provide power to the motor.
3.1.2 Reading crucial values
The PID controller is relying on correct values from the parameters discussed in section 2.2.
To provide the controller with the right values an MPU 6050 sensor was used. The sensor is capable of detecting both acceleration and angles, thanks to its onboard sensors. A crucial step in mounting the sensor was to get it lined up with the vertical axis, to get the right values from the gyroscope sensor. The MPU 6050 sensor is providing the controller with both angle acceleration of the cube and the angle the cube is tilting in.
To make the readings more accurate and cancel out the noise from the sensor a Kalman filter was implemented, which make values of the states more stable over time and the controller can act more accurately.
9
CHAPTER 3. CONSTRUCTION AND IMPLEMENTATION
3.2 Construction and software
The construction will be discussed in the following section. Starting off by explaining the test rig and different parts that went into creating it and followed up by the code that was implemented to make it work.
3.2.1 Test Rig
A test rig was built to simulate balancing on the cubes edge in the form of one side and two cutouts designed to give the rig structure and hold the motor as can be seen in figure 3.1 below. Two spacers were 3D-printed to hold everything together and a rod going through all of the parts. The rod was then connected to a base plate and for the cube to be able to rotate around the axis two wheel bearings were connected on each end.
Figure 3.1. The design of the test rig made (Created using Solid Edge 2019).
3.2.2 Reaction wheel
The reaction wheel was first drafted up in Solid edge, setting the dimension so the weight would be the right. The wheel was then waterjet cut and lathed so the hub and spokes were thinner than the outer ring. Doing so the wheel has the most of its mass concentrated at the periphery giving it a higher moment of inertia. The outcome was a wheel with diameter of 15 cm and 5 mm thick.
An axle hub was created out of aluminium to be able to mount the reaction wheel on the 5 mm motor axle.
10
3.2. CONSTRUCTION AND SOFTWARE
3.2.3 Code
The PID controller only need one input value which in this case was the angle of the cube picked up by the accelerometer. The angle readings are filtered with an already existing Kalman filter that was downloaded, before they are sent to the PID regulator. If the angle is greater than -+10 degrees, the angle will not be sent to the PID. This is to prevent the inertia wheel to start spinning out of control when the prototype is lying down.
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 micropro- cessor. As can be seen in figure 3.2 the PID is implemented with the angle of the cube as setpoint.
11
Chapter 4
Results
As can be seen in figure 4.1 below the intended cube ended up in the prototype state, due to the focus being based at making the prototype balance.
Figure 4.1. Picture of finished product seen from the side
The first approach was to try to implement the Ziegler-Nichols method by increasing the KP
until steady oscillations would occur. However due to the unstable nature of the system this state was never achieved. A decision was made to go towards the trial and error method.
This method gave more pleasing results but the prototype was never able to balance on its own with this method neither. With the PID parameter values KP = 15, KI = 0.25 and KD = 0.7 the prototype was rocking back and forth in between two fingers that was
13
CHAPTER 4. RESULTS
blocking it from falling over. A graph is shown in figure 4.2 which illustrates the movement of the cube in such situation and figure 4.3 shows the voltage sent to the motor during the same time.
In a non linear situation such as this one it was concluded that a trial and error method was more effective. By comparing the two graphs it can be seen that the voltage sent to
Figure 4.2. Graph showing the angle of the cube while balancing with support, created with Matlab.
the motor is opposite the angle which shows that the prototype is trying to balance itself up.
Figure 4.3. Graph showing the voltage sent to the motor in the same duration as figure 4.2, created with Matlab.
14
Chapter 5
Discussion
The following chapter will bring up the different obstacles, thoughts and improvements that was encountered during the process.
5.1 Construction
The plan was at the beginning to construct a cube, but after some internal discussion we came to the conclusion that the time should be focused on getting it to work rather than building a quite complicated construction since the function of the robot was thought to be the same. At first the plan was to use a three phased brushless DC motor, mostly because of the smaller design which makes the whole construction more compact and also the high torque relative to the low mass. But after consulting with the assistant for the course, the conclusion was to skip the brushless motor, mostly due to economic reasons. A brushless DC motor would have been beneficial mostly due to low weight and might have proven to make the prototype able to balance.
5.2 Parasitic inductance
The first obstacle that was encountered building the system was parasitic inductance[11].
When the motor was supplied with an alternating voltage an undesired inductance was formed called parasitic inductance. The parasitic inductance caused a magnetic field that disturbed the MPU 6050 which initially gave off readings but after a short while interrupted the signal and stopped giving readings completely. To solve the encountered obstacle a large number of capacitors in varying sizes were used, connected between Vcc and ground both to the MPU 6050 and the motordriver. The reason behind using capacitors was to have a more stable current by reducing the voltage spikes given off by the motor. The parasitic inductance was not removed completely but the system became a lot more stable and the MPU 6050 was able to give better readings.
5.3 Controller
Initially it was planned to use an LQR controller however after discussions with the course assistant it was decided to implement a PID controller instead of using LQR. The reason
15
CHAPTER 5. DISCUSSION
was communication between Matlab and Arduino will cause a delay in the system and Matlab was required to solve the equations for the LQR. To implement the LQR controller the modelling of the system had to be made which is the reason it remains in the thesis.
The PID controller that was implemented instead, never received the correct parameters to make the cube balance. The controller was able to counter falling over on one side however doing so the overshoot put the system in a too large angle on the opposite side and the prototype fell over. A lot of time was spent trying to find the best parameter values with help from the course assistant without prevail.
5.4 Improvements
The inertia wheel was not properly balanced, mainly the dynamic balance. One reason could be the hub not being completely flat on the side mated with the wheel. To improve this a new hub could be created and after properly dynamically balance the wheel, reason for it not being done was not having the proper equipment. The ball bearings had a small play on the axle the test rig was mounted on which could also be a factor as to why the rig was not balancing. Future work would be to implement everything into a cube as was the original idea.
5.5 COVID-19
During the time of this project the pandemic COVID-19 was in full effect. The Swedish government decided to close off all of the higher education which meant all studies were to be performed from distance, meaning no access to the Royal Institute of Technology’s facilities. The communication with the course assistants was mostly through e-mail and workshops online through video conferences. Due to this when an obstacle was encountered a lot of time was spent trying to solve it while receiving suggestions from prior mentioned course assistant. Because of this changes was made to the design, first plan was to create a cube that was able to balance but instead the test rig became the final product. Some parts that was needed but was difficult to create at home was made at KTH and then picked up, the rest was solved by implementing certain parts that was accessible at home.
5.6 Conclusion
With the given time for this project we were not able to make the cube balance however we learned a lot about PID regulators and what obstacles that can occur while working on such a project. Ziegler-Nichols method for tuning PID controllers seems to be difficult to implement on an object that is non linear. It may be our prototype that was too difficult to find an oscillation point because of the instability of the system. With more time a regulator might have been tuned to make the system able to balance, but with a more test oriented method.
16
Bibliography
[1] K.J. ˚ Astr¨om and K. Furuta. “Swinging up a pendulum by energy control”. In:
Automatica
36.2 (2000), pp. 287–295. url: https://doi.org/10.1016/
S0005-1098(99)00140-5 .
[2] Aage Skullestad and James M. Gilbert. “H control of a gravity gradient sta- bilised satellite”. In: Control Engineering Practice 8.9 (2000), pp. 975–983.
url: https://doi.org/10.1016/S0967-0661(00)00044-7.
[3] C.Pao. What is an IMU sensor? 2018. url: https://www.ceva- dsp.
com/ourblog/what-is-an-imu-sensor/ (visited on 05/13/2020).
[4] Erik Bjerke and Bj¨orn Pehrsson. “Development of a Nonlinear Mechatronic Cube”. Master Thesis. Chalmers University of Technology, 2016. url: http:
//publications.lib.chalmers.se/records/fulltext/233543/
233543.pdf .
[5] M. Gajamohan et al. “The Cubli: A cube that can jump up and balance”. In:
2012 IEEE/RSJ International Conference on Intelligent Robots and Systems
. Vilamoura: 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2012, pp. 3722–3727. url: http://ieeexplore.ieee.org.
focus.lib.kth.se/stamp/stamp.jsp?tp=%7B%5C&%7Darnumber=
6385896%7B%5C&%7Disnumber=6385431 .
[6] Nicholas Apazidis. Mekanik II - Partikelsystem, stel kropp och analytisk mekanik.
1:3. Lund: Studentlitteratur AB, 2017, pp. 271–277.
[7] Hans B. Johansson. Elektroteknik. 2013˚ars. Stockholm: Institutionen f¨or Maskinkon- struktion, KTH, 2013, p. 298.
[8] Torkel Glad and Lennart Ljung. Reglerteknik - Grundl¨aggande teori. 4:16.
Lund: Studentlitteratur AB, 2018.
[9] B.Jafari. What are good strategies for tuning PID loops? 2018. url: https:
/ / robotics . stackexchange . com / questions / 167 / what - are - good-strategies-for-tuning-pid-loops (visited on 05/29/2020).
[10] 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 .
17
BIBLIOGRAPHY
[11] 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 .
18
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
C.1 Main code
1 /* KEX 15
2 * Self Balancing Cube
3 * Date: 2020-05-29
4 * Written by: Tim Gidlof and Carl Gruneau 5 * Examiner: Nihad Subasic
6 * TRITA number: TRITA-ITM-EX 2020:35 7 * Course code: MF133X
89 *Bachelors Thesis in mechatronics at KTH 1011 * Description of the code:
12 * This program is intended to make a cube with one DOF balance 13 * on its edge. It’s doing so by reading an angle measurement 14 * from an IMU mpu6050 sensor, filter the angle through a 15 * Kalmanfilter and then use the angle to calculate the right 16 * voltage for the motor. A PID is implemented which takes the 17 * filtered angle and gives an output.
18 */
1920
2122 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
2324 This software may be distributed and modified under the terms of the GNU
25 General Public License version 2 (GPL2) as published by the Free Software
26 Foundation and appearing in the file GPL2.TXT included in the packaging of
27 this file. Please note that GPL2 Section 2[b] requires that all works based
23
APPENDIX C. ARDUINO CODE
28 on this software must also be made publicly available under the terms of
29 the GPL2 ("Copyleft").
3031 Contact information 32 ---
3334 Kristian Lauszus, TKJ Electronics
35 Web : http://www.tkjelectronics.com 36 e-mail : kristianl@tkjelectronics.com 37 */
38 #include <Wire.h>
39 #include <Kalman.h> // Source:
40 https://github.com/TKJElectronics/KalmanFilter
4142 #define RESTRICT_PITCH // Comment out to restrict roll to 90deg instead - please read: http://www.freescale.com/files/sensors /doc/app_note/AN3461.pdf
4344 Kalman kalmanX; // Create the Kalman instances 45 Kalman kalmanY;
4647 /* IMU Data */
48 double accX, accY, accZ;
49 double gyroX, gyroY, gyroZ;
50 int16_t tempRaw;
5152
53 double gyroXangle, gyroYangle; // Angle calculate using the gyro only
54 double compAngleX, compAngleY; // Calculated angle using a complementary filter
55 double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
56 double offsetAngle = 2.7;
5758 uint32_t timer;
59 uint8_t i2cData[14]; // Buffer for I2C data 6061 //motor constants
62 const int inaPin = 12;
63 const int inbPin = 8;
64 const int pwmPin = 6;
65 const int trimPin = A0;
66 int i;
6768 // PID variables
69 double Setpoint, Input, Output, output;
70 double Kp = 15, Ki = 0.25, Kd = 0.7;
24
C.1. MAIN CODE
71 //PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
72 double error=0, integral=0, derivative=0, error_prior=0, integral_prior=0, windup_diff=0, windup=0;
7374
75 void setup() {
7677 Serial.begin(115200);
78 Wire.begin();
79 #if ARDUINO >= 157
80 Wire.setClock(400000UL); // Set I2C frequency to 400kHz 81 #else
82 TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
83 #endif
8485 i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
86 i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
87 i2cData[2] = 0x00; // Set Gyro Full Scale Range to 250deg /s 88 i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to
2g
89 while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
90 while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
9192 while (i2cRead(0x75, i2cData, 1));
93 if (i2cData[0] != 0x68) { // 0x68 Read "WHO_AM_I" register 94 Serial.print(F("Error reading sensor"));
95 while (1);
96 }
9798 delay(100); // Wait for sensor to stabilize 10099 /* Set kalman and gyro starting angle */
101 while (i2cRead(0x3B, i2cData, 6));
102 accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
103 accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
104 accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
105106 // Source: http://www.freescale.com/files/sensors/doc/app_note/
AN3461.pdf eq. 25 and eq. 26
107 // atan2 outputs the value of - to (radians) - see http://
en.wikipedia.org/wiki/Atan2
108 // It is then converted from radians to degrees 109 #ifdef RESTRICT_PITCH // Eq. 25 and 26
110 double roll = atan2(accY, accZ) * RAD_TO_DEG;
25
APPENDIX C. ARDUINO CODE
111 double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
112 #else // Eq. 28 and 29
113 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
114 double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
115 #endif
116117 kalmanX.setAngle(roll); // Set starting angle 118 kalmanY.setAngle(pitch);
119 gyroXangle = roll;
120 gyroYangle = pitch;
121 compAngleX = roll;
122 compAngleY = pitch;
123124 //MOTOR parameters
125 pinMode(inaPin, OUTPUT);
126 pinMode(inbPin, OUTPUT);
127 pinMode(pwmPin, OUTPUT);
128 pinMode(trimPin, INPUT);
129130 // PID parameters 131 Setpoint = 0;
132 //myPID.SetMode(AUTOMATIC);
133 timer = micros();
134 }
135136 // Main loop where all the magic happends 137 void loop(){
138 mpu_kalman(); // reads the values
from the sensor, and filters them
139 Input =( kalAngleY-offsetAngle); // a slight offset is subtracted from the angle to get the
140 // correct input for
the PID 141 Serial.print(kalAngleY-offsetAngle);
142 Serial.print(" ");
143 Serial.print(millis());
144 Serial.print(" ");
145 //Serial.println(error);
146 Serial.println(Output);
147148 // PID-regulator,
149 error = Setpoint - Input; // estimates the error
150 integral = integral_prior + error * 0.005; // integral part 151 derivative = (error - error_prior) / 0.005; // derivative
part
26
C.1. MAIN CODE
152 output = Kp * error + (Ki)* integral + Kd * derivative; //
calculates output with respect to Kp, Ki and Kd 153 error_prior = error;
154 integral_prior = integral;
155156 if (output > 255) 157 Output = 255;
158 else if (output < -255) 159 Output = -255;
160 else
161 Output = output;
162163 // if the angle is bigger than 10 degrees, the cube will do nothing
164 if (abs(Input) > 10){
165 digitalWrite(inaPin, HIGH);
166 digitalWrite(inbPin, HIGH);
167 analogWrite(pwmPin, 0);
168 //Serial.println("STOP");
169 }
170171 // If the angle is between 0 and -10 degrees 172 if (Output < 0 && abs(Input) < 10){
173 digitalWrite(inaPin, LOW);
174 digitalWrite(inbPin, HIGH);
175 analogWrite(pwmPin, (uint8_t) abs(Output));
176 }
177 // If the angle is between 0 and 10 degrees 178 else if (Output >= 0 && abs(Input) < 10 ) { 179 digitalWrite(inaPin, HIGH);
180 digitalWrite(inbPin, LOW);
181 analogWrite(pwmPin, (uint8_t) abs(Output));
182 } 183184 185186 187188 189 }
190191 // void that reads all the values from the MPU650 sensor, calculates the angles
192 // and then filters them 193 void mpu_kalman() {
194 /* Update all the values */
195 while (i2cRead(0x3B, i2cData, 14));
196 accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
197 accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
27
APPENDIX C. ARDUINO CODE
198 accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
199 //tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
200 gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
201 gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
202 gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;
203204 double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
205 timer = micros();
206207 // Source: http://www.freescale.com/files/sensors/doc/app_note/
AN3461.pdf eq. 25 and eq. 26
208 // atan2 outputs the value of - to (radians) - see http://
en.wikipedia.org/wiki/Atan2
209 // It is then converted from radians to degrees 210 #ifdef RESTRICT_PITCH // Eq. 25 and 26
211 double roll = atan2(accY, accZ) * RAD_TO_DEG;
212 double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
213 #else // Eq. 28 and 29
214 double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
215 double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
216 #endif
217218 double gyroXrate = gyroX / 131.0; // Convert to deg/s 219 double gyroYrate = gyroY / 131.0; // Convert to deg/s 220221 #ifdef RESTRICT_PITCH
222 // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
223 if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX <
-90)) {
224 kalmanX.setAngle(roll);
225 compAngleX = roll;
226 kalAngleX = roll;
227 gyroXangle = roll;
228 } else
229 kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); //
Calculate the angle using a Kalman filter 230231 if (abs(kalAngleX) > 90)
232 gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
233 kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
234 #else
235 // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
28
C.2. I2C CODE FOR KALMAN FILTER
236 if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY
< -90)) {
237 kalmanY.setAngle(pitch);
238 compAngleY = pitch;
239 kalAngleY = pitch;
240 gyroYangle = pitch;
241 } else
242 kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); //
Calculate the angle using a Kalman filter 243244 if (abs(kalAngleY) > 90)
245 gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
246 kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
247 #endif
248249 gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
250 gyroYangle += gyroYrate * dt;
251 //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
252 //gyroYangle += kalmanY.getRate() * dt;
253254 compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll
; // Calculate the angle using a Complimentary filter 255 compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 *
pitch;
256257 // Reset the gyro angle when it has drifted too much 258 if (gyroXangle < -180 || gyroXangle > 180)
259 gyroXangle = kalAngleX;
260 if (gyroYangle < -180 || gyroYangle > 180) 261 gyroYangle = kalAngleY;
262263 //Serial.print("\r\n");
264 delay(2);
265266 }
C.2 I2C code for Kalman filter
1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
23 This software may be distributed and modified under the terms of the GNU
4 General Public License version 2 (GPL2) as published by the Free Software
29
APPENDIX C. ARDUINO CODE
5 Foundation and appearing in the file GPL2.TXT included in the packaging of
6 this file. Please note that GPL2 Section 2[b] requires that all works based
7 on this software must also be made publicly available under the terms of
8 the GPL2 ("Copyleft").
10 Contact information9 11 ---
1213 Kristian Lauszus, TKJ Electronics
14 Web : http://www.tkjelectronics.com 15 e-mail : kristianl@tkjelectronics.com 16 */
1718 const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB 19 const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in
I2C communication
2021 uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
22 return i2cWrite(registerAddress, &data, 1, sendStop); //
Returns 0 on success 23 }
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 success
30 if (rcode) {
31 Serial.print(F("i2cWrite failed: "));
32 Serial.println(rcode);
33 }
34 return rcode; // See: http://arduino.cc/en/Reference/
WireEndTransmission 35 }
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 bus
42 if (rcode) {
30
C.2. I2C CODE FOR KALMAN FILTER
43 Wire.endTransmission(true);
44 Wire.beginTransmission(IMUAddress);
45 Wire.write(registerAddress);
46 //Serial.print(F("i2cRead failed: "));
47 //Serial.println(rcode);
48 //return rcode; // See: http://arduino.cc/en/Reference/
WireEndTransmission 49 }
50 Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
51 for (uint8_t i = 0; i < nbytes; i++) { 52 if (Wire.available())
53 data[i] = Wire.read();
54 else {
55 timeOutTimer = micros();
56 while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.
available());
57 if (Wire.available())
58 data[i] = Wire.read();
59 else {
60 Serial.println(F("i2cRead timeout"));
61 return 0; // This error value is not already taken by endTransmission
62 }
63 }
64 }
65 return 0; // Success 66 }
31
Appendix D
Matlab-code
1 c l c
2 c l e a r a l l
3 c l o s e a l l
4
5 %% reads a n g l e s and time from arduino , the a n g l e s i s then p l o t t e d with r e s p e c t to time
6
7 f i l e = fopen(” values . txt ” , ” r ”) ;
8 formatSpec = ”%f ” ;
9 sizeArray = [ 3 I n f] ;
10
11 array = f s c a n f( f i l e , formatSpec , sizeArray ) ;
12 13
14 f c l o s e( f i l e ) ;
15 angle = array ( 1 , : ) ;
16 t i d = ( array ( 2 , : ) − array ( 2 , 1 ) ) ∗10ˆ( −3) ;
17 v o l t = ( array ( 3 , : ) .∗24/256) ;
18
19 f i g u r e(1 )
20 p l o t( tid , angle)
21 g r i d on
22 t i t l e(” Angle ”)
23 x l a b e l(” Time [ s ] ” )
24 y l a b e l(” Angle [ degrees ] ” )
25
26 f i g u r e(2 )
27 p l o t( tid , v o l t ) ;
28 g r i d on
29 t i t l e(” Voltage ”)
30 x l a b e l(” Time [ s ] ” )
31 y l a b e l(” Voltage to the motor [V] ” )
33
TRITA TRITA-ITM-EX 2020:35
www.kth.se
