1 00:00:00,740 --> 00:00:10,160 ‫So let's now go to our support file, and so here is your trajectory generator function that takes in 2 00:00:10,490 --> 00:00:13,460 ‫your time vector here. 3 00:00:13,880 --> 00:00:20,900 ‫I take the necessary initial constants from my init function from this one here. 4 00:00:21,980 --> 00:00:30,980 ‫So Python classes are built in such a way that if you put something in the Init function, then in other 5 00:00:30,980 --> 00:00:35,930 ‫functions you can take that stuff from the unit function. 6 00:00:36,860 --> 00:00:42,040 ‫Then here you have the high difference, final height minus initial height. 7 00:00:42,050 --> 00:00:48,950 ‫So the spiral starts at five meters and ends at twenty five metres in altitude. 8 00:00:49,340 --> 00:00:55,370 ‫Then twenty five minus five would give you 20 meters, which is the difference. 9 00:00:55,370 --> 00:00:55,790 ‫Height. 10 00:00:56,780 --> 00:01:04,400 ‫And while this alpha this is what goes into these cosine and sine functions, so you have two times 11 00:01:04,400 --> 00:01:07,200 ‫by times, frequency, times, time. 12 00:01:08,120 --> 00:01:14,330 ‫So if you have a spiral, then it essentially determines how many rotations you will have in one hundred 13 00:01:14,330 --> 00:01:14,970 ‫seconds. 14 00:01:15,770 --> 00:01:19,190 ‫And, well, if you remember, then you have nine trajectories. 15 00:01:20,060 --> 00:01:21,960 ‫This is your first trajectory. 16 00:01:22,610 --> 00:01:26,630 ‫So these are your equations for your spiral. 17 00:01:26,870 --> 00:01:31,220 ‫And if you remember, we also had to take their derivatives and their second derivative. 18 00:01:32,090 --> 00:01:37,550 ‫So this is your derivative part and then this is your second derivative part. 19 00:01:38,390 --> 00:01:40,870 ‫And we did this one in the lectures as well. 20 00:01:41,510 --> 00:01:44,830 ‫But then we also have different trajectories. 21 00:01:44,840 --> 00:01:51,980 ‫So we have trajectory to here, trajectory to is built in such a way that first of all, I create a 22 00:01:51,980 --> 00:02:00,920 ‫trajectory one and then somewhere in the middle, a little bit before that, I just overwrite the second 23 00:02:00,920 --> 00:02:01,380 ‫part. 24 00:02:02,120 --> 00:02:09,280 ‫So in trajectory to the first part of the trajectory would be the spiral that you have here. 25 00:02:09,680 --> 00:02:13,560 ‫But then the second part of that trajectory will be something else. 26 00:02:14,510 --> 00:02:21,230 ‫So that's why you see that I simply overwrite the elements starting from the one hundred and first element 27 00:02:21,230 --> 00:02:22,370 ‫up until the end. 28 00:02:23,120 --> 00:02:27,440 ‫And then you have its derivatives and second derivatives. 29 00:02:28,520 --> 00:02:36,110 ‫The same logic applies to trajectory three and trajectory four, where I overwrite the second part, 30 00:02:36,920 --> 00:02:39,890 ‫then trajectory five and trajectory six. 31 00:02:39,900 --> 00:02:41,000 ‫They are here. 32 00:02:41,460 --> 00:02:50,210 ‫They are like tornadoes and trajectory five, your radius increase linearly and then in trajectory six, 33 00:02:50,690 --> 00:02:53,960 ‫it increases with the power of two. 34 00:02:54,680 --> 00:02:58,430 ‫And so that's why trajectory six really looks like a tornado. 35 00:02:59,300 --> 00:03:06,790 ‫Then you have trajectory seven, you have trajectory eight and then you have trajectory nine. 36 00:03:06,830 --> 00:03:11,840 ‫This is the crown that I think I showed you one time in the lectures. 37 00:03:12,650 --> 00:03:17,440 ‫And, well, you can see that all these trajectories, they are inside the if statements. 38 00:03:17,440 --> 00:03:24,080 ‫So when you choose something in your init function, when you choose a trajectory, then it will give 39 00:03:24,080 --> 00:03:25,370 ‫you the right one. 40 00:03:25,950 --> 00:03:29,750 ‫So that's why you have these if and live statements. 41 00:03:30,620 --> 00:03:39,800 ‫Then here I calculate the difference of the X, Y and Z values, and here I add one more element to 42 00:03:39,800 --> 00:03:47,360 ‫it and they need that in order to construct our reference value vector for the yawing. 43 00:03:47,360 --> 00:03:59,360 ‫Also BPCI Subha and it's this one here and the code here is written in such a way that it remembers 44 00:04:00,200 --> 00:04:03,170 ‫how many rotations you have had. 45 00:04:03,770 --> 00:04:13,400 ‫So for example, if in a spiral you have rotated two times, then you will have gone not from a zero 46 00:04:13,400 --> 00:04:19,730 ‫to two pi radians two times, but you will have gone from zero to four pi radians. 47 00:04:19,910 --> 00:04:25,970 ‫And if you go in another direction then you will reference values become negative. 48 00:04:25,980 --> 00:04:34,610 ‫So if you start turning clockwise, then you go from zero to minus two pi radians or from zero to minus 49 00:04:34,610 --> 00:04:35,690 ‫four pi radians. 50 00:04:36,590 --> 00:04:45,680 ‫But this code remembers how many rotations you have had and then you just take all your vectors and 51 00:04:45,680 --> 00:04:47,240 ‫you give it to your main file. 52 00:04:48,020 --> 00:04:52,010 ‫And in the main file, these are your reference value vectors. 53 00:04:52,020 --> 00:04:54,050 ‫You see, you've got everything here. 54 00:04:54,050 --> 00:05:02,360 ‫You have X ref, X dot traffics, dot, dot, ref, the same thing for the wise and zis and also the 55 00:05:02,780 --> 00:05:07,880 ‫your angle reference vector and then this one here. 56 00:05:08,300 --> 00:05:13,300 ‫Well that's just the number of outer control loop iterations. 57 00:05:14,180 --> 00:05:22,120 ‫So when you go from zero to one hundred seconds with an interval of zero point four seconds, then it 58 00:05:22,130 --> 00:05:26,540 ‫essentially tells you how many elements you have in this factor here. 59 00:05:27,470 --> 00:05:35,870 ‫And I'm going to need that when I start running the outer loop and then here are my states. 60 00:05:35,990 --> 00:05:40,100 ‫All right, so I have u v w p q ah. 61 00:05:40,280 --> 00:05:46,160 ‫So these are the translational velocities in the body frame. 62 00:05:46,160 --> 00:05:49,190 ‫These are angular velocities in the body frame. 63 00:05:50,240 --> 00:05:58,610 ‫And these are your inertial frame, X, Y, Z states, and these are your inertial frame Fifita and 64 00:05:58,620 --> 00:06:03,260 ‫precise states following our convention. 65 00:06:03,500 --> 00:06:07,730 ‫That was our stop the Z, Y, X. 66 00:06:08,210 --> 00:06:13,670 ‫So we first rotate about the Z axis, right. 67 00:06:14,240 --> 00:06:21,200 ‫About the moving frame Z axis, then about the moving from Y axis and then about the moving frame X 68 00:06:21,200 --> 00:06:21,710 ‫axis. 69 00:06:22,550 --> 00:06:25,100 ‫And so all these are my initial states. 70 00:06:25,430 --> 00:06:28,100 ‫That's what I have at the very beginning. 71 00:06:28,730 --> 00:06:31,120 ‫So at the very beginning, I'm not going anywhere. 72 00:06:31,460 --> 00:06:39,770 ‫So that's why all my velocities are zero meters per second and zero radians per second. 73 00:06:40,310 --> 00:06:42,720 ‫And then these are my initial position values. 74 00:06:42,740 --> 00:06:47,310 ‫You can see that I start from X equals zero Y equals minus one meter. 75 00:06:47,420 --> 00:06:49,720 ‫So I'm not starting completely at the origin. 76 00:06:49,730 --> 00:06:56,600 ‫I'm a little bit shifted in the Y direction at the very beginning and then my initial altitude is zero 77 00:06:56,600 --> 00:06:57,110 ‫meters. 78 00:06:58,070 --> 00:07:06,470 ‫And then in terms of rule and pitch angles, these angles are zero radiance, so I'm not tilted in a 79 00:07:06,680 --> 00:07:08,070 ‫weird way or something. 80 00:07:09,050 --> 00:07:16,920 ‫However, my initial your angle is aligned with the first element of my reference, your angle vector. 81 00:07:17,810 --> 00:07:26,780 ‫So if my reference, your angle vector tells me that at the beginning I should be rotated by pi over 82 00:07:26,780 --> 00:07:34,190 ‫to radians, then at the very beginning I will also place my drone like that so that the drone would 83 00:07:34,190 --> 00:07:37,940 ‫be oriented like that as well in reality. 84 00:07:38,930 --> 00:07:44,890 ‫So normally if my body frame x axis is in this direction. 85 00:07:45,830 --> 00:07:48,110 ‫So this is my body from x axis here. 86 00:07:48,530 --> 00:07:58,010 ‫But if the first element of this vector is pi over to radians, then that means that I will take my 87 00:07:58,010 --> 00:08:05,240 ‫drone and I will rotate it in such a way that this body from x axis will point up. 88 00:08:06,140 --> 00:08:14,720 ‫And then if right now, if you look at this one, then your body from y axis would point up, then if 89 00:08:14,720 --> 00:08:22,400 ‫I rotate my body frame by prior to radians, then my body frame y axis will actually point to the left 90 00:08:23,150 --> 00:08:26,060 ‫because my body frame x axis would point up. 91 00:08:26,990 --> 00:08:35,030 ‫And then I take all my initial states and I'm going to put them in this array and I'm going to call 92 00:08:35,030 --> 00:08:42,650 ‫this area states and then I'm going to have your states total and I'm going to put states inside stepstool. 93 00:08:43,550 --> 00:08:48,950 ‫And the states tool, it will be accumulating all the states, so you can imagine that every year, 94 00:08:49,160 --> 00:08:53,230 ‫one second, all these states will update. 95 00:08:53,900 --> 00:08:57,500 ‫So I will have a new states vector. 96 00:08:57,710 --> 00:09:03,040 ‫However, I want to remember my previous states as well. 97 00:09:03,860 --> 00:09:09,940 ‫So I'm going to be collecting all the states throughout my time in this state's total vector. 98 00:09:10,730 --> 00:09:13,660 ‫And, well, this one is for the animation purposes. 99 00:09:14,420 --> 00:09:18,800 ‫And then here I have a vector for the angle reference values. 100 00:09:19,820 --> 00:09:27,350 ‫And then I also need some kind of a vector for the X, Y, Z velocities, so I'm going to need them 101 00:09:27,350 --> 00:09:31,070 ‫later once I start running my control loops. 102 00:09:32,080 --> 00:09:40,090 ‫And then I'm going to define my initial propellor angular velocity, so you see that they are three 103 00:09:40,090 --> 00:09:42,160 ‫thousand radians per second here. 104 00:09:42,880 --> 00:09:46,900 ‫So at the very beginning, my omega's, they are not at rest. 105 00:09:46,900 --> 00:09:48,190 ‫They are actually rotating. 106 00:09:48,370 --> 00:09:51,550 ‫They are rotating at three thousand radians per second. 107 00:09:51,790 --> 00:09:59,830 ‫However, this rotation will not produce enough thrust to overcome the force of gravity and therefore 108 00:09:59,950 --> 00:10:04,750 ‫your drone will not be flying anywhere with these rotations. 109 00:10:05,530 --> 00:10:12,340 ‫And well, here I take my thrust and drag factor and the length of the drone from the center of the 110 00:10:12,340 --> 00:10:14,620 ‫drone to the center of a propeller. 111 00:10:15,340 --> 00:10:24,190 ‫And based on this initial data here and based on these constants, I can compute my initial you want 112 00:10:24,190 --> 00:10:30,040 ‫you to Q3 and Q4 that I store in this vector. 113 00:10:30,880 --> 00:10:35,620 ‫And later on when I get my new you want you to use three four. 114 00:10:36,130 --> 00:10:38,770 ‫I will also store them in this vector. 115 00:10:38,770 --> 00:10:42,190 ‫But this vector will remember the previous controller inputs as well. 116 00:10:42,850 --> 00:10:50,440 ‫So I need them in order to be able to plot everything in the end is the same thing with this state's 117 00:10:50,440 --> 00:10:53,780 ‫total vector and the same thing here. 118 00:10:53,830 --> 00:10:55,720 ‫This Omega's bundle vector. 119 00:10:56,650 --> 00:11:03,100 ‫I want to collect all my omegle values throughout the time and then while this is again for the animation 120 00:11:03,100 --> 00:11:09,370 ‫purposes and now we can start our controller.