IN
DEGREE PROJECT MECHANICAL ENGINEERING, FIRST CYCLE, 15 CREDITS
,
STOCKHOLM SWEDEN 2021
Balancing a Monowheel with a PID
controller
Balansering av ett Monowheel med
hjälp av en PID regulator
FRITIOF ANDERSEN EKVALL
NILS WINNERHOLT
KTH ROYAL INSTITUTE OF TECHNOLOGY
Balancing a monowheel with a PID controller
FRITIOF ANDERSEN EKVALL NILS WINNERHOLT
Bachelor’s Thesis at ITM Supervisor: Nihad Subasic
Examiner: Nihad Subasic
Abstract
This bachelor’s thesis aimed to create a self balancing monowheel, a vehicle type consisting of one wheel, using a PID con-troller. The wheel is equipped with an accelerometer to gather data about the tilt of the construction, which is then filtered using a Kalman filter. A DC motor is propelling the monowheel forward whereas a stepper motor with a battery pack attached will actively balance the wheel with the help of a PID-controller. This method of balancing had limited success allowing the vehicle to travel for up to 7 seconds before falling over, compared with up to 4 seconds with no balancing implemented.
Referat
Balansera ett monowheel med en PID
kontroller
Detta kandidatexamensarbete har m˚alet att skapa ett sj¨alvbalanserande hjul med hj¨alp av en PID kontroller tillsammans med en
ac-celeromter vars utsignal filtreras med ett kalman filter. En DC motor driver hjulet framm˚at medan en stepper motor med ett batteripack fastsatt ¨ar den r¨orliga vikten som ba-lanserar konstruktionen. PID metoden lyckades balansera upp till 7 sekunder vilket ¨ar en marginell ¨okning j¨amf¨ort med upp till 4 sekunder helt utan aktiv balansering.
Acknowledgements
We would like to thank all our classmates who have all been very helpful. We would also like to offer a special thanks to Rayan Alnakar for his help and encouraging words.
Abbreviations
CAD Computer Aided Design DC Direct Current
IDE Integrated Development Environment PID Proportional Integral Derivative PWM Pulse Width Modulation USB Universal Serial Bus CoG Center of Gravity KF Kalman filter
Contents
1 Introduction 1 1.1 Background . . . 1 1.2 Purpose . . . 1 1.3 Scope . . . 1 1.4 Method . . . 2 2 Theory 3 2.1 Arduino . . . 32.2 Gyroscope and accelerometer . . . 3
2.3 DC and stepper motor . . . 4
2.4 H-Bridge . . . 5 2.5 PID . . . 5 2.6 Kalman filter . . . 6 3 Demonstrator 9 3.1 Problem formulation . . . 9 3.2 Powertrain . . . 11 3.3 Simulation . . . 11 3.4 Component selection . . . 11 3.5 Software . . . 12 4 Results 13 5 Conclusions and Discussion 17 5.1 Discussion . . . 17
5.1.1 Design . . . 17
5.1.2 Kalman filter . . . 17
5.1.3 PID . . . 17
5.1.4 Stepper motor and the natural balance of the construction . 18 5.1.5 Proposal for future work . . . 18
5.2 Conclusions . . . 18
Appendices 20
A Arduino 21
List of Figures
2.1 An Arduino Uno compatible developement board [14] . . . 3
2.2 MPU6050: gyroscope and accelerometer chip [15] . . . 4
2.3 Circuit diagram of DC motor with a power source [5] . . . 4
2.4 Circuit diagram of H-bridge [13] . . . 5
2.5 Block diagram of an implementation of a PID controller. Drawn in Paint 6 3.1 CAD model of the construction. Made in SolidEdge 2020 University Edition . . . 10
3.2 Exploded view of the CAD model. Made in SolidEdge 2020 University Edition . . . 10
3.3 Wiring diagram of the electronics for the monowheel. Made in fritzing 0.9.3 . . . 11
4.1 The constructed prototype. Photo taken by Nils Winnerholt . . . 13
List of Tables
4.1 Table of the PID paramters used . . . 14 4.2 Table of the time balanced with different methods of balancing . . . 14
Chapter 1
Introduction
1.1
Background
A monowheel is a vehicle with only a single wheel that has all of its driving action taking place inside of the wheel. Since the construction only has one point of contact to the ground it suffers from instability if it starts to tilt. Thus a form of stabilization is often implemented where the most common stabilization method is a shifting of the wheels’ center of gravity[1]. A classical monowheel has a person sitting inside the machine balancing it. This version will use a method for stabilizing using a shiftable weight to alter the center of gravity. Another possible method of stabilization is control moment gyroscope which uses a gyroscope paired with motors and flywheel, as was done in a previous bachelor’s project by Arthur Gr¨onlund and Christos Tolis[2]. The challenge of creating a method of stabilization for such a vehicle has enticed us to start this project.
1.2
Purpose
The purpose of this project is to create a stable monowheel using a weight shifting stabilization method. The project aims to answer the research question:
”How well can a monowheel be stabilized using an Arduino microcontroller cou-pled with a shiftable weight using PID regulation?”
1.3
Scope
This project will focus on keeping the monowheel stabilized running in a straight line forwards and backwards using a PID controller. How to get the monowheel to turn is an interesting aspect of further investigation that is beyond the scope of this project.
CHAPTER 1. INTRODUCTION
1.4
Method
A prototype will be constructed using a 3D printer. One battery pack will be the shiftable weight and another will be placed as low as possible for increased stability and balancing the weight and the DC motor. Utilizing the battery packs like this saves weight as no additional material is needed to be added for the purpose as ballast. The shifting of the battery pack will be done by a stepper motor which the battery pack is mounted to and moves perpendicular to the forward motion of the monowheel. An accelerometer and gyroscope will sense the current angle of tilt and then send that information to an Arduino that will control the positioning of the shiftable weight.
To assess where and how to place the motors and electronic controllers as effi-ciently as possible a CAD drawing will be made of the chosen prototype and then different placing schemes for the component mounts will be tested inside of the CAD software. This will allow us to try out multiple geometries at no extra cost.
Chapter 2
Theory
2.1
Arduino
Arduino is a brand of open source microcontrollers, which are small computers made on a single board. Arduino comes in many different models with their own best areas of implementation. In this project an Playknowlogy Uno Rev.3 Arduino compatible developement board is used, which can bee seen in figure 2.1. The microcontroller is attached to a board that has a series of both digital and analog input/output pins. Electrical components such as sensors and motors can be connected to those pins and controlled using the programmable microcontroller. It is programmed by writing code in the Arduino IDE v1.8.13 and then transferring that code onto the microcontroller using a USB cable[3].
Figure 2.1. An Arduino Uno compatible developement board [14]
2.2
Gyroscope and accelerometer
An acceleromemeter is a sensor that detects its own acceleration in space. A gy-roscope measures its own rotation and angular velocity. With these two types of sensors an accurate representation of an objects orientation in space can be
CHAPTER 2. THEORY
structed. The MPU6050 pictured in figure 2.2 is a sensor that combines a 3 axis accelerometer and a 3 axis gyroscope into one part. The gyroscope measure the an-gular rate by measuring the displacement of a mass inside of the sensor due to the Coriolis effect. The mass will move, causing the capacitance between the mass and fixed plates to change. The accelerometer also work by measuring the capacitance between a mass and a fixed plate, but does it by mounting the mass on springs that allow the mass to be offset from its steady state when it is subjected to an acceleration[4].
Figure 2.2. MPU6050: gyroscope and accelerometer chip [15]
2.3
DC and stepper motor
A DC motor has three main parts: rotor, stator and commutator. It uses direct current to induce an electromagnetic force through the commutator to the rotor located within the stator which translates to torque and mechanical energy. It can be adjusted through altering the voltage supplied to the motor. The relation follows the equation:
UA= RAIA+ k2Φω (2.1)
with UAas the voltage, RAis the internal resistance of the motor, IA is the current going over it, k2Φ is a motor specific constant and ω is the angular velocity[5].
Figure 2.3. Circuit diagram of DC motor with a power source [5]
A stepper motor has a similar construction to that of a DC motor but it’s method of control is different. The stepper motor rotates in steps with one or a pair
2.4. H-BRIDGE
of electromagnets powering on for each step[6]. It is possible to change the amount of steps to make smaller individual movements with microstepping, up to 6400 from the regular 200[16].
2.4
H-Bridge
The H-Bridge is an electrical circuit switches polarity of the voltage for a DC motor, thus allowing for control of the direction it spins. It does this with the help of four transistors that function like power switches. When transistor S1 and S4 in figure 2.4 are activated while S2 and S3 are deactivated the current flows into the positive pole of the motor and out of the negative pole, thus making the motor start spinning. To make the motor spin the other way S2 and S3 are activated while S1 and S4 are deactivated[5].
Figure 2.4. Circuit diagram of H-bridge [13]
2.5
PID
The PID controller is a mechanism utilized in closed feedback loops in order to get the desired output y from a system. This is done by comparing the current output of the system with a reference signal r and then taking the difference between them
CHAPTER 2. THEORY
to get the error e.
e(t) = r(t) − y(t) (2.2)
Using the error, the input signal to the system in the following way:
u(t) = KPe(t) + KI Z t 0 e(τ )dτ + KD d dte(t) (2.3)
The equation for the input signal has three different coefficients that correspond to different properties of the controller. KP is the proportional gain and improves the
rise time of the system and decreases the steady state error, but does not eliminate it, introduces overshoot to the system and causes instability if increased to much.
KI is the integrated gain and eliminates the steady state error and improves the
rise time further, but adds additional overshoot and instability at high values. The overshoot is corrected for with the last parameter KD is the derivative gain and
also decreases instability. Fine tuning of the coefficients allows the system to get the desired properties[7] .
Figure 2.5. Block diagram of an implementation of a PID controller. Drawn in
Paint
2.6
Kalman filter
Kalman filter, also called linear quadratic estimation is a recursive algorithm. Here it will be used for reducing the noise from the sensors[8]. To use a Kalman filter, first the Kalman gain will be calculated with EEST as the error (uncertainty) of the estimate and EM EA is the error in the measurment :
KG = EEST EEST + EM EA
(2.4)
The estimate error is calculated with:
EEST = (1 − KG)EESTprev (2.5)
The estimation EST updates regarding the previous estimate ESTprevand the
mea-surment M EA through the Kalman gain KG:
2.6. KALMAN FILTER
EST = ESTprev+ KG(M EA − ESTprev) (2.6)
As the estimate error decreases the Kalman gain will decrease allowing the the estimate to depend more on the previous estimate and less on the measurement[9].
Chapter 3
Demonstrator
3.1
Problem formulation
The demonstrator needed to fulfill a number of requirements in order to function in the desired way. Those functions are:
1. Powertrain that transfers the rotation of a DC-motor to the outer wheel 2. Accelerometer that detects the deviation from upright
3. Balancing mechanism that adjusts the CoG of the monowheel
To test and improve the theory, a prototype will be constructed. The shell is mod-elled in CAD and 3D printed. Two motors, their drivers, sensors and the Arduino mounted inside shown below in figure 3.1 and figure 3.2.
CHAPTER 3. DEMONSTRATOR
Figure 3.1. CAD model of the construction. Made in SolidEdge 2020 University
Edition
Figure 3.2. Exploded view of the CAD model. Made in SolidEdge 2020 University
Edition
3.2. POWERTRAIN
3.2
Powertrain
The powertrain consists of an outer gear with four inner gears. One of the inner gears is driven by the DC motor sitting on the side of figure 3.1.
3.3
Simulation
To verify the wiring it was first drawn up and tested in Tinkercad and fritzing as seen in figure 3.3 before being physically connected. First for the DC motor and afterwards for the stepper motor and last for the gyroscope and accelerometer sensor.
Figure 3.3. Wiring diagram of the electronics for the monowheel. Made in fritzing
0.9.3
3.4
Component selection
A 24V DC motor is coupled with a stepper motor with a driver accepting 8-45V making it possible to run the same voltage for them both. This will be achieved with 9V batteries connected both in parallel and in series for 18V and extra capacity.
CHAPTER 3. DEMONSTRATOR
3.5
Software
The tuning of the PID parameters has been done with the help of PIDtuner[17], a website that allows for the optimization of parameters when supplied with data from the use of a controller. The parameters were fine tuned by repeated use of the software.
Arduino IDE was used to program the Arduino Uno microcontroller. In the code a set of libraries were utilized to aid in the programs simplicity. The inbuilt Wire.h library allows for communication with I2C and TWI devices. The PIDv2.h library, which was created by Brett Beauregard, creates a PID-controller using parameters that the users chooses[18].
Chapter 4
Results
The prototype was constructed using 3D-printed parts held together with screws and reinforced with tape at connection points. Some parts were too large to print in one piece so were split into modules that could fit onto the printing surface. The final PID tuning parameters used are shown below in table 4.1.
Figure 4.1. The constructed prototype. Photo taken by Nils Winnerholt
CHAPTER 4. RESULTS
Table 4.1. Table of the PID paramters used
KP 0.7764 KI -0.004131
KD -72.97
The balance of the monowheel was tested while rolling forward by timing how long it could roll forward on a slightly uneven surface. The test was performed us-ing different balancus-ing methods to evaluate the dependency of the balance method for the performance of the monowheel. The tested balancing methods were PID-controller+Kalman filter, PID-controller and P-controller. The test was also per-formed with no active balancing as a negative control.
As seen in figure 4.1, with no active balancing the monowheel where balanced for approximately four seconds. The result was similar for all the three tries. The longest balancing period was obtained when a PID-controller was used to balanced the monowheel and was approximately 7 seconds. The shortest balancing period was also obtained when using a PID-controller. When using a PID-controller to-gether with a Kalman filter, the monowheel was balanced for approximately 4.5 seconds. As summarized in table 4.2 below.
Table 4.2. Table of the time balanced with different methods of balancing
PID+KF PID P No balancing Average time (ms) 4485 4897 3568 4189
Best time (ms) 4724 6920 4439 4334
0 1000 2000 3000 4000 5000 6000 7000 8000 Time (ms) -40 -20 0 20 40 Roll (°) PID-controller + Kalman Test 1 Test 2 Test 3 0 1000 2000 3000 4000 5000 6000 7000 8000 Time (ms) -20 -10 0 10 20 30 40 Roll (°) PID-controller Test 1 Test 2 Test 3 0 1000 2000 3000 4000 5000 6000 7000 8000 Time (ms) -40 -30 -20 -10 0 10 Roll (°) P-controller Test 1 Test 2 Test 3 0 1000 2000 3000 4000 5000 6000 7000 8000 Time (ms) -20 -15 -10 -5 0 5 10 Roll (°) No active balancing Test 1 Test 2 Test 3
Figure 4.2. Test results for different types of balancing strategies
Chapter 5
Conclusions and Discussion
5.1
Discussion
This section will address and allow discussion regarding the results and the project as a whole.
5.1.1 Design
The play in the support cogs and the side covers allows a bit too much movement sometimes resulting in the wheel getting caught on a side cover, this makes balancing harder and also heavily increases the wear on the DC motor mounted cog (which has been replaced during testing). The cog for the stepper motors balancing motion also sometimes gets stuck which results loss of the active balance and a lot of vibrations causing the accelerometer to give out quite varied values.
5.1.2 Kalman filter
The kalman filter lowers the error from the accelerometer readings but it also incurs a small time penalty that does have some impact for the balancing of the monowheel as the immediate balance is quite time sensitive. Testing has not shown a big differ-ence with the kalman filter included or not suggesting that the increased accuracy might not weigh up for the, albeit slight, time penalty.
5.1.3 PID
A PID controller is a great method for balance. It takes time, a lot of testing and adjusting of parameters for it to reach it’s potential. The balance of the monowheel could probably be improved substantially with the right tuning parameters. Ziegler-Nichols method is one way of finding better values for the parameters. In it’s current state it is sometimes slow, sometime over corrects and does only increase the balance moderately.
CHAPTER 5. CONCLUSIONS AND DISCUSSION
5.1.4 Stepper motor and the natural balance of the construction
There are more possibilities with the use of a stepper motor, most of the testing is done with the 200 base steps. For a smoother motion microstepping up to 6400 steps can be implemented. The attached battery-packs weight on the stepper motor aids the change in CoG it causes with it’s movement but it is a quite light weight in comparison to the whole monowheel. Thus reducing it’s ability to ”save” the monowheel from exaggerated tilt angles. The DC motor has a large part of the total weight which also hangs out quite a bit from the center line of the wheel, to somewhat compensate for this one battery-pack and the powerbank for the arduino has been placed opposite the DC motor. This results in less movable weight and less impact from the stepper motors movement.
5.1.5 Proposal for future work
Other solutions might be better suited for this purpose, such as gyroscopic balance. Involving the DC motor in the algorithm for example: starting with a certain speed then when the monowheel starts tilting a small increase in the speed increases the gyroscopic effects and thus increasing the stability of the monowheel. Furthermore, a more powerful DC motor would provide extra speed for the monowheel, which would improve the stabilization even further. With more precise manufacturing of parts, less play and vibrations can be expected, probably improving on the design.
5.2
Conclusions
It is possible to balance the monowheel to some extent using an Arduino and a PID controller with the method used in this report. The balancing system increased the the average time per run by a second and at its best allowed for a doubling of time spent upright. The construction is however not optimal since the vibrations from both the powertrain and the balancing mechanisms introduced disturbances in the data that caused the balancing to fail.
Bibliography
[1] Patryk Cieslak, Tomasz Buratowsk, Tadeusz Uhl, Mariusz Giergiel,
The Mono-Wheel Robot With Dynamic Stabilization,
Robotics and Autonomous Systems, 59:611–619, 2011
[2] DEGREE PROJECT MECHANICAL ENGINEERING,FIRST CYCLE, 15 CREDITS, STOCKHOLM SWEDEN 2018 Arthur Gr¨onlund, Christos Tolis,
Riderless self-balancing bicycle,
http://kth.diva-portal.org/smash/get/diva2:1237256/FULLTEXT01.pdf [3] Arduino. Date accessed: 2021-05-09
https://www.arduino.cc/
[4] MEMS Accelerometer Gyroscope Magnetometer Arduino. Date accessed: 2021-05-09 https://howtomechatronics.com/how-it-works/ electrical-engineering/mems-accelerometer-gyrocope-magnetometer-arduino/ [5] H. Johansson Elektroteknik Institutionen f¨or Maskinkonstruktion, KTH, 2013
[6] What is a Stepper Motor : Types Its Working Date accessed: 2021-05-09
https://www.elprocus.com/stepper-motor-types-advantages-applications/ [7] Torker Glad, Lennart Ljung Reglerteknik: Grundl¨aggande teori
Studentlitteratur AB, 4(17), 2006
[8] Kalman, R. E. (1960). A New Approach to Linear Filtering and Prediction
Problems
Journal of Basic Engineering. 82: 35–45, 1960. Date accessed: 2021-05-09
https://pdfs.semanticscholar.org/bb55/c1c619c30f939fc792b049172926a4a0c0f7. pdf
[9] Lectures in The Kalman Filter. Date accessed: 2021-05-09
http://www.ilectureonline.com/lectures/subject/SPECIALTOPICS/26/ 190
BIBLIOGRAPHY
[10] Gheorghe Deliu, Mariana Deliu, Monowheel Dynamics.Date accessed: 2021-05-09
http://www.recentonline.ro/027/DELIU_Gheorghe_02.pdf
[11] Dan Botezatu, The Plane Motion of the Monowheel Vehicle. Date accessed: 2021-05-09
http://www.recentonline.ro/042/Botezatu-R42.pdf
[12] Sreevaram Rufus Nireekshan Kumar, Bangaru Akash, T. Thaj Mary Delsy,
Designing the Mono Wheel by Using Self Balancing Techinque,
International Journal of Circuit Theory and Applications, 9(4):29–34, 2016 [13] Build Electronic Circuits. H-bridge. Date accessed: 2021-05-09
https://www.build-electronic-circuits.com/wp-content/uploads/ 2018/11/H-bridge-switches.png
[14] Kjell & Company. Playknowlogy Uno Rev. 3 , Date accessed: 2021-05-09 https://www.kjell.com/globalassets/productimages/559575_88860. tif?ref=61DF06C805&format=jpg&w=960&h=960&mode=pad
[15] F1 Depo. MPU6050, Date accessed: 2021-05-09
https://st2.myideasoft.com/shop/dt/63/myassets/products/293/ mpu6050-sensor.jpg?revision=1540787072
[16] Makerguides. How to control a stepper motor with DRV8825 driver and
Arduino. Date accessed: 2021-05-09
https://www.makerguides.com/drv8825-stepper-motor-driver-arduino-tutorial/ [17] PID Tuner Controller, Date accessed: 2021-05-09
https://pidtuner.com/#/
[18] PIDv2.h library, Date accessed: 2021-05-22
https://playground.arduino.cc/Code/PIDLibrary/
Appendix A
Arduino
1 /* 2 * A u t h o r s : N i l s W i n n e r h o l t & F r i t i o f E k v a l l 3 * D a t e : 2 0 2 1 - 05 - 08 4 * -5 * M a d e for B a c h e l o r s P r o j e c t in M e c h a t r o n i c s 6 * M F 1 3 3 X 7 * -8 * T h a n k you to : D e j a n f r o m h t t p :// h o w t o m e c h a t r o n i c s . com 9 * B e n n e de B a k k e r f r o m h t t p :// m a k e r g u i d e s . com 10 * For p r o v i d i n g e x a m p l e c o d e s t h a t w i l l be u s e d in t h i s p r o j e c t 11 * B r e t t B e a u r e g a r d f r o m h t t p :// b r e t t b e a u r e g a r d . com / p r o j e c t s / 12 * For p r o v i d i n g the P I D _ v 2 l i b r a r y 13 */ 14 15 # i n c l u d e < W i r e . h > 16 # i n c l u d e < P I D _ v 2 . h > 17 18 # d e f i n e d i r P i n 2 19 # d e f i n e s t e p P i n 3 20 # d e f i n e s t e p s P e r R e v o l u t i o n 200 21 22 # d e f i n e en 12 23 # d e f i n e in1 8 24 # d e f i n e in2 7 25 26 // A c c e l e r o m e t e r and G y r o s c o p e 2728 c o n s t int MPU = 0 x68 ; // M P U 6 0 5 0 I2C a d d r e s s
29 f l o a t AccX , AccY , A c c Z ; // A c c e l e r a t i o n v a l u e s
30 f l o a t GyroX , GyroY , G y r o Z ; // G y r o s c o p e v a l u e s
31 f l o a t a c c A n g l e X , a c c A n g l e Y , g y r o A n g l e X , g y r o A n g l e Y , g y r o A n g l e Z ; // A n g l e s for the d i f f e r e n t o u t p u t s f r o m the a c c e l e r o m e t e r
32 f l o a t roll , pitch , yaw ; // C a l c u l a t e d roll , p i t c h and yaw f r o m the a c c e l e r o m e t e r r e a d i n g s
33 f l o a t A c c E r r o r X , A c c E r r o r Y , G y r o E r r o r X , G y r o E r r o r Y , G y r o E r r o r Z ; // E r r o r s in a c c e l e r o m e t e r r e a d i n g s
APPENDIX A. ARDUINO 34 f l o a t e l a p s e d T i m e , c u r r e n t T i m e , p r e v i o u s T i m e ; // K e e p s t r a c k of t i m e 35 int c = 0; // T e m p o r a r y v a r i a b l e 36 37 // K a l m a n 38 39 f l o a t Mea , KG ; 40 f l o a t Est_t0 , E s t _ t ; 41 f l o a t E_mea , E_est , E _ e s t 0 ; 42 43 44 // PID 45 c h a r r e c e i v e d C h a r ; 46 b o o l e a n n e w D a t a = f a l s e; 47 48 u n s i g n e d l o n g c h a n g e T i m e = 0; // L a s t t i m e the s e n s o r was t r i g g e r e d 49 d o u b l e d i f f e r e n c e = 5; // T e m p o r a r y v a l u e for the d i f f e r e n c e b e t w e e n the d e s i r e d a n g l e and t r u e a n g l e 50 d o u b l e s t e p p e r O u t = 2 5 0 ; // T e m p o r a r y v a l u e for the s t r e n g t h of the s t e p p e r m o t o r 51 d o u b l e s e t A n g l e = -4; // D e s i r e d a n g l e t h a t w i l l r e s u l t in b a l a n c e 52 S t r i n g i n S t r i n g ; 53 54 d o u b l e Kp = 0 . 7 7 6 4 ; // P r o p r t i o n a l c o n s t a n t 55 d o u b l e Ki = - 0 . 0 0 4 1 3 1 ; // I n t e g r a t i n g c o n s t a n t 56 d o u b l e Kd = -72.97; // D e r i v a t i n g c o n s t a n t 57
58 PID m y P I D (& d i f f e r e n c e , & s t e p p e r O u t , & s e t A n g l e , Kp , Ki , Kd , D I R E C T ) ; // C r e a t e s a PID f r o m the p r o v i d e d i n f o r m a t i o n 59 60 61 v o i d s e t u p () { 62 63 // PID 64 m y P I D . S e t M o d e ( A U T O M A T I C ) ; 65 66 // A s s i g n s p i n s for the DC m o t o r 67 p i n M o d e ( en , O U T P U T ) ; 68 p i n M o d e ( in1 , O U T P U T ) ; 69 p i n M o d e ( in2 , O U T P U T ) ; 70 71 d i g i t a l W r i t e ( en , H I G H ) ; // E n a b l e s the H - b r i d g e 72 73 // F r o m B e n n e 74 p i n M o d e ( stepPin , O U T P U T ) ; 75 p i n M o d e ( dirPin , O U T P U T ) ; 76 77 // F r o m D e j a n 78 S e r i a l . b e g i n ( 1 9 2 0 0 ) ; 79 W i r e . b e g i n () ; // I n i t i a l i z e c o m u n i c a t i o n 80 W i r e . b e g i n T r a n s m i s s i o n ( MPU ) ; // S t a r t c o m m u n i c a t i o n w i t h M P U 6 0 5 0 // MPU =0 x68 81 W i r e . w r i t e (0 x6B ) ; // T a l k to the r e g i s t e r 6 B 22
82 W i r e . w r i t e (0 x00 ) ; // M a k e r e s e t - p l a c e a 0 i n t o the 6 B r e g i s t e r 83 W i r e . e n d T r a n s m i s s i o n (t r u e) ; // End the t r a n s m i s s i o n 84 85 c a l c u l a t e _ I M U _ e r r o r () ; 86 d e l a y ( 2 0 ) ; 87 88 } 89 90 91 v o i d l o o p () { 92 93 // R u n s the DC m o t o r 94 d i g i t a l W r i t e ( in1 , H I G H ) ; // E n a b l e s the f i r s t t r a n s i s t o r p a i r
95 d i g i t a l W r i t e ( in2 , LOW ) ; // D i s a b l e s the s e c o n d t r a n s i s t o r p a i r
96 97 M P U 6 0 5 0 () ; // C a l l s for the a c c e l e r o m e t e r r e a d i n g s 98 99 // K a l m a n f i l t e r to i m p r o v e the q u a l i t y of d a t a f r o m the a c c e l e r o m e t e r 100 Mea = r o l l ; // I n i t a l m e a s u r m e n t 101 E s t _ t 0 = 1; // I n i t a l e s t i m a t e 102 E _ m e a =1; // E r r o r in m e a s u r e m e n t 103 E _ e s t 0 = 0 . 5 ; // I n i t a l e r r o r in e s t i m a t e 104 KG = E _ e s t 0 /( E _ e s t 0 + E _ m e a ) ; // K a l m a n g a i n 105 E s t _ t = E s t _ t 0 + KG *( Mea - E s t _ t 0 ) ; // C u r r e n t e s t i m a t e 106 E _ e s t = (1 - KG ) * E _ e s t 0 ; // New e r r o r in e s t i m a t e 107 108 KG = E _ e s t /( E _ e s t + E _ m e a ) ; 109 E s t _ t = E s t _ t + KG *( Mea - E s t _ t ) ; 110 E _ e s t = (1 - KG ) * E _ e s t ; 111 112 KG = E _ e s t /( E _ e s t + E _ m e a ) ; 113 E s t _ t = E s t _ t + KG *( Mea - E s t _ t ) ; 114 r o l l = E s t _ t ; 115 116 d i f f e r e n c e = abs ( s e t A n g l e - r o l l ) ; // F i n d s the d i f f e r e n c e b e t w e e n the desired - and a c u a l a n g l e
117
118 m y P I D . C o m p u t e () ; // C o m p u t e PID for t h i s i t t e r a t i o n
119
120 // D e t e r m i n e s the d i r e c t i o n for the s t e p p e r m o t o r ( b a l a n c i n g )
121 if( r o l l > s e t A n g l e +1) { 122 123 // Set the s p i n n i n g d i r e c t i o n c l o c k w i s e : 124 d i g i t a l W r i t e ( dirPin , LOW ) ; 125 126 d i g i t a l W r i t e ( stepPin , H I G H ) ; 127 d e l a y M i c r o s e c o n d s ( s t e p p e r O u t ) ; 128 d i g i t a l W r i t e ( stepPin , LOW ) ; 129 d e l a y M i c r o s e c o n d s ( s t e p p e r O u t ) ; 130 } 131 e l s e if ( r o l l < s e t A n g l e -1) { 23
APPENDIX A. ARDUINO 132 133 // Set the s p i n n i n g d i r e c t i o n c o u n t e r c l o c k w i s e : 134 d i g i t a l W r i t e ( dirPin , H I G H ) ; 135 136 d i g i t a l W r i t e ( stepPin , H I G H ) ; 137 d e l a y M i c r o s e c o n d s ( s t e p p e r O u t ) ; 138 d i g i t a l W r i t e ( stepPin , LOW ) ; 139 d e l a y M i c r o s e c o n d s ( s t e p p e r O u t ) ; 140 } 141 e l s e { 142 143 // T u r n s off the s p i n n i n g 144 d i g i t a l W r i t e ( stepPin , LOW ) ; 145 d e l a y M i c r o s e c o n d s ( 4 0 0 0 ) ; 146 d i g i t a l W r i t e ( stepPin , LOW ) ; 147 d e l a y M i c r o s e c o n d s ( 4 0 0 0 ) ; 148 149 } 150 151 152 } 153 154 155 156 157 // C o d e t h a t r e a d s d a t a f r o m the a c c e l e r o m e t e r 158 // W r i t t e n by D e j a n 159 v o i d M P U 6 0 5 0 () { 160 161 // === R e a d a c c e l e r o m t e r d a t a === // 162 W i r e . b e g i n T r a n s m i s s i o n ( MPU ) ; 163 W i r e . w r i t e (0 x3B ) ; // S t a r t w i t h r e g i s t e r 0 x3B ( A C C E L _ X O U T _ H ) 164 W i r e . e n d T r a n s m i s s i o n (f a l s e) ; 165 W i r e . r e q u e s t F r o m ( MPU , 6 , t r u e) ; // R e a d 6 r e g i s t e r s total , e a c h a x i s v a l u e is s t o r e d in 2 r e g i s t e r s
166 // For a r a n g e of + -2 g , we n e e d to d i v i d e the raw v a l u e s by 16384 , a c c o r d i n g to the d a t a s h e e t
167 A c c X = ( W i r e . r e a d () < < 8 | W i r e . r e a d () ) / 1 6 3 8 4 . 0 ; // X - a x i s v a l u e
168 A c c Y = ( W i r e . r e a d () < < 8 | W i r e . r e a d () ) / 1 6 3 8 4 . 0 ; // Y - a x i s v a l u e
169 A c c Z = ( W i r e . r e a d () < < 8 | W i r e . r e a d () ) / 1 6 3 8 4 . 0 ; // Z - a x i s v a l u e
170 // C a l c u l a t i n g R o l l and P i t c h f r o m the a c c e l e r o m e t e r d a t a
171 a c c A n g l e X = ( a t a n ( A c c Y / s q r t ( pow ( AccX , 2) + pow ( AccZ , 2) ) ) * 180 / PI ) - A c c E r r o r X ; // A c c E r r o r X ˜ ( 0 . 5 8 ) See the c a l c u l a t e _ I M U _ e r r o r () c u s t o m f u n c t i o n for m o r e d e t a i l s
172 a c c A n g l e Y = ( a t a n ( -1 * A c c X / s q r t ( pow ( AccY , 2) + pow ( AccZ , 2) ) ) * 180 / PI ) + A c c E r r o r Y ; // A c c E r r o r Y ˜ ( - 1 . 5 8 ) 173 174 // === R e a d g y r o s c o p e d a t a === // 175 p r e v i o u s T i m e = c u r r e n t T i m e ; // P r e v i o u s t i m e is s t o r e d b e f o r e the a c t u a l t i m e r e a d 176 c u r r e n t T i m e = m i l l i s () ; // C u r r e n t t i m e a c t u a l t i m e r e a d 177 e l a p s e d T i m e = ( c u r r e n t T i m e - p r e v i o u s T i m e ) / 1 0 0 0 ; // D i v i d e by 1 0 0 0 to get s e c o n d s 178 W i r e . b e g i n T r a n s m i s s i o n ( MPU ) ; 24
179 W i r e . w r i t e (0 x43 ) ; // G y r o d a t a f i r s t r e g i s t e r a d d r e s s 0 x43 180 W i r e . e n d T r a n s m i s s i o n (f a l s e) ; 181 W i r e . r e q u e s t F r o m ( MPU , 6 , t r u e) ; // R e a d 4 r e g i s t e r s total , e a c h a x i s v a l u e is s t o r e d in 2 r e g i s t e r s 182 G y r o X = ( W i r e . r e a d () < < 8 | W i r e . r e a d () ) / 1 3 1 . 0 ; // For a 250 deg / s r a n g e we h a v e to d i v i d e f i r s t the raw v a l u e by 131.0 , a c c o r d i n g to the d a t a s h e e t 183 G y r o Y = ( W i r e . r e a d () < < 8 | W i r e . r e a d () ) / 1 3 1 . 0 ; 184 G y r o Z = ( W i r e . r e a d () < < 8 | W i r e . r e a d () ) / 1 3 1 . 0 ; 185 // C o r r e c t the o u t p u t s w i t h the c a l c u l a t e d e r r o r v a l u e s 186 G y r o X = G y r o X + G y r o E r r o r X ; // G y r o E r r o r X ˜ ( - 0 . 5 6 ) 187 G y r o Y = G y r o Y - G y r o E r r o r Y ; // G y r o E r r o r Y ˜ ( 2 ) 188 G y r o Z = G y r o Z + G y r o E r r o r Z ; // G y r o E r r o r Z ˜ ( -0.8) 189
190 // C u r r e n t l y the raw v a l u e s are in d e g r e e s per seconds , deg / s , so we n e e d to m u l t i p l y by s e n d o n d s ( s ) to get the a n g l e in d e g r e e s 191 g y r o A n g l e X = g y r o A n g l e X + G y r o X * e l a p s e d T i m e ; // deg / s * s = deg 192 g y r o A n g l e Y = g y r o A n g l e Y + G y r o Y * e l a p s e d T i m e ; 193 yaw = yaw + G y r o Z * e l a p s e d T i m e ; 194 195 // C o m p l e m e n t a r y f i l t e r - c o m b i n e a c c e l e r o m t e r and g y r o a n g l e v a l u e s 196 /* r o l l = 0 . 9 6 * g y r o A n g l e X + 0 . 0 4 * a c c A n g l e X ; 197 p i t c h = 0 . 9 6 * g y r o A n g l e Y + 0 . 0 4 * a c c A n g l e Y ; 198 */ 199 // I m p r o v e d f i l t e r by C h r i s to s t o p d r i f t 200 g y r o A n g l e X = 0 . 9 6 * g y r o A n g l e X + 0 . 0 4 * a c c A n g l e X ; 201 g y r o A n g l e Y = 0 . 9 6 * g y r o A n g l e Y + 0 . 0 4 * a c c A n g l e Y ; 202 203 r o l l = g y r o A n g l e X ; 204 p i t c h = g y r o A n g l e Y - 8 0 ; 205 206 // P r i n t the v a l u e s on the s e r i a l m o n i t o r 207 S e r i a l . p r i n t (" R o l l : ") ; 208 S e r i a l . p r i n t ( r o l l ) ; 209 S e r i a l . p r i n t (" , ") ; 210 /* 211 S e r i a l . p r i n t ( " / " ) ; 212 S e r i a l . p r i n t ( p i t c h ) ; 213 S e r i a l . p r i n t ( " / " ) ; 214 S e r i a l . p r i n t l n ( yaw ) ; 215 */ 216 } 217 218
219 // C o d e t h a t c a l c u l a t e s the e r r o r in the d a t a f r o m the a c c e l e r o m e t e r
220 // W r i t t e n by D e j a n
221 v o i d c a l c u l a t e _ I M U _ e r r o r () {
222
223 // We can c a l l t h i s f u n t i o n in the s e t u p s e c t i o n to c a l c u l a t e the a c c e l e r o m e t e r and g y r o d a t a e r r o r . F r o m h e r e we w i l l get the e r r o r
v a l u e s u s e d in the a b o v e e q u a t i o n s p r i n t e d on the S e r i a l M o n i t o r .
224 // N o t e t h a t we s h o u l d p l a c e the IMU f l a t in o r d e r to get the p r o p e r values , so t h a t we t h e n can the c o r r e c t v a l u e s
APPENDIX A. ARDUINO 225 // R e a d a c c e l e r o m e t e r v a l u e s 200 t i m e s 226 w h i l e ( c < 2 0 0 ) { 227 W i r e . b e g i n T r a n s m i s s i o n ( MPU ) ; 228 W i r e . w r i t e (0 x3B ) ; 229 W i r e . e n d T r a n s m i s s i o n (f a l s e) ; 230 W i r e . r e q u e s t F r o m ( MPU , 6 , t r u e) ; 231 A c c X = ( W i r e . r e a d () < < 8 | W i r e . r e a d () ) / 1 6 3 8 4 . 0 ; 232 A c c Y = ( W i r e . r e a d () < < 8 | W i r e . r e a d () ) / 1 6 3 8 4 . 0 ; 233 A c c Z = ( W i r e . r e a d () < < 8 | W i r e . r e a d () ) / 1 6 3 8 4 . 0 ; 234 // Sum all r e a d i n g s 235 A c c E r r o r X = A c c E r r o r X + (( a t a n (( A c c Y ) / s q r t ( pow (( A c c X ) , 2) + pow (( A c c Z ) , 2) ) ) * 180 / PI ) ) ; 236 A c c E r r o r Y = A c c E r r o r Y + (( a t a n ( -1 * ( A c c X ) / s q r t ( pow (( A c c Y ) , 2) + pow (( A c c Z ) , 2) ) ) * 180 / PI ) ) ; 237 c ++; 238 }
239 // D i v i d e the sum by 200 to get the e r r o r v a l u e
240 A c c E r r o r X = A c c E r r o r X / 2 0 0 ; 241 A c c E r r o r Y = A c c E r r o r Y / 2 0 0 ; 242 c = 0; 243 // R e a d g y r o v a l u e s 200 t i m e s 244 w h i l e ( c < 2 0 0 ) { 245 W i r e . b e g i n T r a n s m i s s i o n ( MPU ) ; 246 W i r e . w r i t e (0 x43 ) ; 247 W i r e . e n d T r a n s m i s s i o n (f a l s e) ; 248 W i r e . r e q u e s t F r o m ( MPU , 6 , t r u e) ; 249 G y r o X = W i r e . r e a d () < < 8 | W i r e . r e a d () ; 250 G y r o Y = W i r e . r e a d () < < 8 | W i r e . r e a d () ; 251 G y r o Z = W i r e . r e a d () < < 8 | W i r e . r e a d () ; 252 // Sum all r e a d i n g s 253 G y r o E r r o r X = G y r o E r r o r X + ( G y r o X / 1 3 1 . 0 ) ; 254 G y r o E r r o r Y = G y r o E r r o r Y + ( G y r o Y / 1 3 1 . 0 ) ; 255 G y r o E r r o r Z = G y r o E r r o r Z + ( G y r o Z / 1 3 1 . 0 ) ; 256 c ++; 257 }
258 // D i v i d e the sum by 200 to get the e r r o r v a l u e
259 G y r o E r r o r X = G y r o E r r o r X / 2 0 0 ; 260 G y r o E r r o r Y = G y r o E r r o r Y / 2 0 0 ; 261 G y r o E r r o r Z = G y r o E r r o r Z / 2 0 0 ; 262 // P r i n t the e r r o r v a l u e s on the S e r i a l M o n i t o r 263 S e r i a l . p r i n t (" A c c E r r o r X : ") ; 264 S e r i a l . p r i n t l n ( A c c E r r o r X ) ; 265 S e r i a l . p r i n t (" A c c E r r o r Y : ") ; 266 S e r i a l . p r i n t l n ( A c c E r r o r Y ) ; 267 S e r i a l . p r i n t (" G y r o E r r o r X : ") ; 268 S e r i a l . p r i n t l n ( G y r o E r r o r X ) ; 269 S e r i a l . p r i n t (" G y r o E r r o r Y : ") ; 270 S e r i a l . p r i n t l n ( G y r o E r r o r Y ) ; 271 S e r i a l . p r i n t (" G y r o E r r o r Z : ") ; 272 S e r i a l . p r i n t l n ( G y r o E r r o r Z ) ; 273 } 26
Appendix B
Acumen
//Bachelor’s Thesis at ITM, KTH
//Balancing a monowheel with a PID controller
//Date: 2021-05-09
//Written by: Fritiof Andersen Ekvall and Nils Winnerholt //Examiner: Nihad Subasic
//TRITA number: TRITA-ITM-EX 2021:49 //Course code: MF133X
//Description of the program:
//This is a simulation of the movement of the monowheel as it rolls forward.
model Main(simulator) = initially
m1 = create Monowheel((0,0,0),green,(0,0,0)), //Here is the model created x1=0, x1’=0, x1’’=0, //Inital values for the x position and it’s derivatives x2=0, x2’=0, x2’’=0,
z=0 always
if x1<20 //If the x position is small enough it will start moving then x1’’ = -(x1’-5)
else if x1’>0
then x1’’ = -7 else x1’’ = 0,
m1.pos = (x1,0,0),// Change in x-axis position m1.rot = (0,0.2*x1,0),// Change in y-axis rotation x2’’ = 0,
z = x1-x2
APPENDIX B. ACUMEN
model Monowheel(pos,col,rot) = //Here is the formula for the creation of the model. initially
_3D =(), _Plot=()//View of what has been created always
_3D =(Cylinder center = pos //Choice of shape and position for the object size = (1,2) //Size
color = col //Colour
rotation = rot //How it rotates
transparency = 0.5) //It’s transparency
TRITA ITM-EX 2021:49
