PID Regulated Balancing Cube · Referat PID-regulerande Balanserande Kub Syftet med detta projekt...

PID Regulated Balancing Cube

Thesis WIP

SEBASTIAN BRANDMAIER, DENIS RAMSDEN

Bachelor’s Thesis at ITMSupervisor: Nihad SubasicExaminer: Nihad Subasic

TRITA-ITM-EX 2020:33

AbstractThis projects purpose were to do a research of a field us-ing mechanical parts, electronics and coding given a limitedset of resources. In this project a balancing cube were con-structed with the intent of it balancing on of one of its edgesusing a PID-controller to counteract the gravity pulling itdown. To be able to this a good understanding of the sys-tem was required to have the right components for the job.A DC motor were used with two reaction wheel on each sideof its axle using the moment of inertia to convert the torquefrom the motor to a angle acceleration of the cube. An IMUwas used to determine the angle difference of the centre ofmass from a fixed point in the room and this value wereused as an input in the PID-controller. Different methodwere used to determine the P, I and D components of thePID-controller to create a stable system. Even though dif-ferent methods were used no one successfully made the cubebalance perfectly.

Keywords: Mechatronics, PID-control, reaction wheel,Arduino, cube

ReferatPID-regulerande Balanserande Kub

Syftet med detta projekt är att undersöka olika forsknings-fr̊agor med hjälp av att använda mekaniska delar, elekt-ronik och kod med en begränsad mängd resurser. Dettaprojekt best̊ar av att en balanserande kub var konstrueraddär tanken var att den ska balansera p̊a en av sina kan-ter med hjälp av en PID-kontroll för att motverka sin egentyngd. För att göra detta behövdes en bra först̊aelse för sy-stemets uppbyggnad för att kunna införskaffa sig rätt kom-ponenter för projektet. En DC motor användes för dettaändam̊al och den använde tv̊a reaktionshjul p̊a vardera sidaav axeln och använde tröghetsmomentet fr̊an dessa för attöverföra momentet fr̊an motorn till en vinkelaccelerationp̊a kuben. En IMU användes för att mäta vinkeländringenav masscentrumet fr̊an en fixt punk i rummet och dettavärde användes som input i PID-kontrollen. Olika metoderanvändes för att bestämma P, I och D komponenterna avPID-kontrollen som gav ett stabilt system. Trots att olikametoder användes lyckades ingen av dessa att f̊a kuben attbalansera felfritt.

Nyckelord: Mekatronik, PID-kontroll, reaktionshjul, Ar-duino, kub

Acknowledgements

We would like to thank our Seshagopalan Thorapalli Muralidharan that has helpus to manufacture all the necessary parts and guided us through this project.

Contents

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

2 Theory 32.1 System dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1.1 Reaction wheel . . . . . . . . . . . . . . . . . . . . . . . . . . 42.1.2 PID-controller . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.2 Components . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.2.1 Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.2.2 Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2.3 Arduino . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.2.4 H-bridge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3 Design 93.1 Implementations of components . . . . . . . . . . . . . . . . . . . . . 9

3.1.1 Gyroscope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93.1.2 Hall effect sensor . . . . . . . . . . . . . . . . . . . . . . . . . 93.1.3 Motor driver . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.2 Shell . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103.3 Reaction wheel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123.4 Measure of motor speed and direction . . . . . . . . . . . . . . . . . 133.5 PID-controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143.6 Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

4 Results 174.1 IMU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174.2 Motor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

4.2.1 PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

5 Discussion 19

Bibliography 21

Appendices 22

A Electric scheme chart 24

B Datasheet for the motor 25

C Source code 27C.1 Motor experiment in Matlab . . . . . . . . . . . . . . . . . . . . . . 27C.2 Arduino code in C++ . . . . . . . . . . . . . . . . . . . . . . . . . . 28

List of Figures

2.1 Gravitational force acting on the cube in horizontal position. Made inSolid Edge . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.2 Angular velocity over time when a voltage of 24V is applied on the motor.Made in Matlab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.3 A schematic image of a H-bridge.Source: https://www.build-electronic-circuits.com/wp-content/uploads/2018/11/H-bridge-switches.png . . . . . . . . . . . . . . . 8

3.1 MPU-6050 - Gyroscope/Accelerometer.Source: https://www.electrokit.com/uploads/productimage/41016/41016233.jpg . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.2 Hall effect sensor.Source: https://www.electrokit.com/uploads/productimage/41015/41015710.jpg . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.3 Motor driver VNH3SP30Source: https://www.electrokit.com/uploads/productimage/41014/41014059.jpg . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.4 A Solid Edge CAD that shows the cube shell with reaction wheels andmotor installed. Made in Solid Edge . . . . . . . . . . . . . . . . . . . . 11

3.5 Section view from the side of cube in Solid Edge CAD showing the motorholders. Made in Solid Edge . . . . . . . . . . . . . . . . . . . . . . . . . 12

3.6 Reaction wheel from the side. Made in Solid Edge . . . . . . . . . . . . 133.7 Sensors for measuring wheel speed. Made in Pixlr Editor . . . . . . . . 143.8 Flowchart for the balancing cube. Made in draw.io. . . . . . . . . . . . . 16

4.1 Measured values from the IMU over time with and without a capacitor.Made in Matlab. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

4.2 Comparison of theoretical rotating speed from equation 2.12 and mea-sured values. Made in Matlab. . . . . . . . . . . . . . . . . . . . . . . . 18

5.1 Comparison of theoretical rotating speed from equation 2.12 and mea-sured values, adjusted moment of inertia. Made in Matlab . . . . . . . . 20

https://www.build-electronic-circuits.com/wp-content/uploads/2018/11/H-bridge-switches.pnghttps://www.build-electronic-circuits.com/wp-content/uploads/2018/11/H-bridge-switches.pnghttps://www.electrokit.com/uploads/productimage/41016/41016233.jpghttps://www.electrokit.com/uploads/productimage/41016/41016233.jpghttps://www.electrokit.com/uploads/productimage/41015/41015710.jpghttps://www.electrokit.com/uploads/productimage/41015/41015710.jpghttps://www.electrokit.com/uploads/productimage/41014/41014059.jpghttps://www.electrokit.com/uploads/productimage/41014/41014059.jpg

Chapter 1

Introduction

1.1 BackgroundThe project we have chosen is to construct a cube that can balance on one of itsedges. The idea is to make it balance by rotating a reaction wheel with a motorcreating a torque with the moment of inertia making the cube counteracts its ownweight. The acceleration of the wheels is then adjusted relative to the angle betweenthe cube and the horizontal plane to keep the cube stable.

1.2 PurposeWe were inspired by the fact that the design of the physical product seems verysimple, but in reality it is based on a complex control theory problem. Althoughthe final product will not solve any everyday problems, the technology is importantas it provides insight into how control theory can be applied in real projects. Theexperience of controlling motors and handling signals can benefit later projects sincesimilar problems exists in many areas. Examples of areas where this technology isuseful are for designing segways, rockets and aircrafts etc. This study will mainlyfocus on answering following questions:

• What is a convenient way to construct a cube with the necessary hardware tobalance on an edge using a reaction wheel?

• Is it possible with the resources available to balance the cube with a PIDcontroller and what tuning method should be used?

1.3 ScopeThe focus of this project is to design and construct a fully functional prototype ofa product. This regarding a a budget of 1000 SEK and a project deadline of threemonths. Other control systems could work for the same control problem but in thisproject a PID controlled system will be analyzed.

1

CHAPTER 1. INTRODUCTION

1.4 MethodA physical Cube will be constructed to make experiment to be able to answer thestudy questions and all construction of the necessary components will take place atthe KTH Prototype Center. Arduino uno will be used as a development platformin this project to control the components. Mathematical models of the system willbe made to be able to analyze it and create a PID control system making the cubebalance. After that the components for the PID will be tuned until the system isfully stable.

2

Chapter 2

Theory

2.1 System dynamicsTo be able to stabilize the cube on its corner a relationship between the differentaffecting factors is needed and a model of the system dynamics is set up. Mmaxis the maximum torque needed to counteract the construction´s own weight at ahorizontal position and is given by equation 2.1 where a mechanical equilibriumgives this relationship between the cube’s mass m, gravitation g and the horizontallength to its center of gravity l2 according to

Mmax = mgl

2 . (2.1)

Figure 2.1. Gravitational force acting on the cube in horizontal position. Made inSolid Edge

3

CHAPTER 2. THEORY

This sets the requirement for needed torque from the motor. An important aspectis to make the construction mechanically stable because the mechanical stabilitywill set a fundamental stability to the construction that gives a good starting pointfor the automatic control later. This is done by letting the center of mass beingcentered in the middle of the cube.

2.1.1 Reaction wheelThe most suitable shape of a reaction wheel is a ring because it provides the great-est moment of inertia per mass that will result in a decreased acceleration. Thetheoretical moment of inertia for a ring can be calculated with the ring´s mass mand it´s radius r as [2]

Iring = mr2. (2.2)When the motor accelerate the reaction wheel generates torque. Due to Newtonsthird law, a reaction torque that acts on the cube in the opposite direction is alsogenerated. This can be described by the equation below with Iring as the wheelstheoretical moment of inertia and the angle acceleration ω̇motor of it as [2]

Mmotor = Iringω̇motor. (2.3)

By adjusting the speed and acceleration of the motor the angle between the cubeand the surface can be controlled. In this project a Newtonian model was used thatcan be described as [2]

Itotθ̈ = mglsin(θ)−Mmotor (2.4)

2.1.2 PID-controllerA PID-controller is the most commonly used controller in the industry. The con-troller use a reference point that is the value it want to reach and compare it withits current value and this difference is the error. It then uses the error to stabilizethe system with three components. PID means that the controller consists of threecomponents: a proportional, an integrative and a derivative component. The P-component is used to adjust the gain, the I-component is used to reduce the staticerror, and the D-component compensates for changes in the system that increasesthe stability. [8]Since arduino includes a built in a PID-function within its libraries it will be usedin this project. The PID-function has three parameters KP ,KI , and KD that willbe choosen experimentally. [3] The ideal PID-regulator can be written as:

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

t0e(τ)dτ +KD

de(t)dt

. (2.5)

There is different ways a PID-controller can be tuned to stabilize a system. Onemethod is the Ziegler-Nicholas method where analysis of the system could give a

4

2.2. COMPONENTS

good approximation of the three components creating the PID-controller. Given asystem the simplest method is to increase the gain for the system until a steadystate oscillation occurs, giving the Kcr. This with the given oscillation period Pcrcan with this method give a good starting point for tuning the PID. The methodsuggests the following values in relation with the analysed valued above: [6]

KP = 0.6Kcr, (2.6)

TI = 0.5Pcr, (2.7)

TD = 0.125Pcr. (2.8)

Using equation 2.5 the PID-controller can now be written as

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

∫ tt0e(τ)dτ + TD

de(t)dt) . (2.9)

2.2 Components

2.2.1 Motors

In this project a brushed DC motor was used. A brushed motor means that it rotatesinternally. DC stands for direct current which implies that ohm’s law and Kirchhoff’svoltage law can be used in calculations. The relationship between voltage, currentand velocity can therefore be describe by [9]

UA = RAIA +K2Φω. (2.10)

The torque that the motor generates is proportional to the current and is mathe-matically described as [9]

M = K2ΦIA. (2.11)

Combining equation 2.5 and 2.6 results in that the total motor torque is [9]

M = K2ΦRA

U − K2Φ2ω

RA. (2.12)

K2Φ is a constant that can be calculated by measuring the speed of the rotor shaftwhen there is no load on the motor. This gives that the voltage is direct proportionalto the rotating speed (UA = K2Φω). In this project it was instead found in thedata sheet for the motor. Ra is the terminal resistance can be measured, but wasalso found in the data sheet. [9]

5

CHAPTER 2. THEORY

Figure 2.2. Angular velocity over time when a voltage of 24V is applied on themotor. Made in Matlab

The equation 2.7 and 2.3 was used to plot how the angular velocity of the flywheelchanges over time when the voltage 24V is applied on the motor. The angularvelocity was calculated with the Euler forward method and depends on the torquethat was received from equation 2.7. In the data sheet the ”no load speed” for themotor is 3900 rpm. The reason why the motor passes this speed despite there is loadon the motor might be because the used model does not consider internal frictionof the motor. A theoretical value of the load was calculated by approximating theflywheel as a ring. The moment of inertia could the be calculated with equation2.2.

2.2.2 Sensors

Hall effect sensor

To be able to control the angular velocity of the reaction wheels a Hall effect sensoris used. The sensor can sense a magnetic field due to the disturbance of electronsand sends a signal to the micro controller when it sense one. If a magnet is placedon the reaction wheel the hall effect sensor will sense a disturbance for every spin ofthe wheel. With that the micro controller can give us an accurate angular velocity.With two hall effect sensors it is possible to determine the acceleration direction incases were the motor is directed in a different way than the velocity, for exampleduring deacceleration.

6

2.2. COMPONENTS

Gyro/accelometer

To know how the cube is positioned according to the room a gyro/accelometer isused. The gyrometer´s origo is set to the position the cube is supposed to balancein. When the cube is falling in some direction the gyrometer will give a signal to themicro controller of this change in angle. The micro controller will then do actionsto stabilize the cube and bring its position back to origo.

2.2.3 Arduino

For this project an Arduino UNO micro controller was used. Arduino is an opensource development platform which makes it easy to control motors and sensor. Thisis done using built in input and output pins. Arduino UNO uses an ATmega328Pprocessor which is programmable in a language very similar to C++. It has manyof built in libraries which makes it easy to control sensors and motors. ArduinoUNO is an 8 bit controller which uses a reference voltage of 5V. Therefore whenit sends analog signals it converts an int between 0-255 to a PWM signal between0-5V. [7],[5]

2.2.4 H-bridge

To drive a DC-motor with higher voltage it is necessary to use a H-bridge. Thisis due to the Arduino´s inability to output voltage over 5 V. A H-bridge is usedto control an external power source with small PWM-signals described in section2.2.3. Conventionally an H-bridge is made of four transistors that not only makes itpossible to change the output voltage, it can also change the current direction. Another reason why it is important to use an H-bridge is because DC-motors also canwork as generators. Therefore if some external force would make the motor startspinning it would induce a current which could destroy the arduino. [9]

7

CHAPTER 2. THEORY

Figure 2.3. A schematic image of a H-bridge.Source: https://www.build-electronic-circuits.com/wp-content/uploads/2018/11/H-bridge-switches.png

8

https://www.build-electronic-circuits.com/wp-content/uploads/2018/11/H-bridge-switches.pnghttps://www.build-electronic-circuits.com/wp-content/uploads/2018/11/H-bridge-switches.png

Chapter 3

Design

3.1 Implementations of components

3.1.1 Gyroscope

The gyroscope used in this project is the MPU-6050 accelerometer and gyroscope.It give values for the change of angle on the z-axle which was used as input for thePID-controller to control the control system.

Figure 3.1. MPU-6050 - Gyroscope/Accelerometer.Source: https://www.electrokit.com/uploads/productimage/41016/41016233.jpg

3.1.2 Hall effect sensor

The hall effect sensors were used to measure the angular velocity of the reactionwheel. This was done by mounting two magnets on the reaction wheel and thesensors on the motor holder giving a good distance between these. The sensorssends a signal every time the magnet is close to it with a function in Arduino thatmeasure past time gives us the angular velocity. This because one of the sensorswill send the signal right before the other.

9

https://www.electrokit.com/uploads/productimage/41016/41016233.jpghttps://www.electrokit.com/uploads/productimage/41016/41016233.jpg

CHAPTER 3. DESIGN

Figure 3.2. Hall effect sensor.Source: https://www.electrokit.com/uploads/productimage/41015/41015710.jpg

3.1.3 Motor driverA H-bridge is used to control the voltage over the motor and the choice of thespecific H-bridge depended on it to be able to output a current as high as 12 A andcontrol a voltage up to 24 V. The best suited motor driver was the VNH3SP30 andtherefore it was used. It can output a current up to 30 A and voltages up to 40 V.[11], [10]

Figure 3.3. Motor driver VNH3SP30Source: https://www.electrokit.com/uploads/productimage/41014/41014059.jpg

3.2 ShellThe cube is designed to have all of its components within its sides with the sidesacting as a protective shell and its corner as good balancing points. The shell willconsist of six plates that make up the cubes sides that will be laser cut from acrylicglass with an thickness of three millimeter. Acrylic glass was chosen because of itslow density and good enough yield strength.

In every corner three v-brackets is used to connect all plates to each other us-ing M5 screws and nuts, that is visible in figure 3.4. To connect the motor to the

10

https://www.electrokit.com/uploads/productimage/41015/41015710.jpghttps://www.electrokit.com/uploads/productimage/41015/41015710.jpghttps://www.electrokit.com/uploads/productimage/41014/41014059.jpghttps://www.electrokit.com/uploads/productimage/41014/41014059.jpg

3.2. SHELL

cube we are using two other plate acting as motor holders that is connected to twoopposite sides with distance screws in each corner of the plates, see figure 3.5. Themotor is mounted on the motor holders with M4 screws. The two reaction wheelsis placed on the motor axle on opposite sides between the side of the cube and themotor holders, see figure 3.5.

Figure 3.4. A Solid Edge CAD that shows the cube shell with reaction wheels andmotor installed. Made in Solid Edge

11

CHAPTER 3. DESIGN

Figure 3.5. Section view from the side of cube in Solid Edge CAD showing themotor holders. Made in Solid Edge

3.3 Reaction wheelThe wheel’s purpose is to generate as much moment of inertia per mass, thereforethe shape of a ring. Two wheels with a radius of 5,5 centimeter and the weight of150 g where created to generate it.

The reaction wheel is made of steel and was water cut because of its simplicitywith the wheel only needed to be cut in one dimension. The wheel is then mountedon the motor axle with a hub that is connected directly on the reaction wheel withsix M3 screws and then locked on the axle with two set screws from the hub

A requirement for the wheel is to be able to store enough energy that it takesfor the cube to tilt to an angle of 45 ◦. The kinetic energy that the wheel can storeis [1]

T = l2Iringω2max (3.1)

where ωmax is the maximum speed [rad/s] and Iring is the moment of inertia that isdescribed in equation 2.2. The energy it needs to store is the difference in potentialenergy between the states when it is lying horizontal on the surface and balancingat a 45 ◦ angle. This is mathematically described [1]

V =√

2mcubegl

2 (3.2)

where mcube is the mass of the cube [kg], g is gravitational acceleration [m/s2] andl is the length of the cube. By using equation 2.2 and setting V equal to T thewheels minimum mass can be calculated to

12

3.4. MEASURE OF MOTOR SPEED AND DIRECTION

mmin,ring =√

2mcubegω2maxr

2ring

(3.3)

The Buehler 24V motor has a rated speed at 3000 rpm and with that as ωmax itgives that the wheel must weight at least 93 g. Since the wheel is not ideally a ringa mass of 150 g gives a big margin.

Figure 3.6. Reaction wheel from the side. Made in Solid Edge

3.4 Measure of motor speed and direction

To be able to control the DC motor it was necessary to measure the speed and di-rection of motor. Since there was no rotary encoder available a method described inthe research ”Reaction Wheel Stabilized Stick” [12] was used instead. This methoduses two hall effect sensor and two magnets, but the number of magnets was in-creased to four to reduce vibrations that occur if the reaction wheel is unbalanced.The mounting of the magnets is shown below in figure 3.7.

13

CHAPTER 3. DESIGN

Figure 3.7. Sensors for measuring wheel speed. Made in Pixlr Editor

To measure the wheel speed a built in function in called micros() was used. Itreturns the time in microseconds since the Arduino was powered on. If both halleffect sensors detects a magnet the wheel speed [rad/s] can be calculated by [4]

ω = π∆t (3.4)

where ∆t [s] is the time difference between the current and the previous measure. Todetermine which direction the wheel is rotating a variable called state was defined.Before both sensors detects a magnet at the same time, one of the magnets willhave been detected by one of the hall effect sensor. This information is stored inthe state variable and is used to determine the wheel rotating direction.

3.5 PID-controllerTo construct a stable system with a PID-controller, the three components KP , KIand KD need to be tuned right. With a block diagram on the system with a transferfunction it is possible to theoretically find these components.

14

3.6. CODE

For this system, equation 2.4 can be used for small angles with the assumption thatsin(θ) ≈ θ and with the laplace transform of equation 2.4 the transfer function forthis system can be given with

θ(s)s2Itot = Mmotor(s)−mglθ(s), (3.5)

where the output of the system can be written as

θ(s) = 1s2Itot +mglθ

Mmotor(s). (3.6)

The laplace tranform of the equation 2.8 can be written as:

Mmotor(s) =K2ΦRA

U(s)− K2Φ2

RAω(s). (3.7)

3.6 Code

The code was written in the Arduino IDE. At the beginning of the document allvariables was defined. In the setup function the I/O pins was initialize as inputsor outputs. To make it easy to use the IMU a library written by Jeff Rowberg wasdownloaded and included in the file. With this library it comes an example codethat was rewritten to suit the rest of the program. Also a PID library made byBrett Beauregard was downloaded and included.

The PID function uses an input signal to calculate an output signal dependingon the parameter Kp, Ki and Kd. The used input signal is the a angle that isreceived from the IMU. The output signal is used as a PWM signal and regulatesthe speed of the motor. The PID values was calculated in the loop function and istherefore updated repetitively when the program is running.

The analog output was used on the hall effect sensors to detect the magnets if themagnet field was greater than a set value. This method worked better than usingthe digital output. Since the void function is running repetitively a boolean vari-able was used to prevent that the code does not measure the velocity many timeswhen a magnet is detected. If the boolean is 1 when the sensor detects a magnet itmeasures the velocity, after it is set to 0. When the sensor doesn’t detect a magnetanymore the boolean is set to 1 again.

Below in figure 3.8 is a flowchart of the balancing cube that describes the structureof written code for the Arduino.

15

CHAPTER 3. DESIGN

Figure 3.8. Flowchart for the balancing cube. Made in draw.io.

16

Chapter 4

Results

4.1 IMU

At the beginning of the development process the MPU-6050 that was used as anIMU did not seem to work correctly. It took many seconds for the IMU to stabilizeand it did not always stabilize at the same values. This problem was solved with acapacitor. The MPU-6050 is sensitiv to voltage variations and the voltage supplyfrom the arduino is not always reliable. The capacitor compensated for this andthe result is shown in figure 4.1. A similar problem also occurred when the gainwas increased in the PID-controller. By adding a capacitor over the power supplyit helped sustain a steady voltage even when the gain was increased.

Figure 4.1. Measured values from the IMU over time with and without a capacitor.Made in Matlab.

17

CHAPTER 4. RESULTS

4.2 MotorAn experiment was done to determine the deviation between the mathematicalmodel used in figure 2.2 and the real dynamics of the motor. All constant valuesfor the mathematical model was received from the datasheet for the motor. Theused motor was a Buhler 24V with terminal resistance Ra = 2Ω and the torqueconstant k2φ = 5.7Ncm. In figure 4.2 it is shown that there is a big differencebetween the theoretical values and the measured values. [10]

Figure 4.2. Comparison of theoretical rotating speed from equation 2.12 and mea-sured values. Made in Matlab.

4.2.1 PIDThe PID components were determined by experiments on the cube. The KP com-ponent was increased until the value made the cube counteract its own weight whenfalling to one side. Though the controller was able to counteract the cube falling itcouldn’t counteract the response from the controller and the cube fell to the otherside, the system was unstable. To compensate for this response a KD componentwas added that will counteract the response based on the speed of the error change.When the cube oscillates with only the KP component the KD component will de-crease the oscillation, making the system more stable. Due to the controller havingthe error as input it will only respond when an error occur making the system neversettling to the set point. Adding a KI component calculating the integral of theerror over time will make the system even more stable. Though using this methodto find accurate components for the PID controller the system in this project couldnot stabilize completely, but made the system many times more stable than usingno controller.

18

Chapter 5

Discussion

During this project a research has been done in how a balancing cube should beconstructed. The chosen design of screwing the sides of the cube together with threeattachments in each corner resulted in a very robust design. Using laser printersto cut out all the necessary parts from acrylic glass also benefited the constructionsince this method gave high precision. It worked very well to use two plates asmotor attachments. This design was very stable and also allowed all the sensor tobe mounted on the plates. A disadvantage with this design is that it only works fortwo dimensional balancing. For multidimensional balancing another motor must beadded that would not fit in this construction.

No PID parameters where found that could balance the cube. There are manypossible reasons to why it didn’t work. Despite that a lot of focus have been ondeveloping a mechanically balanced cube there is a risk that weight is not equallydistributed on both sides. Also the angles from the gyro does not always settle atthe same point that causes problem in the controlling of the cube. Also the weightof the motor makes it harder to control the cube because a low power to weightratio reduces the maximum deviation angle from the equilibrium point that is whenthe angle is zero. There are also methods to iterate the PID parameters that issomething that could be investigated.

This project was inspired by Cubli, a cube that was built at ETH Zurich. Forthe cube to be able to jump up from horizontal position to balancing on an edge itwould require a voltage of approximately 36 V. An alternative to this would be todesign a brake system with servomotors that can perform the ”jump” action andthen a smaller motor can balance the cube for smaller angles. This would not onlyreduce the required voltage and current supply, but also free space which is veryimportant if the project would expand to multi dimensional balancing. It wouldalso be interesting to use brushless motors since they generally have a higher torqueper weight ratio.

19

CHAPTER 5. DISCUSSION

At first the dynamic model of the DC motor described in section 2.2.1 was in-tended to be used to regulate the motor torque so it corresponded with the com-puted output from the PID controller. Since it was difficult to know how reliablethe measurement of the calculated velocity from the hall effect sensors were, theoutput was instead directly used to control the PWM signal to the motor.

Another thing that can be analysed is why the result from the experiment andthe theoretical values deviated so much in figure 4.2. When the angular velocityis small it looks like the two lines are parallel that is good because the torque isproportional to the slope. When the velocity gets higher it differs a lot. This mightbe because the internal friction is not considered in mathematical model that canbe assumed to be proportional to the rotating velocity. Also the moment of inertiamight have been underestimated. [1]

Figure 5.1. Comparison of theoretical rotating speed from equation 2.12 and mea-sured values, adjusted moment of inertia. Made in Matlab

In figure 5.1 the reaction wheels moment of inertia has been increased with a factorof 2. Now the model looks more similar to the experimental values, but they curvesstill have different characteristics and therefore no conclusion can be taken.

20

Bibliography

[1] Nicholas Apazidis. Mekanik 1 Statik och partikeldynami.Studentlitteratur, 2013.

[2] Nicholas Apazidis. Mekanik 2 Partikelsystem, stel kropp och analytisk mekanik.Studentlitteratur, 2012.

[3] Arduino.cc. PID.https://playground.arduino.cc/Code/PIDLibraryConstructor/retrieved 2020-03-29

[4] Arduino.cc. micros().https://www.arduino.cc/reference/en/language/functions/time/micros/ retrieved 2020-05-11

[5] Arduino.cc. What is arduino?.https://www.arduino.cc/en/Guide/Introductionretrieved 2020-05-12

[6] Dr. Hodge Jenkins. Tuning for PID Controllers, http://faculty.mercer.edu/jenkins_he/documents/TuningforPIDControllers.pdf, retrieved 2020-05-12

[7] Arduino.cc. Ardunio uno rev3.https://store.arduino.cc/arduino-uno-rev3 retrieved 2020-05-12

[8] Torkel Glad, Lennart Ljung. Reglerteknik: grundläggande teori. Studentlitter-atur, 2006

[9] Hans Johansson. Elektroteknik. Instutitionen för maskinkonstruktion och meka-tronik, 2013

[10] Buehlermotor. DC Motor 51x88.https://www.buehlermotor.com/fileadmin/user_upload/stock_service/datasheets/DC-Motor_51x88__1.13.044.2XX.pdf?fbclid=IwAR1ru7WgIEmUk8lEha7CSzmgLtvLIcKwwQ4AvKP0RA68nEo4glcVtn7Yeh0,retrieved 2020-05-11

21

https://playground.arduino.cc/Code/PIDLibraryConstructor/https://www.arduino.cc/reference/en/language/functions/time/micros/https://www.arduino.cc/reference/en/language/functions/time/micros/https://www.arduino.cc/en/Guide/Introductionhttp://faculty.mercer.edu/jenkins_he/documents/TuningforPIDControllers.pdfhttp://faculty.mercer.edu/jenkins_he/documents/TuningforPIDControllers.pdfhttps://store.arduino.cc/arduino-uno-rev3https://www.buehlermotor.com/fileadmin/user_upload/stock_service/datasheets/DC-Motor_51x88__1.13.044.2XX.pdf?fbclid=IwAR1ru7WgIEmUk8lEha7CSzmgLtvLIcKwwQ4AvKP0RA68nEo4glcVtn7Yeh0https://www.buehlermotor.com/fileadmin/user_upload/stock_service/datasheets/DC-Motor_51x88__1.13.044.2XX.pdf?fbclid=IwAR1ru7WgIEmUk8lEha7CSzmgLtvLIcKwwQ4AvKP0RA68nEo4glcVtn7Yeh0https://www.buehlermotor.com/fileadmin/user_upload/stock_service/datasheets/DC-Motor_51x88__1.13.044.2XX.pdf?fbclid=IwAR1ru7WgIEmUk8lEha7CSzmgLtvLIcKwwQ4AvKP0RA68nEo4glcVtn7Yeh0

BIBLIOGRAPHY

[11] ST. Fully integrated H-bridge motor driver.https://www.electrokit.com/uploads/productfile/41014/vnh3sp30.pdf,retrieved 2020-05-12

[12] Pontus Gräsberg, Bill Lavebratt. Reaction Wheel Stabilized Stick.KTH. 2019. oai:DiVA.org:kth-264481

22

https://www.electrokit.com/uploads/productfile/41014/vnh3sp30.pdf

APPENDIX A. ELECTRIC SCHEME CHART

Appendix A

Electric scheme chart

24

Appendix B

Datasheet for the motor

25

Appendix C

Source code

C.1 Motor experiment in Matlab

c l e a r a l l , c l o s e a l l , c l c %c l ean command window and workspace}%% Mathematical motor model :

format long

%mehanical cons tant sm cube = 2 % Total mass o f th cube [ kg ]l cube = 15∗10ˆ(−2) % length o f cube [m]m wheel = 0.300 % f l ywhee l mass [ kg ]r whee l = (11/2) ∗10ˆ(−2) % rad iu s o f the f l ywhee l [m]I whee l = m wheel∗ r whee l ˆ2 % momen o f i n e r t i a o f the f l y whee l [ kgmˆ2 ]I motor = 180∗10ˆ(−7) % momen o f i n e r t i a o f the ro to r s h a f t [ kgmˆ2 ]

J = I whee l+I motor ;

%motor v a r i b l e s :omega = 0 ; % Angular v e l o c i t yu = 8 % Voltage [V]

%Motor s p e c i f i c a t i o n s :Ra = 2 % Terminal r e s i s t a n c e [ ohm ]Kt = 5.7∗10ˆ(−2) %Torque constant Nm/AU max = 8 % max vo l tage [V]

% Other cons tant sN = 500 % Number o f i t e r a t i o n s

%%Motor dynamics :t = 5 % t i d [ s ]

27

APPENDIX C. SOURCE CODE

dU = U max/(N−1)dt = t /(N−1)

T = [ 0 : dt : t ] ;

M = [ ]OMEGA = [ ] ;

f o r x = Ttorque = (Kt/Ra) ∗U max−(((Ktˆ2) /Ra) ) ∗omega ;

a c c e l e r a t i o n = torque /J ;omega = omega + a c c e l e r a t i o n ∗ ( t /(N−1) ) ;

OMEGA = [OMEGA omega ] ;

M = [M torque ] ;end

OMEGA = OMEGA∗60/(2∗ pi )

f i g u r e

p l o t (T,OMEGA)x l a b e l ( ’Time [ s ] ’ )y l a b e l ( ’ Angular v e l o c i t y [ rpm ] ’ )s e t ( gcf , ’ c o l o r ’ , ’w ’ ) ;hold on%t i t l e ( ’U=24V’ )

%% Values from experiment

data = load ( ”kex /8 v motor . txt ” )

Time = ( data ( 1 : 6 6 6 , 1 ) /(10ˆ6) ) −1.3;rpm = data ( 1 : 6 6 6 , 2 ) ∗ 0 . 5 ;p l o t (Time , rpm)legend ( ’ Theo r e t i c a l va lue s ’ , ’ Values from experiment ’ )

C.2 Arduino code in C++

#inc lude

// Var iab l e s f o r gyrodouble r o l l d e g ;

// Var iab l e s f o r PIDdouble input ;

28

C.2. ARDUINO CODE IN C++

double output ;double s e t p o i n t ;double Kp = 5 , Ki=0, Kd=0;

PID PID cont ro l l e r (&input , &output , &se tpo in t , Kp, Ki , Kd, DIRECT) ;double pwmSignal ;

// Var iab l e s f o r motorconst i n t inaPin = 13 ;const i n t inbPin = 9 ;const i n t pwmPin = 11 ;const i n t diagaPin = 10 ;const i n t diagbPin = 12 ;const i n t buttonPin = 2 ;const i n t trimPin = A0 ;i n t i = 0 ;

// I2C dev i ce c l a s s ( I2Cdev ) demonstrat ion Arduino sketch f o r MPU6050c l a s s us ing DMP ( MotionApps v2 . 0 )

// 6/21/2012 by J e f f Rowberg // Updates should ( h o p e f u l l y ) always be a v a i l a b l e at https : // github . com

/ jrowberg / i 2 c d e v l i b//// Changelog :// 2013−05−08 − added seamles s Fastwire support// − added note about gyro c a l i b r a t i o n// 2012−06−21 − added note about Arduino 1 . 0 . 1 + Leonardo

c o m p a t i b i l i t y e r r o r// 2012−06−20 − improved FIFO over f l ow handl ing and s i m p l i f i e d

read proce s s// 2012−06−19 − complete ly rearranged DMP i n i t i a l i z a t i o n code and

s i m p l i f i c a t i o n// 2012−06−13 − p u l l gyro and a c c e l data from FIFO packet in s t ead

o f read ing d i r e c t l y// 2012−06−09 − f i x broken FIFO read sequence and change i n t e r r u p t

d e t e c t i o n to RISING// 2012−06−05 − add grav i ty−compensated i n i t i a l r e f e r e n c e frame

a c c e l e r a t i o n output// − add 3D math he lpe r f i l e to DMP6 example sketch// − add Euler output and Yaw/ Pitch / Rol l output formats// 2012−06−04 − remove a c c e l o f f s e t c l e a r i n g f o r b e t t e r r e s u l t s (

thanks Sungon Lee )// 2012−06−01 − f i x e d gyro s e n s i t i v i t y to be 2000 deg/ sec in s t ead

o f 250// 2012−05−30 − bas i c DMP i n i t i a l i z a t i o n working

/∗ ============================================I2Cdev dev i ce l i b r a r y code i s p laced under the MIT l i c e n s eCopyright ( c ) 2012 J e f f Rowberg

Permiss ion i s hereby granted , f r e e o f charge , to any person obta in ing acopy

o f t h i s so f tware and a s s o c i a t e d documentation f i l e s ( the ” Software ”) ,

29


to dea lin the Software without r e s t r i c t i o n , i n c l u d i n g without l i m i t a t i o n the

r i g h t sto use , copy , modify , merge , publ i sh , d i s t r i b u t e , sub l i c en s e , and/ or

s e l lc o p i e s o f the Software , and to permit persons to whom the Software i sf u rn i shed to do so , s u b j e c t to the f o l l o w i n g c o n d i t i o n s :

The above copyr ight n o t i c e and t h i s permis s ion n o t i c e s h a l l be inc ludedin

a l l c o p i e s or s u b s t a n t i a l po r t i on s o f the Software .

THE SOFTWARE IS PROVIDED ”AS IS ” , WITHOUT WARRANTY OF ANY KIND, EXPRESSOR

IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALLTHE

AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHERLIABILITY , WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

FROM,OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS

INTHE SOFTWARE.===============================================∗/

// I2Cdev and MPU6050 must be i n s t a l l e d as l i b r a r i e s , or e l s e the . cpp/ . h f i l e s

// f o r both c l a s s e s must be in the inc lude path o f your p r o j e c t#inc lude ” I2Cdev . h”

#inc lude ”MPU6050 6Axis MotionApps20 . h”//#inc lude ”MPU6050 . h” // not nece s sa ry i f us ing MotionApps inc lude

f i l e

// Arduino Wire l i b r a r y i s r equ i r ed i f I2Cdev I2CDEV ARDUINO WIREimplementation

// i s used in I2Cdev . h#i f I2CDEV IMPLEMENTATION == I2CDEV ARDUINO WIRE

#inc lude ”Wire . h”#e n d i f

// c l a s s d e f a u l t I2C address i s 0x68// s p e c i f i c I2C addre s s e s may be passed as a parameter here// AD0 low = 0x68 ( d e f a u l t f o r SparkFun breakout and InvenSense

eva lua t i on board )// AD0 high = 0x69MPU6050 mpu;//MPU6050 mpu(0 x69 ) ; //


NOTE: In add i t i on to connect ion 3 .3 v , GND, SDA, and SCL, t h i s sketchdepends on the MPU−6050 ’ s INT pin being connected to the Arduino ’ se x t e r n a l i n t e r r u p t #0 pin . On the Arduino Uno and Mega 2560 , t h i s i sd i g i t a l I /O pin 2 .

∗=========================================================================

∗/

/∗=========================================================================

NOTE: Arduino v1 . 0 . 1 with the Leonardo board gene ra t e s a compi lee r r o r

when us ing S e r i a l . wr i t e ( buf , l en ) . The Teapot output uses t h i smethod .

The s o l u t i o n r e q u i r e s a mod i f i c a t i on to the Arduino USBAPI . h f i l e ,that

i s f o r t u n a t e l y simple , but annoying . This w i l l be f i x e d in the nextIDE

r e l e a s e . For more in fo , s e e these l i n k s :

http :// arduino . cc / forum/ index . php/ topic , 1 0 9 9 8 7 . 0 . htmlhttp :// code . goog l e . com/p/ arduino / i s s u e s / d e t a i l ? id =958

∗=========================================================================

∗/

// uncomment ”OUTPUT READABLE QUATERNION” i f you want to see the ac tua l// quatern ion components in a [w, x , y , z ] format ( not bes t f o r par s ing// on a remote host such as Proce s s ing or something though )//#d e f i n e OUTPUT READABLE QUATERNION

// uncomment ”OUTPUT READABLE EULER” i f you want to see Euler ang l e s// ( in degree s ) c a l c u l a t e d from the quatern ions coming from the FIFO .// Note that Euler ang l e s s u f f e r from gimbal l ock ( f o r more in fo , s e e// http :// en . w ik iped ia . org / wik i / Gimbal lock )//#d e f i n e OUTPUT READABLE EULER

// uncomment ”OUTPUT READABLE YAWPITCHROLL” i f you want to see the yaw/// p i t ch / r o l l ang l e s ( in degree s ) c a l c u l a t e d from the quatern ions

coming// from the FIFO . Note t h i s a l s o r e q u i r e s g rav i ty vec to r c a l c u l a t i o n s .// Also note that yaw/ p i t ch / r o l l ang l e s s u f f e r from gimbal l o ck ( f o r// more in fo , s e e : http :// en . w ik iped ia . org / wik i / Gimbal lock )#d e f i n e OUTPUT READABLE YAWPITCHROLL

// uncomment ”OUTPUT READABLE REALACCEL” i f you want to seea c c e l e r a t i o n

// components with g rav i ty removed . This a c c e l e r a t i o n r e f e r e n c e framei s

// not compensated f o r o r i e n t a t i o n , so +X i s always +X accord ing to the// sensor , j u s t without the e f f e c t s o f g rav i ty . I f you want

31


a c c e l e r a t i o n// compensated f o r o r i e n t a t i o n , us OUTPUT READABLE WORLDACCEL ins t ead .//#d e f i n e OUTPUT READABLE REALACCEL

// uncomment ”OUTPUT READABLE WORLDACCEL” i f you want to seea c c e l e r a t i o n

// components with g rav i ty removed and adjusted f o r the world frame o f// r e f e r e n c e (yaw i s r e l a t i v e to i n i t i a l o r i e n t a t i o n , s i n c e no

magnetometer// i s pre sent in t h i s case ) . Could be qu i t e handy in some ca s e s .//#d e f i n e OUTPUT READABLE WORLDACCEL

// uncomment ”OUTPUT TEAPOT” i f you want output that matches the// format used f o r the InvenSense teapot demo//#d e f i n e OUTPUT TEAPOT

#d e f i n e LED PIN 13 // ( Arduino i s 13 , Teensy i s 11 , Teensy++ i s 6)bool b l i n k S ta t e = f a l s e ;

// MPU c o n t r o l / s t a t u s varsbool dmpReady = f a l s e ; // s e t t rue i f DMP i n i t was s u c c e s s f u lu i n t 8 t mpuIntStatus ; // ho lds ac tua l i n t e r r u p t s t a t u s byte from MPUu i n t 8 t devStatus ; // re turn s t a t u s a f t e r each dev i ce opera t i on (0

= succes s , ! 0 = e r r o r )u i n t 1 6 t packetS i ze ; // expected DMP packet s i z e ( d e f a u l t i s 42

bytes )u i n t 1 6 t f i f oCount ; // count o f a l l bytes c u r r e n t l y in FIFOu i n t 8 t f i f o B u f f e r [ 6 4 ] ; // FIFO sto rage b u f f e r

// o r i e n t a t i o n /motion varsQuaternion q ; // [w, x , y , z ] quatern ion conta ine rVectorInt16 aa ; // [ x , y , z ] a c c e l s enso r

measurementsVectorInt16 aaReal ; // [ x , y , z ] grav i ty−f r e e a c c e l

s enso r measurementsVectorInt16 aaWorld ; // [ x , y , z ] world−frame a c c e l

s enso r measurementsVectorFloat g rav i ty ; // [ x , y , z ] g rav i ty vec to rf l o a t e u l e r [ 3 ] ; // [ ps i , theta , phi ] Euler ang le conta ine rf l o a t ypr [ 3 ] ; // [ yaw , pitch , r o l l ] yaw/ p i t ch / r o l l

c on ta ine r and g rav i ty vec to r

// packet s t r u c t u r e f o r InvenSense teapot demou i n t 8 t teapotPacket [ 1 4 ] = { ’ $ ’ , 0x02 , 0 ,0 , 0 ,0 , 0 ,0 , 0 ,0 , 0x00 , 0x00 ,

’ \ r ’ , ’ \n ’ } ;

// ================================================================// === INTERRUPT DETECTION ROUTINE ===// ================================================================

32


v o l a t i l e bool mpuInterrupt = f a l s e ; // i n d i c a t e s whether MPUi n t e r r u p t pin has gone high

void dmpDataReady ( ) {mpuInterrupt = true ;

}

// ================================================================// === INITIAL SETUP ===// ================================================================

void setup ( ) {// Setup f o r PIDs e t p o i n t = 20 ;

P ID cont ro l l e r . SetMode (AUTOMATIC) ;P ID cont ro l l e r . SetTunings (Kp, Ki ,Kd) ;P ID cont ro l l e r . SetOutputLimits (−255 , 255) ;

// j o i n I2C bus ( I2Cdev l i b r a r y doesn ’ t do t h i s automat i ca l l y )#i f I2CDEV IMPLEMENTATION == I2CDEV ARDUINO WIRE

Wire . begin ( ) ;TWBR = 24 ; // 400kHz I2C c lo ck (200kHz i f CPU i s 8MHz)

#e l i f I2CDEV IMPLEMENTATION == I2CDEV BUILTIN FASTWIREFastwire : : setup (400 , t rue ) ;

#e n d i f

// I /O: s f o r motor p inspinMode ( buttonPin , INPUT) ;pinMode ( inaPin , OUTPUT) ;pinMode ( inbPin , OUTPUT) ;pinMode (pwmPin , OUTPUT) ;pinMode ( diagaPin , INPUT) ;pinMode ( diagbPin , INPUT) ;pinMode ( trimPin , INPUT) ;

// Example code f o r MPU6050 downloaded

// i n i t i a l i z e s e r i a l communication// (115200 chosen because i t i s r equ i r ed f o r Teapot Demo output ,

but i t ’ s// r e a l l y up to you depending on your p r o j e c t )S e r i a l . begin (115200) ;whi l e ( ! S e r i a l ) ; // wait f o r Leonardo enumeration , o the r s cont inue

immediately

// NOTE: 8MHz or s lower host p roce s so r s , l i k e the Teensy @ 3 .3 v orArdunio

// Pro Mini running at 3 .3 v , cannot handle t h i s baud ra t e r e l i a b l ydue to

// the baud timing being too misa l i gned with p r oc e s s o r t i c k s . Youmust use

// 38400 or s lower in these cases , or use some kind o f e x t e r n a l

33


s epara te// c r y s t a l s o l u t i o n f o r the UART timer .

// i n i t i a l i z e dev i c eS e r i a l . p r i n t l n (F( ” I n i t i a l i z i n g I2C d ev i c e s . . . ” ) ) ;mpu. i n i t i a l i z e ( ) ;

// v e r i f y connect ionS e r i a l . p r i n t l n (F( ” Test ing dev i c e connec t i ons . . . ” ) ) ;S e r i a l . p r i n t l n (mpu. tes tConnect ion ( ) ? F( ”MPU6050 connect ion

s u c c e s s f u l ” ) : F( ”MPU6050 connect ion f a i l e d ” ) ) ;

// wait f o r readyS e r i a l . p r i n t l n (F( ”\nSend any charac t e r to begin DMP programming and

demo : ” ) ) ;whi l e ( S e r i a l . a v a i l a b l e ( ) && S e r i a l . read ( ) ) ; // empty b u f f e rwhi l e ( ! S e r i a l . a v a i l a b l e ( ) ) ; // wait f o r datawhi l e ( S e r i a l . a v a i l a b l e ( ) && S e r i a l . read ( ) ) ; // empty b u f f e r again

// load and c o n f i g u r e the DMPS e r i a l . p r i n t l n (F( ” I n i t i a l i z i n g DMP. . . ” ) ) ;devStatus = mpu. d m p I n i t i a l i z e ( ) ;

// supply your own gyro o f f s e t s here , s c a l e d f o r min s e n s i t i v i t ympu. setXGyroOffset (220) ;mpu. setYGyroOffset (76) ;mpu. setZGyroOffset (−85) ;mpu. s e tZAcce lO f f s e t (1788) ; // 1688 f a c t o r y d e f a u l t f o r my t e s t chip

// make sure i t worked ( r e tu rn s 0 i f so )i f ( devStatus == 0) {

// turn on the DMP, now that i t ’ s readyS e r i a l . p r i n t l n (F( ” Enabling DMP. . . ” ) ) ;mpu. setDMPEnabled ( t rue ) ;

// enable Arduino i n t e r r u p t d e t e c t i o nS e r i a l . p r i n t l n (F( ” Enabling i n t e r r u p t d e t e c t i o n ( Arduino

e x t e r n a l i n t e r r u p t 0) . . . ” ) ) ;a t t a ch In t e r rup t (0 , dmpDataReady , RISING) ;mpuIntStatus = mpu. ge t In tS ta tu s ( ) ;

// s e t our DMP Ready f l a g so the main loop ( ) func t i on knows i t ’s okay to use i t

S e r i a l . p r i n t l n (F( ”DMP ready ! Waiting f o r f i r s t i n t e r r u p t . . . ” ) ) ;dmpReady = true ;

// get expected DMP packet s i z e f o r l a t e r comparisonpacketS i z e = mpu. dmpGetFIFOPacketSize ( ) ;

} e l s e {// ERROR!// 1 = i n i t i a l memory load f a i l e d// 2 = DMP c o n f i g u r a t i o n updates f a i l e d// ( i f i t ’ s going to break , u su a l l y the code w i l l be 1)S e r i a l . p r i n t (F( ”DMP I n i t i a l i z a t i o n f a i l e d ( code ” ) ) ;

34


S e r i a l . p r i n t ( devStatus ) ;S e r i a l . p r i n t l n (F( ” ) ” ) ) ;

}

// c o n f i g u r e LED f o r outputpinMode (LED PIN , OUTPUT) ;

}

// ================================================================// === MAIN PROGRAM LOOP ===// ================================================================

void loop ( ) {// i f programming f a i l e d , don ’ t t ry to do anythingi f ( ! dmpReady) re turn ;

// wait f o r MPU i n t e r r u p t or ext ra packet ( s ) a v a i l a b l ewhi l e ( ! mpuInterrupt && f i f oCount < packetS i z e ) {

// other program behavior s t u f f here// .// .// .// i f you are r e a l l y paranoid you can f r e q u e n t l y t e s t in

between other// s t u f f to s ee i f mpuInterrupt i s true , and i f so , ” break ; ”

from the// whi l e ( ) loop to immediately p roce s s the MPU data// .// .// .

}

// r e s e t i n t e r r u p t f l a g and get INT STATUS bytempuInterrupt = f a l s e ;mpuIntStatus = mpu. ge t In tS ta tu s ( ) ;

// get cur rent FIFO countf i f oCount = mpu. getFIFOCount ( ) ;

// check f o r over f l ow ( t h i s should never happen u n l e s s our code i stoo i n e f f i c i e n t )

i f ( ( mpuIntStatus & 0x10 ) | | f i f oCount == 1024) {// r e s e t so we can cont inue c l e a n l ympu. resetFIFO ( ) ;S e r i a l . p r i n t l n (F( ”FIFO over f l ow ! ” ) ) ;

// otherwise , check f o r DMP data ready i n t e r r u p t ( t h i s shouldhappen f r e q u e n t l y )

} e l s e i f ( mpuIntStatus & 0x02 ) {// wait f o r c o r r e c t a v a i l a b l e data length , should be a VERY

shor t waitwhi l e ( f i f oCount < packetS i z e ) f i f oCount = mpu. getFIFOCount ( ) ;

35


// read a packet from FIFOmpu. getFIFOBytes ( f i f o B u f f e r , packetS i ze ) ;

// t rack FIFO count here in case the re i s > 1 packet a v a i l a b l e// ( t h i s l e t s us immediately read more without wai t ing f o r an

i n t e r r u p t )f i f oCount −= packetS i ze ;

#i f d e f OUTPUT READABLE QUATERNION// d i s p l a y quatern ion va lue s in easy matrix form : w x y zmpu. dmpGetQuaternion(&q , f i f o B u f f e r ) ;S e r i a l . p r i n t ( ” quat\ t ” ) ;S e r i a l . p r i n t ( q .w) ;S e r i a l . p r i n t ( ”\ t ” ) ;S e r i a l . p r i n t ( q . x ) ;S e r i a l . p r i n t ( ”\ t ” ) ;S e r i a l . p r i n t ( q . y ) ;S e r i a l . p r i n t ( ”\ t ” ) ;S e r i a l . p r i n t l n ( q . z ) ;

#e n d i f

#i f d e f OUTPUT READABLE EULER// d i s p l a y Euler ang l e s in degree smpu. dmpGetQuaternion(&q , f i f o B u f f e r ) ;mpu. dmpGetEuler ( eu l e r , &q ) ;S e r i a l . p r i n t ( ” e u l e r \ t ” ) ;S e r i a l . p r i n t ( e u l e r [ 0 ] ∗ 180/M PI) ;S e r i a l . p r i n t ( ”\ t ” ) ;S e r i a l . p r i n t ( e u l e r [ 1 ] ∗ 180/M PI) ;S e r i a l . p r i n t ( ”\ t ” ) ;S e r i a l . p r i n t l n ( e u l e r [ 2 ] ∗ 180/M PI) ;

#e n d i f

#i f d e f OUTPUT READABLE YAWPITCHROLL// d i s p l a y Euler ang l e s in degree smpu. dmpGetQuaternion(&q , f i f o B u f f e r ) ;mpu. dmpGetGravity(&grav i ty , &q ) ;mpu. dmpGetYawPitchRoll ( ypr , &q , &grav i ty ) ;// S e r i a l . p r i n t (” ypr\ t ”) ;// S e r i a l . p r i n t ( ypr [ 0 ] ∗ 180/M PI) ;// S e r i a l . p r i n t (”\ t ”) ;// S e r i a l . p r i n t ( ypr [ 1 ] ∗ 180/M PI) ;// S e r i a l . p r i n t (”\ t ”) ;// S e r i a l . p r i n t l n ( ypr [ 2 ] ∗ 180/M PI) ;

#e n d i f

#i f d e f OUTPUT READABLE REALACCEL// d i s p l a y r e a l a c c e l e r a t i o n , ad justed to remove g rav i tympu. dmpGetQuaternion(&q , f i f o B u f f e r ) ;mpu. dmpGetAccel(&aa , f i f o B u f f e r ) ;mpu. dmpGetGravity(&grav i ty , &q ) ;mpu. dmpGetLinearAccel(&aaReal , &aa , &grav i ty ) ;S e r i a l . p r i n t ( ” a r e a l \ t ” ) ;

36


S e r i a l . p r i n t ( aaReal . x ) ;S e r i a l . p r i n t ( ”\ t ” ) ;S e r i a l . p r i n t ( aaReal . y ) ;S e r i a l . p r i n t ( ”\ t ” ) ;S e r i a l . p r i n t l n ( aaReal . z ) ;

#e n d i f

#i f d e f OUTPUT READABLE WORLDACCEL// d i s p l a y i n i t i a l world−frame a c c e l e r a t i o n , ad justed to

remove g rav i ty// and rotated based on known o r i e n t a t i o n from quatern ionmpu. dmpGetQuaternion(&q , f i f o B u f f e r ) ;mpu. dmpGetAccel(&aa , f i f o B u f f e r ) ;mpu. dmpGetGravity(&grav i ty , &q ) ;mpu. dmpGetLinearAccel(&aaReal , &aa , &grav i ty ) ;mpu. dmpGetLinearAccelInWorld(&aaWorld , &aaReal , &q ) ;S e r i a l . p r i n t ( ” aworld\ t ” ) ;S e r i a l . p r i n t ( aaWorld . x ) ;S e r i a l . p r i n t ( ”\ t ” ) ;S e r i a l . p r i n t ( aaWorld . y ) ;S e r i a l . p r i n t ( ”\ t ” ) ;S e r i a l . p r i n t l n ( aaWorld . z ) ;

#e n d i f

#i f d e f OUTPUT TEAPOT// d i s p l a y quatern ion va lue s in InvenSense Teapot demo

format :teapotPacket [ 2 ] = f i f o B u f f e r [ 0 ] ;teapotPacket [ 3 ] = f i f o B u f f e r [ 1 ] ;teapotPacket [ 4 ] = f i f o B u f f e r [ 4 ] ;teapotPacket [ 5 ] = f i f o B u f f e r [ 5 ] ;teapotPacket [ 6 ] = f i f o B u f f e r [ 8 ] ;teapotPacket [ 7 ] = f i f o B u f f e r [ 9 ] ;teapotPacket [ 8 ] = f i f o B u f f e r [ 1 2 ] ;teapotPacket [ 9 ] = f i f o B u f f e r [ 1 3 ] ;S e r i a l . wr i t e ( teapotPacket , 14) ;teapotPacket [11]++; // packetCount , l oops at 0xFF on

purpose#e n d i f

// b l i nk LED to i n d i c a t e a c t i v i t yb l i n k S t a t e = ! b l i nk S ta t e ;d i g i t a l W r i t e (LED PIN , b l i n k S t a t e ) ;

}

input = r o l l d e g = ypr [ 1 ] ∗ 180/M PI ;// S e r i a l . p r i n t l n ( r o l l d e g ) ;

P ID cont ro l l e r . Compute ( ) ;

S e r i a l . p r i n t ( input ) ;S e r i a l . p r i n t ( ”\ t ” ) ;S e r i a l . p r i n t l n ( output ) ;

37


i f ( output >= 0) {pwmSignal = output ;d i g i t a l W r i t e ( inaPin , LOW) ; //CW d i r e c t i o n o f motor .d i g i t a l W r i t e ( inbPin , HIGH) ;

} e l s e {pwmSignal = −1∗output ;d i g i t a l W r i t e ( inaPin , HIGH) ; //CCW d i r e c t i o n o f motor .d i g i t a l W r i t e ( inbPin , LOW) ;

}

analogWrite (pwmPin , pwmSignal ) ;

}

38

IntroductionBackgroundPurposeScopeMethod

TheorySystem dynamicsReaction wheelPID-controller

ComponentsMotorsSensorsArduinoH-bridge

DesignImplementations of componentsGyroscopeHall effect sensorMotor driver

ShellReaction wheelMeasure of motor speed and directionPID-controllerCode

ResultsIMUMotorPID

DiscussionBibliographyAppendicesElectric scheme chartDatasheet for the motorSource codeMotor experiment in MatlabArduino code in C++

PID Regulated Balancing Cube · Referat PID-regulerande Balanserande Kub Syftet med detta projekt...

Documents

Transcript of PID Regulated Balancing Cube · Referat PID-regulerande Balanserande Kub Syftet med detta projekt...