Arduino IMU PID Control System Design and Tuning

[00:00:08][Applause] hello guys this is palma porter with top tech boy comm and we are here today with lesson number 26 in our incredible tutorial series where you are learning about inertial measurement systems what i'm gonna need you to do today is i'm going to need you to pour yourself a nice big mug of iced coffee and i am going to need you to also pour yourself some hot coffee because this is gonna be a long lesson it's a long lesson but it's an important lesson because what we're going to learn today is we're gonna learn about PID controllers we're gonna learn the theory behind PID control systems and we're going to demonstrate that PID control system on the platform of our self-leveling arduino bno 0:55 system and so we have a bno 0:55 nine axis inertial measurement system we sensor we have an arduino nano we have a sour supply and then we have two servos and the idea is you measure the position with the sensor and then you apply a correction signal to these servos so that this platform will self level now we have done quite a bit of work on this in 26 lessons and we are sort of to the point now that we have something with a very simple control system that will actually work we basically have a self leveling platform and so i can kind of show this it takes a second to find its eyes find its level let me reset it okay so you can see that if I tilt it towards mirror towards you it's self-corrects pitch itself corrects and so using the sensor in a very simple control system no matter where I move my hand the system will come back in level itself well that looks really really cool but the problem is it's a very very simple control system the nice thing about the simple control system is it's easy to implement the problem is when we actually go in and look at it it doesn't really perform very well so what we're going to learn today is we're gonna learn about PID controllers how they work and then we're gonna program it into this system and show you how much better the system works when you have a true PID control system so I think at this point we are going to jump over here and we are going to start talking about the theory of PID controllers and the first thing that you have to understand when you're talking about control theory is you have to understand the concept of error okay whatever it is that you're trying to control whether it's temperature speed rpm angle whatever you're trying to control you got to think there's a place that I want to be there's a place that I am and between those two there is an error and so you really do all the theory around that error but the theory works for whatever control system that you're trying to do for us what is the parameter of interest the parameter of interest is angle and the target is zero degrees if I tilt it like this I have put it at 45 degrees the difference between where I am and where I want to be that's 45 degrees that's my error and the idea of a control system is to look at that error and then be smart in the way that we eliminate the error and so let's look here and let's just kind of review like what would be the simplest way that we could control the error of this system and I believe this is what we actually did in Lesson number 24 so let me draw it out and let's think about this this is the target and I'm gonna introduce a forty five-degree era let's just say I'm going to introduce a ten degree error so it sort of matches my it matches my drawing here so let's make sure I have the right pen so I'm coming here I'm flat so I have an error of zero and then very quickly I tilt it to ten degrees and so now I have an error of ten degrees now the simplest control system possible is called constant constant correction okay and that means the correction signal is the same whether you have a big error or a small error and so let's say that our constant correction factor is one degree so if I'm off by 50 degrees the correction will be 1 degree per cycle or if I'm off 3 degrees the correction is 1 per cycle so the way that would work is I introduce a 10 degree error and then I the first time through the loop I take out one degree the next time through I take out another degree I look I'm still too big I take out a degree still too big take out a degree each time through I say is my error positive yes take out a degree yes take out a degree take out a degree take out a degree and so forth until I come and I had this error I drove the error to zero one degree at a time now why would you want to do a control system like this because it is very simple right it is as simple as possible if I'm too large I make it smaller smaller smaller smaller smaller smaller if I'm too small I make it larger large or large or larger and that correction factor is a fixed amount for this case it is one degree so one good thing about a constant correction control system is it's very simple another one is it is very stable ok if you look at this I gradually correct until I get to my target and then I stopped correcting it tends to be very stable well why don't we just do everything with a constant correction algorithm the problem is it is slow okay because even if I'm 50 degrees off I just correct one degree at a time and that turns out to be very slow and so then that led to what we did in Lesson number 25 and that was let's be a little bit smarter about what we do and let's make the correction factor the correction signal to not just be a constant but let's have it be proportional to the error so let's just say that what we would like to do is we would like the correction signal to decry to correct out have the error each cycle so let's see how that would work well I come up here I have 10 degree error well I'm going to try on the first cycle to take out 5 degrees and then on the next cycle I'm going to take out half of that 2 and 1/2 degrees 1 and then very quickly we eliminate the error ok we eliminate the error because we're doing it odd that is terrible let's see if I can get rid of that ok let's see if I can draw that better I did a little loop-d-loop there ok let's see if I can draw this smooth or ok so you see it's about twice as fast using a proportional control system is you get closer to the target you have smaller and smaller Corrections so that works pretty good and so in this case what we are doing is we are doing a proportional control proportional and we go ahead and show you P I D ok PID the thing for you to see is what we did last week and what I just drew up here that is the P in P I d it's proportional and that means that your correctional signal is some constant k1 times the error and that k1 like I think we used about 0.3 or 0.5 last time so each time we were trying to take out about half the air well that worked pretty good so you might ask yourself why don't we just make everything proportional control well the problem is if you look at it you would really like to go a little bit faster and if you wanted to go a little bit faster what if we said instead of taking out half the error each time what if we took out 80% of the error each time ok we're going to try to take out 80% of the error each time so we come up here we have an error of 10 degrees so we're going to take out 8 degrees of error but look at that if you think that you sling this thing that far in one step you're building up a lot of momentum and you're almost to zero and what's going to happen you're going to overshoot and now you've got a negative error and then you overshoot here and you overshoot and this thing begins to ring and so if you're using proportional control and you're trying to make it go fast what happens is you overshoot overshoot overshoot and even though you're moving faster and you're taking out more error you end up you end up in introducing this ringing and it's actually slower than if you would just use a proportionality constant that was less and if you try to apply too much proportional control it'll actually become unstable you'll lose control and the thing will just fly all over the place and so the problem with a proportional control only is the problem with a proportional control only is that you end up you end up what would I say you you you end up with a fundamental trade-off between speed and stability and you can make it stable but it'll be slow you can make it fast but it will be unstable so what you want to do is you want to get a little bit more clever in how you do this thing so I'm trying to get rid of some of these let me see if I can just maybe I can erase these let's see give me just a second I don't want to erase everything okay erase these data points I want to leave that first curve that's why I'm doing it this way okay actually I think I want to take out the P curve too alright so now let's think about how we could be a little bit smarter about this all right so we are we are here at 0 degrees we introduce very quickly we introduce a 10 degree error well chances are you would make a measurement like let's say right here so you see that you're good as you start tilting all the sudden you have like a three and a half degree error and then boom you have a 10 degree error well let's think about how you would think about it as a person versus how the proportional control system thinks about it the proportional control system looks at it and says well I've got an error of about 4 degrees mmm I'll apply a correction of 1 degree you see its thinking 4 degrees it's not that much small error I'll apply a small correction okay and what is the problem with that well it's a person you see I was at zero degrees and all of a sudden I'm at 4 degrees I'm losing control I need to sort of panic and get it back under control like if you think if this was a car and you're going this way it's like you're gonna spin out of control so what really you see intuitively as a person one of the things that matters is not just it's not just how large the error was but it's how quickly that error occurred it's the rate of change of the air and if you think about math what is rate of change rate of change is slope okay and so you want part of your correction signal to not just be based on the magnitude of the error but how quickly that error is getting there so you kind of want a nice stable correction that's just based on the error but then you want part of your correction to kind of be a panic factor that panics not on the magnitude of the error but how quickly the error change and so we want a correction term that is going to be proportional to slope now from your math class what is slope slope is y2 minus y1 over x2 minus x1 well for our case what is y it's error so for us the slope would be error 2 minus error 1 over what is our x axis our x axis is time so our denominator would be time to minus time one okay that is slope in terms of algebra but if you think in terms of calculus calculus the slope is the derivative and kind of putting this more in calculus terms we would say e 2 minus e 1 is what that's Delta e the change in the error over what Delta T okay and that is more like the calculus derivative so in a PID controller you want one part of the correction signal to be related to P and we did that here and then you want another part that is control it that is proportional to D the derivative or the slope so you have proportional control you have derivative control and you can think of that if you're not familiar with calculus you can think of that in terms of slope and so you want that term - BK - a second proportionality constant times what the change in the error over change in time so if we have a fastly changing air signal we respond quickly with this derivative term okay so now what that would do is when we get here this would not just have a signal proportional to the error it would have a signal proportional to the slope and that would kick in quickly to turn this thing around and bring it back to zero okay so we've start our correction quicker because we see a fast changing signal and when we add derivative control to proportional control it really works very well but let me show you something else about how we avoid that swinging past where we want well once we turn it around and come down and we start coming down now the slope is not positive now the slope is negative so at that point this derivative term is not speeding you up towards zero it's backing off it's not adding to the proportional signal its subtracting from it and what that allows you to do is it allows you to begin to slow down and come into a nice smooth landing so when you're getting away from where you want to be you're piling on correction and as you begin to come to your target you're backing off and so both on your way up and on your way down this derivative signal is adding to the stability it's making you begin to correct quicker and then backing off so you come in for a smooth landing men this is so important I feel like that I need to repeat it okay when I go from here to here okay I'm moving away from my target so I'm gonna respond with a signal that's proportional to the air and also I'm going to add that a derivative term the slope is positive so I'm adding more correction than I normally would when I turn around and start coming down now the slope is negative so I'm beginning to back off of that proportional signal so that I can come in for a smooth landing and so that is what you can do when you combine proportional control with derivative control well what about this I term that is I guess I would describe it this way that lets say that you ended up down here that you came down and you ended up with a little error like this okay why would you end up with a steady-state constant error like that well let's think about our control system when we end up with a constant error does the derivative term do anything no the slope is zero it's not changing so this correction term would be 0 and then for proportional yes there's an error but I'm only feeding back a small part of that error and so I can kind of just get stuck here at this small steady-state error and so what we want to do is we want to add in a third control signal and that third control signal is I or integral ok and integral again is a calculus term but if you think in terms of algebra it is a correction term proportional to area and it is the area between the error curve and the target value the zero value and so how to think of it this way the derivative term is a fast-acting panik term the integral term is a slow-acting term to take those long term errors out and the way you can see it is at this point right here what is the area well it would be this shaded region here that's real small so would you get much of an integral correction no but if you come to the next data point all of a sudden now what is the air the area well it's bigger because now I have two chunks of time where there was one and then after three it gets even bigger so area is getting bigger and bigger and bigger and so if you have something proportionate that area over time it will see this integrated area and it will begin to apply a signal proportional to that to bring the area air to zero and so this would be some K 3 and that K 3 is going to be multiplied by the area error okay now what is the area error well you see each time the area is getting larger so what is that area term well the area is going to be equal to the old area okay what the area was before your old area plus your new area what is your new area well its width times height what is the width remember its delta T what is the height the height is air so the overall area error is area plus width times height I'll say height times width error times delta T okay so this is a slow-moving signal that we're tracking the overall error area over time and we're slowly driving that to zero okay so this then this term would be k3 times the area plus the new little slice which would be the error times delta T okay now what is our overall correction factor that we're going to put on this thing our overall correction is going to be equal to the proportional correct which is some constant times the error okay and this is the term that we used in lesson 25 okay but now to be able to have it be faster more responsive more snappier and more stable as we're coming to our target we're going to add a second proportionality constant k2 but that is going to be related to the derivative of the curve or you could think of it as the slope of the curve and that is Delta e over delta T and then we're going to have some k3 times the integrated area which is where at whatever area you had before plus error that's the height times the width delta T okay so do you see what this is we're going to have a correction term each time through the loop that has three components one component of the correction is related and proportional to the error itself the second term is proportional to how quickly the error is changing it helps you correct faster on the front end and then come in smoother on the back end it helps you in the right direction both ways and then finally to get rid of that little remnant drift or slow-moving error that you end up with after applying P and D we have the I the integral correction and so now what we need to do is we need to go in and start building this into our code so that our correction on this self levelling platform has three different parameters how does that sound okay so I hope this makes sense you might need to go back and watch this twice okay you might need to go back and watch this twice but what I am going to do now is we and I will need you to go out and get yourself a new a fresh new e arduino ide I mean let's make sure you can see that okay and then what we're going to start is we're going to start where we left off we are going to start where we left off in Lesson number 25 and so I need you to go to the most excellent wwp tech boy calm I'm going I'm going to need to go to a different view here I think this will be a better view is that gonna work yeah so you're gonna go to ww2 pH boy calm and I need to get this to make sure that you can see it and let me I'm sorry this top camera isn't really pointing at anything but I'm gonna point it at the platform when we actually start I'm gonna point it at the platform when we actually start doing the correction so right now you're just looking at some chunk on my desk but you need to go to WWF Tag Heuer comm you need to go to non-access IMU lesson number 25 you can get there with a little search icon and then you can come down and you see the code you can click these two little pages that selects all the code you can do a ctrl C and then come to your arduino into a control v and now i need to make sure that you can see this all the way so that looks pretty good okay I think that's gonna work so this is where we left off in lesson number 25 and this is our proportional control system so let's see if we can go ahead and download that and just let's sort of look and see how this thing behaves with our proportional control so I got to give it a second to download that looks like it downloaded now I will come and I will turn it on let's see if I can give you a nice shot of this okay so what I'm going to do is I'm going to roll it towards you and you can see it correct I'm gonna pitch down it corrects I'm going to pitch up it corrects I'm gonna roll it towards you CeCe however I move this it corrects well that looks pretty good right that looks like it's working until we go to our actual plotting of the data and what I'm plotting here is the orange is the error in pitch and the purple is the error in roll and so let's see if we can turn this thing on okay and that seems to be working what I'm gonna do I'm gonna give about a 45 degree pitch or a 45 degree roll your direction okay and what you see is what is the problem let me see if I can stop this and kind of look at this what is the problem with this proportional control yes it brings the error to zero but look at this this is what I was talking about I rolled it towards you about 45 degrees but when you had like a 5-10 degree error it's thinking yeah I'll give it two degrees of Correction well I'm on my way to 45 degrees I got to have a bigger correction than that and by the time it starts correcting its all the way up to 45 degrees and now it's correcting great big and it's swinging it back quickly and it comes in too fast and it what it overshoots by 20 degrees and so you see you end up with this overshooting in this ringing in the data so we need a better control system so that we can operate quickly and stabili and hopefully this data kind of shows you what our problem is and what we are trying to do okay so let's think about this what we need is we need in the code like we already in the code we sort of already in the code have this k1 times the error factor and so we're gonna need to add these other two components all right well in order to do that we're gonna need a lot more variables right we're gonna need a variable for slope we're gonna need a variable for area to get the variable for slope we're gonna need to have a change in error and a change in time so there's a lot more variables that we're going to have to use and so let's just jump in and start okay we've got let's see here we got where we left off in Lesson number 25 here and what we're gonna need to do now is we're gonna need to go in and the first thing we're going to do is we're going to set up our new variables at the top of the code and I'm going to explain these variables a lot and that way when we get down to programming them it's gonna kind of make sense because I really I really explained them in the in the setup so we're gonna have to define new variables so let's see here we come in and we load our libraries in the code we create our servo elements we define our quaternion x' okay and then after the quaternion definition this is really where we want to come in and kind of start putting in our new variables and again we're trying to program those equations and that is creepy having that thing point at me so I'm going to turn this back to nothing okay so we've defined our quaternions now we need to begin to put in our new variables and if you think of our if you think of our equation here you can see that we have three proportionality constants k1 k2 and k3 and so we probably should go ahead and define those and then start putting in our other variables and so we're going to have our proportionality constant k1 that is going to be a float and k1 we know that just from our earlier work that a value of about point 5 would probably be right that's saying that each time through we want to take out half the error so our proportionality constant is going to be 0.5 now we're also going to have a second one k2 and this is going to be associated with the derivative and that needs to be a lot bigger okay and it has to do with the fact that we're measuring time in milliseconds and we're dividing by milliseconds and so this is going to be a lot bigger this is gonna be like 250 okay so we're going to have a large signal correction are a large proportionality constant for that derivative term and then finally we're going to need our third one float k3 and this is that that integral term so this is a proportionality constant for the integral term and we're going to make that point O one alright now why is k2 really big and k3 really small well because I'm operating in milliseconds in a short amount of time you have a lot of milliseconds so milliseconds is going to be a large number and in k2 I'm dividing by delta T so I'm dividing by a large number so the proportionality constant needs to be big to offset that and kind of oppositely the integral term is multiplied by delta T or it's the yeah it's multiplied by delta T which makes it a big number so I need a small proportionality constant and so 0.5 250 and point a 1 really they're all kind of putting eat more or less equal weight between the three terms if you think about the fact that delta T is measured in milliseconds okay I hope that makes sense I got those two proportionality constants I think what I'm gonna do is I'm gonna make this probably 0.5 I'm gonna make this probably like 70 a little bit smaller and then that point 0:01 all right so now I have my proportionality constants now if you think of that equation over here you will remember that now I've got this very important concept of delta T that is how much time has elapsed since the last measurement okay it's like the old time it's the new time minus the old time that's the change in time it's t2 minus t1 the old time of the new time minus the old time that's delta T so I need to get kind of this this timing business in there so I'm going to have some new variables I'm gonna have milliseconds old that's like where the clock was last time through the loop okay and then I'm gonna need Milly knew that's where the clock is this time so last time through the clock was at Milly old this loop through I meant Milly new and then what would I have well I would have DT for delta T that's the change in time now I'm not putting equations in now I'm just I'm just declaring my variables but I'm telling you what I'm gonna do when I get down in the loop DT is gonna be the new milliseconds - the old milliseconds and that's going to be DT now if you think slope is Delta e over delta T I've got the denominator now of the slope so that's kind of neat or at least I've got the variables set up for it and now what I'm going to need is I am going to need to define some variables associated with our control now before we were doing proportional control so we already have this concept of a target roll and then we have an actual roll and then the difference between the target and the actual is the error and then we've got some value that we send some signal that we sent to the servos and so those were the variables that we needed to do proportional control but now we're not doing proportional control we are doing proportional differential and integral control so we need more values we need more variables and so you've got this this idea of roll error and I'm gonna go ahead and I'm gonna put roll error I'm gonna go ahead and I'm gonna put roll error at zero just so when it turns on it doesn't start flying around I'm gonna kind of tell it you're okay where you are so it gives the the Lupus a chance to start working so I've got roll error is zero but now if you think our delta e our change in error is the error - the old error that means i need to roll errors I need roll error that's what the error is this time in roll error old that's what the role error was last time okay so if I'm gonna take a difference in error I've got to have two error values the one this time through the loop the one last time through the loop does that make sense okay well what did I want I want change in error okay you know the Delta e change in air and so that would be role error change okay when I program it what's role error change gonna be well it's gonna be role error the error this time - role error old the error last time okay and so then I'm gonna need float I'm gonna need role error slope okay and what is the role error slope gonna be well the role error slow is going to be the change in the error over the change in time I just did change in error I did change in time I divide those two and I'll be able to get this term okay so I have roll error change and then roll errors slope it's going to be roll error change divided by DT just making sure I've got all the variables I need to do the calculation later then also remember those are kind of the derivative terms we do I better not say Sol error I better say roll error okay now what else am I gonna need I also have this area term okay and that would be like the roll error area okay and that is going to be if we look over here that is going to be like I showed you down here over time this area between the error and the x-axis and so I need a variable to keep track of that so that is an important variable all right roll error area and I think I'm gonna set that to zero to just so I just so it you know kind of starts without trying to do a correction you got give the loop a chance to to actually start working and I'm gonna set the roll era slope also to zero and this means the first time through the loop it's going to give the sensor a chance to start working before it starts correcting and it kind of keeps it from running away from you when the loop is just getting getting cranked up alright so we're gonna need to do those same things with pitch and so we've got pitch target pitch actual pitch error we're gonna go ahead you know where to set that to zero and now we got to start putting in our new variables all right so I'm going to need float and I'm gonna need pitch error old that's what the pitch error was last time through the loop and then I'm gonna float pitch error change okay and then I'm going to need float pitch error slope I'm going to set that to zero I'm gonna go ahead and set that to zero and then I'm going to need float I wanna meet need pitch error area okay and I'm gonna go ahead and set that says throat equals zero and then my initial servo value for both of them are set to 90 so that's just saying set it at 90 degrees 90 you reset should roughly set it to the flat position okay does that make sense that looks pretty good that looks pretty good so now I've got the variables that I need in order to go in and start programming up that equation that we're trying for and I'm wondering if I can kind of try to show you that equation with my camera give you something a little bit more useful to look at this is kind of hard to balance but let's see if I can get it yeah I think that you can kind of see what we're going to start trying to build now is we're gonna start trying to build this correction factor it's going to be our k1 times our error plus our k2 times our slope plus RK 3 times our area all right and we are gonna start trying to program this thing up and so I'll leave that equation so you can kind of look at it as we were trying to do this okay so I need to come down and kind of see what different things we have we're modifying this code from before so we're defining our we're defining our sensor we're loading our sensor libraries we're turning on the serial monitor we're turning on our sensor we're telling we're setting up the servos and then we are putting our servos to naughty and naughty so it kind of the servos will start in kind of a nice flat position so it'll become of the position from which we then start doing our control so just trying to get it Wordle start up nice and easy okay now that is the void setup now there's kind of a little quirky thing we have to do in the void setup and what that is is remember that our change in time is going to be our new time - our old time but the first time through the loop we don't have an old time because it's the first time through so we need to do that first time measurement and the void setup right before we go in the void loop and so here we're gonna say milli new is equal to Millie's and that will give us a value to use the first time through the loop so the program doesn't crash okay now we're gonna get down into our loop and let's see what we do here in the loop we pull the data off of the I am you we get the quaternion vector from the IMU and then we put those quaternions in q0 q1 q2 q3 so that's the data from the sensor then from our quaternions we calculate roll actual and pitch actual okay that's what we want we want roll actual to match roll target we want pitch actual to match pitch target but to do the control we have to have a measured value and we have to measure that value every time through the loop so here at this point we have actually where our role is and actually where our pitch is okay and that returns the values in radians and we want to work in degrees so we convert those two to degrees all right and now this was our old control here we did it with these if statements so I'm just going to take this out starting with a second if statement up to this roll error and roll target roll error in pitch air I'm going to take all that out and now we're going to start building our control algorithm from scratch the good thing is we have all the things we need and we can start doing it ok the first thing that we need to figure out is we need to figure out like we're gonna figure out slope slope is Delta e over delta T de over DT well I need to figure out what that DT is well I need to put milli old lily old B is equal to milli new now that sounds strange but what you got to think about last time through the loop that milli new it's no longer new it's got to be put in Millie old last time through the loop I had a million new this time through that Millie new is no longer Millie new it's now the old value so got to take my Millie new and I've got to put it over in Millie old ok now what do I need I need a new Millie new okay and that's gonna be Millie's so I just go out and read the clock okay so my new value gets the clock and ok so now I've got a new value and I've got an old value well what do I want to do well I want to subtract the - okay I want to subtract the two so DT that change the Delta in time is gonna be milli new what I just measured - milli old okay what's the good news the good news is I now have this delta T that I need I now have that delta T okay so you see I'm trying to build this formula I've got the delta T down here I can use delta T here I use delta T there I've got it now that is really very good that is what I needed okay so let's bring this back up hopefully that makes sense so I've taken care of my DT all right now I've got to take care of the error two minus error one well what you've got to see is we are doing two totally separate control system one is controlling roll and one is controlling pitch so I've got to do everything twice one for controlling roll and one for controlling pitch I'll do the roll control first well what is the first thing that I need okay well I need to roll values I need the OL roll value in the new roll value okay so what I'm going to say is I'm gonna say last time through the loop I had a roll error okay I need to save that now that becomes what roll error old because roll err it's no longer the fresh one it's the old one so roll error old is gonna be equal to what roll error all right and I think I might need to make sure that roll error has a value up here so it doesn't crash the first time through pitch err roll error okay I just want to make sure the first time through the loop there are actually numbers in there and it does appear that there are okay so i have roll error old is equal to roll error so that earlier i put it in old well now what I need is I to get the present role error and what is that going to be equal to well that's going to be equal to the rolled target where I want to be - the role actual all right and how did I get role actual while I measured it up here right I measured it so I take that measured value and I subtract the I subtract it from Roll target in that is roll error well now what do I have I have to roll errors I have the one last time through the loop and the one this time through the loop so what do I need I need to find roll error change how much did the roll error change between last time and this time between the last time through the loop and this time through the loop well that is pretty easy right it's the roll error this time - roll error old okay so that would be the change the present value minus the old value is the change value so now I have roll error change but it's not really the roll error change that I want what is it it's the slope it's the roll error slope roll error slope and that's equal to what let me just see if I can let you look at it here remind you right the slope is the change in the error divided by the change in time well I just calculated the change in error roll error change so roll your slope is what roll error change divided by what somebody tell me / what tt and I've got those values so now I guess I could show you over here so now what would be something good and suitable to point with now I not only have my error term which we're calling roll error I've like a1 that's good I got roll error I got k1 what I also have is I have k2 and I have the change in roll error divided by the change in time so I've got this whole term and I've got this whole term that is pretty slick so what do I need to work on now I do believe I need to work on that third term okay I need to work on that third term that odd term I've done P I've done D I need to do I and what is that going to be well I need the role error area and that is going to be Rho error area plus Rho error times DT so every time through I take that little bit of error error area and I add it to the area at it to the area so over time that small area is going to get bigger and bigger and bigger because you just keep adding to it and then that will force that small error to zero all right so role error times DT so I have role error area is role error area what it was before plus the error times DT okay I think one two three four five one two three four five that looks good now remember we're doing two control systems so I'm gonna need to do that same thing with you pitch I think the easiest way to do that I copied and pasted and I'm gonna put pitch here and now I'm going to copy pitch and then I'm just gonna come in and I'm gonna paste everywhere I have roll I'm gonna paste pitch and the bad thing about this is everywhere that I had an error I just copied and pasted the air but we will figure it out later when we run it [Music] you've got to make sure you change all of these because if you have a role parameter floating around in your pitch calculation it's not going to be pretty it's gonna be terrible okay so I think I've got pitch pitch pitch pitch pitch pitch pitch pitch pitch pitch pitch five changed all those so that looks pretty good all right now what are we ready to do we have got all of these different terms so we're ready now to calculate our new signal are corrected signal to go to the servos okay we're ready to do that and so what I'm gonna do here is I'm now gonna calculate the new role servo value and right this is the thing that we send to the servo the role servo so what is that going to be well it's going to be k1 times the error and so that would be well it's that but you're adding it to where you already are so roll servo vowel is gonna be rolled servo Val plus the correction okay where you were before plus the correction what's the correction it's k1 times the error for us k1 was k1 and then times okay our error here was roll error okay now that is our proportional term now we're going to add our derivative term which is going to be K 2 times what K 2 times roll error slope okay it's K 2 times the slope plus K 3 times the area which we called roll error area all right and now this same thing should be true for pitch and so I'm going to get this and I'm going to copy it and I'm going to paste it and then I'm going to come here and I'm going to change this to pitch so this is our control for our sec servo our pitch servo and I'm gonna copy I got to make sure I get all of these in here change all the roles to pitch [Music] okay that looks good now so I have my new signal for the roll servo what do I need to do I need to apply it now so I'm going to say roll servo crow servo dot right my servo is called roll servo I'm gonna write to it what am I going to write to it roll servo value right that is that corrected term that I just calculated okay similarly I need to apply the pitch value to the pitch servo so I'll copy that and I'll come here and now I'm gonna have my pitch servo I'm gonna write to my pitch servo the pitch value pitch set servo value Wow how many mistakes have I made in this I don't know I don't even wanna try to run this thing you guys are probably screaming and pulling your hair out trying to tell me all the mistakes that I made so let's make sure did we do you know we did our time I'm just gonna kind of glance over this all right then we print some stuff out let's just go ahead and see if this thing will compile so I'm gonna go ahead and try to run it try to download it Oh what did we do ahh Millie knew equal Millie's up here you guys probably saw it in put my semicolon in so let's fix that let's try it again okay probably a similar error yeah roll error old equal rolled error okay that needed one I think probably down here at kitchen I'm a the same mistake right that's what I was saying I was play copying and pasting mistakes you know had code a lot in Python in Python doesn't use semicolons as I go back and forth I'm always like making mistakes or I'm putting semicolons in Python Russia okay ah the DT here I didn't have a semicolon and probably down here I didn't have a semicolon and so now let's try this thing see what happens ah now I know what that is when I had my trusty old serial plotter going I forgot to turn it off so the USB was tied up so it couldn't download alright it compiled that's good okay it compiled that's good and it's probably gonna download me all right and so let's turn this thing on so we have P we have AI we have D so I'm going to turn this on sometimes it takes a little bit to get it to actually come to life okay let's see if this thing is a wife hey that is alive boom P D control look at that PID control alright I've got all three terms so you can see the thing is self-leveling is I'm moving my arm as I'm moving my hand the thing is leveling itself okay so that looks good but you know that looking good doesn't mean that it's doing good what we really need to do is we need to come back over here and we need to turn the serial monitor on and we need to see what's happening here okay so let's say I'm gonna roll towards me 45 degrees so I roll okay we have an instability do you see how it's really overshooting kind of badly okay and what that looks like to me is that looks like to me that we have too big of an i term okay we have too big of an integral term that k3 so I'm going to turn this off I'm going to come back to the code and when you have something just overshooting and I know I know that my Oh point five is kind of reasonable and so I think probably I need like another zero there because I think that integral term is kicking in and it's causing that instability and so we're gonna have to play with this a little bit to tweak this thing I might even have to make it smaller and really the truth is in this particular case you really don't need the I term it doesn't help you but I'm putting it in there so that you have a real PID controller now what you're going to see in this application it's the the work is being done by the P and the D and we just sort of have the AI and they're just so you you see how to do it because there will be applications where you need to use I okay so let's turn this thing back on and let's see if we can get this to come to life okay it is kind of coming to life I'm going to roll towards me 45 degrees oh wow do you see that okay it is still kind of doing that it's doing it less this is really working pretty good but if I put a big tilt it is still overshooting so I'll tell you what I'm gonna do so I'm seeing like as I move quickly it is overshooting and ringing and so what I'm gonna do is I'm gonna sit I'm going to tweak these parameters a little bit more and I still don't trust that I'm gonna put this as 0.001 and then I'm gonna think about this what I think is is that if it is overshooting as it's coming down that means that derivative term is not backing off quick enough that means the derivative term is not really doing its job and so I'm going to turn that derivative term up I'm going to turn that derivative term up to like I want to turn it up to 200 because I want it to back off and to pull that thing into a small thing and also the 200 means that it will respond quicker and then it will back off quicker and I think that should make this thing more stable so I've turned I down and I have turned the derivative term up and so let's make sure that we have this off okay and now let's turn let's download this me turn the power on and okay that's that was kind of crazy wouldn't it okay you can see that this is not happy at all yeah so you can see that that is way way too much of the derivative term I was I was hoping I could put it up there but that did not work at all so we're gonna have to go back to 100 okay and let's see if we can get it to work it 100 and really there's not a magic formula you just got to kind of play around with these things until you get something that looks like it's going to work and of course I left the serial plotter going which I need to turn off and now let me come here let it download and let's see if this thing will run at a 100 for the derivative term okay we'll turn it on you need to get it up here where you can see it okay give it a second to come to life okay that kana is looking pretty good it's a little jittery let's let's take a look at it on the serial plotter and see what what that is really doing okay so I'm gonna roll it 45 degrees towards me and you can see that that is still too much you see we get a lot of that ringing that I talked about what if we just turn D down I mean what if we turn proportional down what if we turn proportional down to 0.25 and let derivative do more of the work let's see what happens there what I hope you guys are doing in your system will be different because depending on how much momentum the size of your system your parameters could be way different than this every system is going to be completely different so now let's turn this on turn it back on and then we're gonna turn the power on and see what happens okay so [Music] okay this is kind of interesting I'm gonna roll towards you 45 degrees okay you see that glitch on the way down now that derivative term is so large that it's almost preventing it the derivative term is so large that it is actually slowing the system down because it's coming in to slowly remember the derivative term works against the proportional term on the way down and so now derivative is too large and proportional is too small but there is an aspect of this thing really tracking well until I do a fast change like that okay so I think that that derivative term is a little bit too large and the proportional term is a little bit too small so I'm gonna go to point I'm gonna go to 0.4 and then I'm gonna go to about 80 all right and let's use that ah no good turn the serial monitor off call up our code download our code okay a we will come back to the serial plotter I think this is gonna work as I think this is gonna be a pretty good value here all right you will open that and we're gonna turn this on okay it's come to life I'm gonna roll 45 degrees towards me oh look at that okay and I am moving this thing fast and do you see how it is responding quickly I'm gonna pitch I'm gonna pitch okay still on the way down it's almost like that derivative term is still too large and the reason I say that is you see you're coming down and then it just almost stops right here that derivative term is bringing in nice and smooth but it's actually slowing it up at this point so that derivative term I think I'm gonna make it 70 and then I think I'm gonna make this a little bit bigger of about 0.5 and let's try and see what happens there and guys I'm just I'm gonna this will probably be the one that I'm gonna use and let's see here it's gonna download okay come over here open this up and then we are going to turn this on okay okay so now I'm gonna roll okay so now I will roll towards me 45 degrees okay roll towards me 45 degrees pitch pitch okay guys I am really thinking these are pretty good parameters here you see as I'm moving it's really you know just sort of visibly now looking at the platform it is really keeping it really pretty nice okay I'm gonna roll towards me 45 degrees okay getting a little instability there okay as much as I hate to I'm gonna have to turn the derivative down okay that is still too much derivative I want to make it like 55 because once it starts going like that that is that fast responding derivative term and it is just too much there okay turn my monitor off download the code okay you will turn this back on and then we will turn the system on okay it's come to life okay all right so I'm going to roll towards me roll towards you then I'm gonna pitch I'm gonna pitch the other way okay look at that now practically this is really working very well now I'm gonna try a quick roll 45 degrees towards me look at that it came in very nice a quick 45 degree roll towards you it's coming in very nicely pitch okay and you see look at that that pitch is not overshooting now and you see it's operating very quickly like if I do a 45 degree pitch do you see how it didn't go off the screen it never got to an error of about 20 so you can see the derivative term coming in and really correcting it rolling okay so guys is I look at this for me in my system it seems like kind of the best parameters were point five on the proportionality constant and 55 on the derivative term and then this point oh oh oh oh one on the integral term that's just almost turning the integral term off because it's really not needed but what I really want to here is I want to hear how this worked for you I want you to tell me what your I guess this is your homework what were your k1 and k2 values that you use that got the best performance out of this thing and and it's like it's really kind of hard because maybe as you make k1 bigger you've got to make k2 bigger but then you reach the point that there's just not stability and then if you bring them down if you're not careful you're just not getting the performance out of the system that you want and so the goal is to have it as quick as possible and as stable as you can and what values did you find and put your values in before you read other people's comments for me it seems like 0.5 and 55 is sort of the sweet spot as far as getting this thing to go so man PID control it's basically solving calculus problems in real time on the Arduino you're finding the derivative of your error curve you're finding the integral of your error curve and you're creating a correction signal that has three parts part of it is proportional to the error part of it is proportional to the derivative of the error and part of it is proportional to the integral of the error and I hope that even through this video you could see that with those three components it works much much much better than either the constant correction or the proportional correction alone okay guys this has been a long video little bit tedious but I hope some of you made it through if you made it through you know give me a comment letting me know that you watch the whole lesson if you liked the lesson think about giving us a thumbs up subscribe to the channel when you subscribe make sure to ring the bell so that you get notifications on future videos appreciate you guys tuning in would really like to hear your feedback down below palma quarter from top tech boy comm I will talk to you guys later