1 00:00:00,520 --> 00:00:01,300 ‫Welcome back. 2 00:00:02,050 --> 00:00:10,420 ‫So now let's start with our global controller, the difference from the previous course is that now 3 00:00:10,720 --> 00:00:16,760 ‫we have one for loop for the outer loop and then one for loop for the inner loop. 4 00:00:17,560 --> 00:00:21,280 ‫So one for loop is nested inside, another for loop. 5 00:00:22,240 --> 00:00:24,820 ‫So this is for my outer loop. 6 00:00:25,960 --> 00:00:29,350 ‫If you remember this variable here plot L.. 7 00:00:29,920 --> 00:00:35,820 ‫That was the number of how many elements I had in the T vector here. 8 00:00:36,490 --> 00:00:39,350 ‫Then I'm going to put it here, plotted on minus one. 9 00:00:40,090 --> 00:00:50,530 ‫And so if I now make this code a little bit more global, then you will see that, OK, this part here, 10 00:00:50,560 --> 00:00:52,960 ‫this is for the outer loop. 11 00:00:53,800 --> 00:01:02,880 ‫And inside this for loop, I have another for loop here and this one is then for the inner loop. 12 00:01:03,670 --> 00:01:07,410 ‫So all that here, that's for the inner loop. 13 00:01:08,260 --> 00:01:12,610 ‫This thing runs everything up on one second. 14 00:01:12,790 --> 00:01:18,070 ‫So four times per this outer loop that's here. 15 00:01:18,820 --> 00:01:24,590 ‫And so when I start with my outer loop, I'm going to go to my position controller function. 16 00:01:25,630 --> 00:01:30,510 ‫So now I'm here and I'm going to this position controller function. 17 00:01:31,450 --> 00:01:39,720 ‫So I need information from my reference vectors and their derivatives at a certain time. 18 00:01:40,360 --> 00:01:48,100 ‫So that's why I have here I underscore global and I'm also going to put it here inside my reference 19 00:01:48,100 --> 00:01:52,070 ‫value vectors in order to get the right one out of it. 20 00:01:52,720 --> 00:01:59,350 ‫And so if we now go back to our support file, then you see that after the trajectory generate a function, 21 00:01:59,350 --> 00:02:07,090 ‫you have your position controller function, and then again, you take in the necessary initial constants. 22 00:02:07,880 --> 00:02:10,870 ‫You also need your latest states. 23 00:02:11,830 --> 00:02:19,960 ‫Then if you remember this feedback lionization controller, then from the plant, you got these states, 24 00:02:19,960 --> 00:02:22,960 ‫X, Y, Z, and then you've W and then this. 25 00:02:23,110 --> 00:02:32,980 ‫You've W had to go through this rotation matrix in order to give you X, Y and Z dot that would then 26 00:02:33,310 --> 00:02:36,130 ‫be used in this feedback unionisation controller. 27 00:02:36,910 --> 00:02:38,770 ‫And so that happens here. 28 00:02:39,190 --> 00:02:49,900 ‫You see I have here my rotation matrix and then here I have my U v w variables and then I multiply this 29 00:02:50,110 --> 00:02:59,380 ‫rotation matrix by these UW variables and then I get my X, Y and Z dot. 30 00:03:00,160 --> 00:03:07,390 ‫And then if you look at the schematics here, then that's how you computed your error and error dot 31 00:03:07,570 --> 00:03:09,630 ‫in the X, Y and Z dimension. 32 00:03:10,600 --> 00:03:13,060 ‫And so that happens here. 33 00:03:13,780 --> 00:03:23,170 ‫And then here I compute my key values for the X, Y and Z dimension, my K one and K to values these 34 00:03:23,170 --> 00:03:28,330 ‫formulas here that you see this one and this one. 35 00:03:29,350 --> 00:03:31,150 ‫These are these formulas here. 36 00:03:31,150 --> 00:03:37,030 ‫You see you have your K one as a function of lambda one and then lambda two and then you have your K 37 00:03:37,030 --> 00:03:40,000 ‫two as a function of the one and lump two. 38 00:03:40,870 --> 00:03:48,310 ‫So you use these formulas to compute your K one and K two in order to construct a differential equation 39 00:03:48,310 --> 00:03:51,740 ‫that would drive your error and error to zero. 40 00:03:52,420 --> 00:03:58,040 ‫And so here you have your K values and here you have your control. 41 00:03:58,440 --> 00:03:58,970 ‫All right. 42 00:03:59,890 --> 00:04:03,960 ‫So that's this one here, you X, Y and Z. 43 00:04:04,330 --> 00:04:09,500 ‫And remember, they are actually the error double derivatives. 44 00:04:09,650 --> 00:04:10,120 ‫All right. 45 00:04:10,300 --> 00:04:13,870 ‫With respect to time, they are e double dots. 46 00:04:14,710 --> 00:04:17,410 ‫So essentially, you compute your E double dots here. 47 00:04:18,320 --> 00:04:25,680 ‫Then if you remember, you had to have your second derivative reference values with respect to time. 48 00:04:26,260 --> 00:04:34,750 ‫So you had your X double dots up R Y double dots up are and then Z double that Subha and then you had 49 00:04:34,750 --> 00:04:38,620 ‫to subtract E double dots from them. 50 00:04:39,040 --> 00:04:46,840 ‫You see here you have the plus sign and then here you have the minus sign and then as a result there 51 00:04:46,840 --> 00:04:52,840 ‫difference became X, Y double date and Z double that. 52 00:04:53,590 --> 00:04:55,810 ‫And that's what happens here. 53 00:04:56,500 --> 00:04:59,650 ‫So this V, X, V, Y and. 54 00:05:00,990 --> 00:05:08,040 ‫They are your true accelerations in the initial frame, X, Y and Z direction. 55 00:05:08,820 --> 00:05:17,250 ‫They are these ones here you see X, Y and Z, double dot and well, then you have to compute your Phi 56 00:05:17,610 --> 00:05:19,710 ‫Seeta and you won. 57 00:05:20,460 --> 00:05:27,330 ‫And so if you remember, then that was the procedure to compute our city angle here. 58 00:05:28,080 --> 00:05:32,190 ‫And then this was the procedure to compute the fi angle here. 59 00:05:32,850 --> 00:05:35,540 ‫And then we also computed our you won here. 60 00:05:36,270 --> 00:05:45,570 ‫And so all that happens here in this block, if you remember, then when we try to calculate our fi 61 00:05:45,570 --> 00:05:49,710 ‫angle, then we had to be careful with these two denominators. 62 00:05:50,430 --> 00:06:00,150 ‫So we define certain regions in our unit circle and then depending on our size up our value, we would 63 00:06:00,150 --> 00:06:07,470 ‫either use this equation or this equation in order to compute Tangent Phi. 64 00:06:08,400 --> 00:06:15,360 ‫That means that in order to compute PHI, we would either use this one or this one, either the purple 65 00:06:15,360 --> 00:06:25,620 ‫or the green equation, depending on the value of Sysop or whether it's in this green region or in this 66 00:06:25,620 --> 00:06:26,760 ‫purple region. 67 00:06:27,360 --> 00:06:30,420 ‫If it's in the purple region, you take the purple equation. 68 00:06:30,570 --> 00:06:34,460 ‫If it's in the green region, you take the green equation. 69 00:06:35,220 --> 00:06:41,570 ‫That's how we avoided a situation where we would divide by a singularity. 70 00:06:42,270 --> 00:06:48,260 ‫In other words, we try to avoid dividing by zero. 71 00:06:49,110 --> 00:06:53,880 ‫And so this code is what does exactly that. 72 00:06:54,690 --> 00:07:03,300 ‫The goal of this part of the code is this one that, for example, if my precise up R is, let's say, 73 00:07:04,050 --> 00:07:16,110 ‫six pi radians, but then for my fi angle calculation purposes, I want my precise up r b between zero 74 00:07:16,110 --> 00:07:24,780 ‫and two pi irradiance because then it's easier to manage these regions, these green and purple regions. 75 00:07:25,560 --> 00:07:34,980 ‫So if I have six pi radians then this code here, it will take this six pi and it will transform it 76 00:07:35,310 --> 00:07:37,650 ‫to two PI irradiance. 77 00:07:38,370 --> 00:07:48,260 ‫Or for example if my size up Rs nine pi over four radians then that would be bigger than two pi radiance. 78 00:07:49,050 --> 00:07:58,650 ‫But this code here, it would then take this nine pile of four radiance and transform it into pi over 79 00:07:58,830 --> 00:08:04,230 ‫four radiance because powerful radiance is this point here. 80 00:08:04,770 --> 00:08:10,590 ‫But nine pi over four radians is the same point is just one extra rotation. 81 00:08:11,430 --> 00:08:17,850 ‫And so I want to get rid of that extra rotation so that I would end up having pi over four radians. 82 00:08:18,830 --> 00:08:25,790 ‫And in this code, it's the other way around, so you have, for example, minus nine PI over four radians 83 00:08:26,690 --> 00:08:32,360 ‫and then it would make it minus Pi over four radiance. 84 00:08:33,110 --> 00:08:35,750 ‫So that's when I wrote it in another direction. 85 00:08:36,440 --> 00:08:43,370 ‫Didn't mince pie over four Radiance would be here in this point and minus nine pi over four radians 86 00:08:43,370 --> 00:08:45,620 ‫would also be this point here. 87 00:08:45,830 --> 00:08:48,080 ‫That's if I rotate clockwise. 88 00:08:49,140 --> 00:08:57,810 ‫And then once I have my angles between zero and two irradiance, then with these if statements here, 89 00:08:58,590 --> 00:09:05,040 ‫I can determine whether I'm in the purple region or in the green region, and then I choose my equation 90 00:09:05,040 --> 00:09:05,760 ‫accordingly. 91 00:09:06,450 --> 00:09:07,980 ‫And that's what I do here. 92 00:09:08,760 --> 00:09:17,020 ‫And then I can compute my fire ref and then I compute my you won control input. 93 00:09:17,820 --> 00:09:21,390 ‫So this equation here, it's this one here. 94 00:09:22,080 --> 00:09:22,920 ‫And there you go. 95 00:09:22,920 --> 00:09:26,190 ‫I have my firefighter and you won. 96 00:09:26,610 --> 00:09:29,920 ‫And then they go back to the main file. 97 00:09:30,810 --> 00:09:35,130 ‫So in the main file, they will be stored in these variables here. 98 00:09:36,150 --> 00:09:45,210 ‫And so once I have my firearm seats are safe and you won, then perhaps you'll remember that I had to 99 00:09:45,210 --> 00:09:51,940 ‫take my fire in theater and then I had to multiply it by this vector here. 100 00:09:51,940 --> 00:09:58,410 ‫And that has five elements so that my vector here would look like this. 101 00:09:58,680 --> 00:10:01,860 ‫This is the one that goes into the NPC controller. 102 00:10:02,610 --> 00:10:08,490 ‫So this is for Feis up R and this is for Thetis up R and then from the planner. 103 00:10:08,490 --> 00:10:18,060 ‫I also had to give Sysop are in this form as well and well actually they will later get transposed so 104 00:10:18,060 --> 00:10:26,040 ‫the role vectors will become column vectors and then they will go into the NPC controller in that form 105 00:10:26,040 --> 00:10:27,860 ‫in the form of convertors. 106 00:10:28,800 --> 00:10:30,420 ‫So that's why I wrote it here. 107 00:10:30,570 --> 00:10:37,200 ‫So the role vectors will be then transposed to form column vectors for the NPC to use. 108 00:10:38,280 --> 00:10:40,380 ‫So that's what happens here. 109 00:10:41,310 --> 00:10:50,580 ‫And so here I prepare the reference value vector for the NPC controller and then from here I will also 110 00:10:50,580 --> 00:10:54,240 ‫take my horizon period from the Init function. 111 00:10:54,990 --> 00:11:02,820 ‫And then this key variable here, well, I'm going to use it later in here where I make sure that my 112 00:11:02,820 --> 00:11:08,480 ‫reference value vector takes into account the length of the horizon period. 113 00:11:09,150 --> 00:11:11,840 ‫And so that's the stuff for the outer loop. 114 00:11:11,850 --> 00:11:16,710 ‫And now we're going to enter the inner loop, which starts from here. 115 00:11:17,520 --> 00:11:20,130 ‫Starting from here, the inner loop starts.