‫Welcome back.
‫So now let's start with our global controller, the difference from the previous course is that now
‫we have one for loop for the outer loop and then one for loop for the inner loop.
‫So one for loop is nested inside, another for loop.
‫So this is for my outer loop.
‫If you remember this variable here plot L..
‫That was the number of how many elements I had in the T vector here.
‫Then I'm going to put it here, plotted on minus one.
‫And so if I now make this code a little bit more global, then you will see that, OK, this part here,
‫this is for the outer loop.
‫And inside this for loop, I have another for loop here and this one is then for the inner loop.
‫So all that here, that's for the inner loop.
‫This thing runs everything up on one second.
‫So four times per this outer loop that's here.
‫And so when I start with my outer loop, I'm going to go to my position controller function.
‫So now I'm here and I'm going to this position controller function.
‫So I need information from my reference vectors and their derivatives at a certain time.
‫So that's why I have here I underscore global and I'm also going to put it here inside my reference
‫value vectors in order to get the right one out of it.
‫And so if we now go back to our support file, then you see that after the trajectory generate a function,
‫you have your position controller function, and then again, you take in the necessary initial constants.
‫You also need your latest states.
‫Then if you remember this feedback lionization controller, then from the plant, you got these states,
‫X, Y, Z, and then you've W and then this.
‫You've W had to go through this rotation matrix in order to give you X, Y and Z dot that would then
‫be used in this feedback unionisation controller.
‫And so that happens here.
‫You see I have here my rotation matrix and then here I have my U v w variables and then I multiply this
‫rotation matrix by these UW variables and then I get my X, Y and Z dot.
‫And then if you look at the schematics here, then that's how you computed your error and error dot
‫in the X, Y and Z dimension.
‫And so that happens here.
‫And then here I compute my key values for the X, Y and Z dimension, my K one and K to values these
‫formulas here that you see this one and this one.
‫These are these formulas here.
‫You see you have your K one as a function of lambda one and then lambda two and then you have your K
‫two as a function of the one and lump two.
‫So you use these formulas to compute your K one and K two in order to construct a differential equation
‫that would drive your error and error to zero.
‫And so here you have your K values and here you have your control.
‫All right.
‫So that's this one here, you X, Y and Z.
‫And remember, they are actually the error double derivatives.
‫All right.
‫With respect to time, they are e double dots.
‫So essentially, you compute your E double dots here.
‫Then if you remember, you had to have your second derivative reference values with respect to time.
‫So you had your X double dots up R Y double dots up are and then Z double that Subha and then you had
‫to subtract E double dots from them.
‫You see here you have the plus sign and then here you have the minus sign and then as a result there
‫difference became X, Y double date and Z double that.
‫And that's what happens here.
‫So this V, X, V, Y and.
‫They are your true accelerations in the initial frame, X, Y and Z direction.
‫They are these ones here you see X, Y and Z, double dot and well, then you have to compute your Phi
‫Seeta and you won.
‫And so if you remember, then that was the procedure to compute our city angle here.
‫And then this was the procedure to compute the fi angle here.
‫And then we also computed our you won here.
‫And so all that happens here in this block, if you remember, then when we try to calculate our fi
‫angle, then we had to be careful with these two denominators.
‫So we define certain regions in our unit circle and then depending on our size up our value, we would
‫either use this equation or this equation in order to compute Tangent Phi.
‫That means that in order to compute PHI, we would either use this one or this one, either the purple
‫or the green equation, depending on the value of Sysop or whether it's in this green region or in this
‫purple region.
‫If it's in the purple region, you take the purple equation.
‫If it's in the green region, you take the green equation.
‫That's how we avoided a situation where we would divide by a singularity.
‫In other words, we try to avoid dividing by zero.
‫And so this code is what does exactly that.
‫The goal of this part of the code is this one that, for example, if my precise up R is, let's say,
‫six pi radians, but then for my fi angle calculation purposes, I want my precise up r b between zero
‫and two pi irradiance because then it's easier to manage these regions, these green and purple regions.
‫So if I have six pi radians then this code here, it will take this six pi and it will transform it
‫to two PI irradiance.
‫Or for example if my size up Rs nine pi over four radians then that would be bigger than two pi radiance.
‫But this code here, it would then take this nine pile of four radiance and transform it into pi over
‫four radiance because powerful radiance is this point here.
‫But nine pi over four radians is the same point is just one extra rotation.
‫And so I want to get rid of that extra rotation so that I would end up having pi over four radians.
‫And in this code, it's the other way around, so you have, for example, minus nine PI over four radians
‫and then it would make it minus Pi over four radiance.
‫So that's when I wrote it in another direction.
‫Didn't mince pie over four Radiance would be here in this point and minus nine pi over four radians
‫would also be this point here.
‫That's if I rotate clockwise.
‫And then once I have my angles between zero and two irradiance, then with these if statements here,
‫I can determine whether I'm in the purple region or in the green region, and then I choose my equation
‫accordingly.
‫And that's what I do here.
‫And then I can compute my fire ref and then I compute my you won control input.
‫So this equation here, it's this one here.
‫And there you go.
‫I have my firefighter and you won.
‫And then they go back to the main file.
‫So in the main file, they will be stored in these variables here.
‫And so once I have my firearm seats are safe and you won, then perhaps you'll remember that I had to
‫take my fire in theater and then I had to multiply it by this vector here.
‫And that has five elements so that my vector here would look like this.
‫This is the one that goes into the NPC controller.
‫So this is for Feis up R and this is for Thetis up R and then from the planner.
‫I also had to give Sysop are in this form as well and well actually they will later get transposed so
‫the role vectors will become column vectors and then they will go into the NPC controller in that form
‫in the form of convertors.
‫So that's why I wrote it here.
‫So the role vectors will be then transposed to form column vectors for the NPC to use.
‫So that's what happens here.
‫And so here I prepare the reference value vector for the NPC controller and then from here I will also
‫take my horizon period from the Init function.
‫And then this key variable here, well, I'm going to use it later in here where I make sure that my
‫reference value vector takes into account the length of the horizon period.
‫And so that's the stuff for the outer loop.
‫And now we're going to enter the inner loop, which starts from here.
‫Starting from here, the inner loop starts.