API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jan 18th, 2020
91
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 9.50 KB | None | 0 0
raw download clone embed print report
  1. %% Simulator
  2. % ideally this program only runs after drone has taken off and is in the
  3. % air, and ends before landing
  4. % need algorithm to tweak gains per conditions, as well z distance and time
  5. % seems to not be calculated correctly
  6.  
  7. clear all
  8. close all
  9.  
  10. Va = 370/3600; %kps
  11. % Va = 101; %kps
  12. psi = [];
  13. psi(1) = 0;
  14. DistanceTraveled = 0;
  15. wn = 0;
  16. we = 0;
  17. dt = 0.0051;
  18. dt = 0.0001;
  19. bx = 0.00008525; %0.00008525;
  20. bxdot = 0.0195; % 0.0195;
  21. bh = 0.000083525; %0.000083525;
  22. bhdot = 0.0291; % 0.0291;
  23.  
  24. bx = 25.558525; %0.00008525; 2.558525
  25. bxdot = 25.99759195; % 0.0195; 2.99759195
  26. bh = 0.00008525; %0.000083525;
  27. bhdot = 0.0195; % 0.0291;
  28. bva = 0.5;
  29. Pn = [];
  30. % Va = [];
  31. Vac = Va;
  32. h_dot = [];
  33. Pn_dot = [];
  34. Pe_dot = [];
  35. x_ddot = [];
  36. h_ddot = [];
  37. Va_dot = [];
  38. h = [];
  39. x = [];
  40. xc = [];
  41. % hc = [];
  42.  
  43. h(1) = 0;
  44. Pn(1) = 0;
  45. Pe(1) = 0;
  46. h_dot(1) = 0;
  47. x_dot(1) = 0;
  48. Target1Acquired = 0;
  49. Target2Acquired = 0;
  50. Target3Acquired = 0;
  51. Arrival2Target1Acquired = 0;
  52. Arrival2Target2Acquired = 0;
  53. Arrival2Target3Acquired = 0;
  54. DepartureTarget1Acquired = 0;
  55. DepartureTarget2Acquired = 0;
  56. DepartureTarget3Acquired = 0;
  57. success = 0;
  58. start = [0,0,0];
  59. t1 = [70,70,5];
  60. t1 = [-6,00,0];
  61. t2 = [150, 120, 10];
  62. t2 = [4, 8, 0];
  63.  
  64.  
  65. PC1 = [0.00, -0.27]; % Path circle center 1
  66. PC2 = [-14.7338, 0.0450];
  67. PC3 = [7.7301, 8.0062];
  68. PC4 = [3.1173, 9.7568];
  69. PC5 = [0.1909, 0.1909];
  70. tA = [90, -170.4105, -1.3112, 115.7497, -135.00]; % target location
  71. pDA = [90.8752, 108.2160, 69.2173, 162.9902]; % target departure location
  72. pAA = [-89.1248, -71.7840, 69.2173, 162.9902]; % Target arrival location
  73. r= 0.270; % radius of path circle
  74.  
  75. start= [0,0,0];
  76. t1 = [0,0,0];
  77. t2 = [0,0,0];
  78. t3 = [0,0,0];
  79. t4 = [0,0,0];
  80. t5 = [0,0,0];
  81. At1 = [0,0,0];
  82. Dt1 = [0,0,0];
  83. At2 = [0,0,0];
  84. Dt2 = [0,0,0];
  85. At3 = [0,0,0];
  86. Dt3 = [0,0,0];
  87. At4 = [0,0,0];
  88.  
  89. start(1) = PC1(1) + r*cosd(pDA(1));
  90. start(2) = PC1(2) + r*sind(pDA(1));
  91. t1(1) = PC2(1) + r*cosd(tA(2));
  92. t1(2) = PC2(2) + r*sind(tA(2));
  93. t2(1) = PC3(1) + r*cosd(tA(3));
  94. t2(2) = PC3(2) + r*sind(tA(3));
  95. t3(1) = PC4(1) + r*cosd(tA(4));
  96. t3(2) = PC4(2) + r*sind(tA(4));
  97. At1(1) = PC2(1) + r*cosd(pAA(1));
  98. At1(2) = PC2(2) + r*sind(pAA(1));
  99. At2(1) = PC3(1) + r*cosd(pAA(2));
  100. At2(2) = PC3(2) + r*sind(pAA(2));
  101. At3(1) = PC4(1) + r*cosd(pAA(3));
  102. At3(2) = PC4(2) + r*sind(pAA(3));
  103. At4(1) = PC1(1) + r*cosd(pAA(4));
  104. At4(2) = PC1(2) + r*sind(pAA(4));
  105. Dt1(1) = PC2(1) + r*cosd(pDA(2));
  106. Dt1(2) = PC2(2) + r*sind(pDA(2));
  107. Dt2(1) = PC3(1) + r*cosd(pDA(3));
  108. Dt2(2) = PC3(2) + r*sind(pDA(3));
  109. Dt3(1) = PC4(1) + r*cosd(pDA(4));
  110. Dt3(2) = PC4(2) + r*sind(pDA(4));
  111.  
  112.  
  113. hc = At1(3);
  114. posx = [];
  115. posy = [];
  116. posx(1) = start(1);
  117. posy(1) = start(2);
  118. Pn(1) = start(1);
  119. Pe(1) = start(2);
  120.  
  121. % DepartureTarget1Acquired=1;
  122. t = At1;
  123. xc(1) = atan2d((t(2)-start(2)),(t(1)-start(1)));
  124. x(1) = xc(1);
  125. for k = 1:4500000
  126. % Position = [posx(k), posy(k), h(k)];
  127. Position = [Pn(k), Pe(k), h(k)];
  128. [D2At1, D2t1,D2Dt1] = Distance2target3(Position,At1,t1,Dt1);
  129. [D2At2, D2t2,D2Dt2] = Distance2target3(Position,At2,t2,Dt2);
  130. [D2At3, D2t3,D2Dt3] = Distance2target3(Position,At3,t3,Dt3);
  131. [D2At4, D2H,~ ] = Distance2target3(Position,At4,start,Dt3);
  132.  
  133. % [D2H, ~] = Distance2target(Position,start,start);
  134. [D2H2d, ~] = Distance2target(Position,start,start);
  135.  
  136.  
  137.  
  138.  
  139. Dist2T1(k) = D2t1;
  140. Dist2T2(k) = D2t2;
  141.  
  142. if (D2At1 < .1)
  143. t = t1;
  144. hc = t1(3);
  145. Arrival2Target1Acquired=1;
  146. end
  147.  
  148. if ((D2t1 < .1) && (Arrival2Target1Acquired==1))
  149.  
  150. t = Dt1;
  151. hc = Dt1(3);
  152. Target1Acquired=1;
  153. end
  154.  
  155. if ((D2Dt1 < .1) && (Target1Acquired==1))
  156. t = At2;
  157. hc = t2(3);
  158. DepartureTarget1Acquired=1;
  159. end
  160.  
  161.  
  162.  
  163. if ((D2At2 < .1) && (DepartureTarget1Acquired==1))
  164. t = t2;
  165. hc = t2(3);
  166. Arrival2Target2Acquired=1;
  167. end
  168.  
  169. if ((D2t2 < .1) && (Arrival2Target2Acquired==1))
  170.  
  171. t = Dt2;
  172. hc = Dt2(3);
  173. Target2Acquired=1;
  174. end
  175.  
  176. if ((D2Dt2 < 0.1) && (Target2Acquired==1))
  177. t = At3;
  178. hc = t3(3);
  179. DepartureTarget2Acquired=1;
  180. end
  181.  
  182.  
  183. if ((D2At3 < .1) && (DepartureTarget2Acquired==1))
  184. t = t3;
  185. hc = t3(3);
  186. Arrival2Target3Acquired=1;
  187. end
  188.  
  189. if ((D2t3 < .2) && (Arrival2Target3Acquired==1))
  190.  
  191. Target3Acquired=1;
  192. %%%%%%%%%% HERE IS WHERE THE IF STATEMENT NO LONGER EXECUTES%%%%%%%%%%%%
  193. t = Dt3;
  194. hc = Dt3(3);
  195. end
  196.  
  197. if ((D2Dt3 < .4) && (Target3Acquired==1))
  198. t = At4;
  199. hc = At4(3);
  200. DepartureTarget3Acquired=1;
  201. end
  202.  
  203. if ((D2At4 < .1) && (DepartureTarget3Acquired==1))
  204. break
  205. end
  206.  
  207.  
  208. if ((Target1Acquired==1) && (Target2Acquired==1) && (Target3Acquired==1))
  209. success = 1;
  210. end
  211.  
  212. % % %
  213. % % % if ( (success == 1))
  214. % % %
  215. % % % bh = 0.0013525; %0.000083525;
  216. % % % bhdot = 0.1191; % 0.0291;
  217. % % % bx = 0.000098525; %0.00008525;
  218. % % % bxdot = 0.01; % 0.0195;
  219. % % % bx = 0.00008525; %0.000525;
  220. % % % bxdot = 0.0195; % 0.1;
  221. % % % bh = 0.00008525; %0.000083525;
  222. % % % bhdot = 0.0195; % 0.0291;
  223. % % %
  224. % % % % hc = (start(3) + hc)/2;
  225. % % % end
  226.  
  227. % if ((D2H2d < 10) && (success == 1))
  228. % hc = start(3);
  229. % end
  230.  
  231. if ((D2H < 2) && (success == 1))
  232. break
  233. end
  234.  
  235.  
  236. Pn_dot(k) = Va*cosd(psi(k)) + wn; %
  237. Pe_dot(k) = Va*sind(psi(k)) + we;
  238. xc(k) = atan2d((t(2)-posy(k)),(t(1)-posx(k)));
  239. xc(k) = atan2d((t(2)-Pe(k)),(t(1)-Pn(k)));
  240. % xc(k) = atand((t(2)-Pn(k))/(t(1)-Pe(k)));
  241. x_ddot(k) = bxdot*(0-x_dot(k)) +bx*(xc(k) - x(k));
  242. h_ddot(k) = bhdot*(0-h_dot(k)) + bh*(hc - h(k));
  243. if (h_ddot(k) > 1e-4)
  244. h_ddot(k) = 1e-4;
  245. end
  246. if (h_ddot(k) < -1e-4)
  247. h_ddot(k) = -1e-4;
  248. end
  249.  
  250. % if (x_ddot(k) > 15)
  251. % x_ddot(k) = 15;
  252. % end
  253. % if (x_ddot(k) < -15)
  254. % x_ddot(k) = -15;
  255. % end
  256.  
  257.  
  258. posx(k+1) = posx(k) + Va*dt*cosd(psi(k));
  259. posy(k+1) = posy(k) + Va*dt*sind(psi(k));
  260. % Va_dot(k) = bva*(Vac-Va);
  261.  
  262. Pn(k+1) = Pn(k) + Pn_dot(k)*dt;
  263. Pe(k+1) = Pe(k) + Pe_dot(k)*dt;
  264. % Va(k+1) = Va(k) + Va_dot(k)*dt;
  265. h_dot(k+1) = h_dot(k) + h_ddot(k)*dt;
  266. h(k+1) = h(k) + h_dot(k)*dt;
  267. x_dot(k+1) = x_dot(k) + x_ddot(k)*dt;
  268. x(k+1) = x(k) + x_dot(k)*dt;
  269. psi(k+1) = x(k+1);
  270. % if (h(k+1) > t1(3))
  271. % break
  272. % end
  273.  
  274. DistanceTraveled = DistanceTraveled + sqrt((Pn(k+1)-Pn(k))^2 + (Pe(k+1)-Pe(k))^2 + (h(k+1)-h(k))^2); %km
  275.  
  276. if (DistanceTraveled > 300)
  277. break
  278. end
  279.  
  280. end
  281.  
  282. MissionTime = (k*dt)/60; %minutes
  283. % DistanceTraveled = (k*dt)*Va; % km
  284.  
  285. str1 = "Mission sucessful ";
  286. str2 = "Need more time ";
  287. str3 = "Mission completed in " + MissionTime + " minutes.";
  288. str4 = "Mission ran for " + MissionTime + " minutes.";
  289. str5 = "Total distance traveled was " + DistanceTraveled + " kilometers.";
  290. strT1 = "Target 1 captured. ";
  291. strT2 = "Target 2 captured. ";
  292. strT3 = "Target 3 captured. ";
  293.  
  294. if (Target1Acquired ==1)
  295. fprintf(strT1); fprintf('\n');
  296. end
  297.  
  298. if (Target2Acquired ==1)
  299. fprintf(strT2); fprintf('\n');
  300. end
  301.  
  302. if (Target3Acquired ==1)
  303. fprintf(strT3); fprintf('\n');
  304. end
  305.  
  306. if ((D2H < 2) && (success == 1))
  307. fprintf(str1); fprintf('\n');
  308. fprintf(str3); fprintf('\n');
  309. fprintf(str5); fprintf('\n');
  310. else
  311. fprintf(str2); fprintf('\n');
  312. fprintf(str4); fprintf('\n');
  313. fprintf(str5); fprintf('\n');
  314. end
  315.  
  316. % figure(42)
  317. %
  318. % plot3(t1(1),t1(2),t1(3),'r o',t2(1),t2(2),t2(3),'k o' );
  319. % hold on
  320. % grid on
  321. % curve = animatedline('Linewidth',1);
  322. % set(gca,'Xlim',[(min(Pn)-10) (max(Pn)+10)], 'Ylim',[(min(Pe)-10) (max(Pe)+10)], 'Zlim',[(min(h)-10) (max(h)+10)]);
  323. % title('Trajectory of vehicle')
  324. % xlabel('x km');
  325. % ylabel('y km');
  326. % zlabel('z km');
  327. % legend({ 'target_1','target_2','Vehicle path'}, 'Location', 'southwest');
  328. %
  329. %
  330. % for g=1:k
  331. % addpoints(curve,Pn(g),Pe(g),h(g));
  332. % head = scatter3(Pn(g),Pe(g),h(g),'*','b');
  333. % drawnow limitrate
  334. % % pause(0.00005)
  335. % delete(head);
  336. % end
  337. %
  338. % hold off
  339.  
  340.  
  341. figure(41)
  342. % axis('Xlim',[(min(Pn)-10) (max(Pn)+10)], 'Ylim',[(min(Pe)-10) (max(Pe)+10)], 'Zlim',[(min(h)-10) (max(h)+10)]);
  343.  
  344. plot3(Pn,Pe,h)
  345. grid on;
  346. hold on
  347. plot3(t1(1),t1(2),t1(3),'r o',At1(1),At1(2),At1(3),'r *',Dt1(1),Dt1(2),Dt1(3),'r *' );
  348. hold on
  349. plot3(t2(1),t2(2),t2(3),'k o',At2(1),At2(2),At2(3),'k *',Dt2(1),Dt2(2),Dt2(3),'k *' );
  350. hold on
  351. plot3(t3(1),t3(2),t3(3),'b o',At3(1),At3(2),At3(3),'b *',Dt3(1),Dt3(2),Dt3(3),'b *' );
  352. hold on
  353. % plot3(p(1,breakpoint-1),p(2,breakpoint-1),p(3,breakpoint-1),'*')
  354. hold off;
  355. title('Trajectory of vehicle')
  356. xlabel('x km');
  357. ylabel('y km');
  358. zlabel('z km');
  359. legend({'Vehicle stepping on path', 'target_1','target_2'...
  360. }, 'Location', 'northwest');
  361. % axis square;
  362.  
  363. % %
  364. % % figure(1)
  365. % % plot(x)
  366. % % title('height position')
  367. % % xlabel('time (s)');
  368. % % ylabel('position km');
  369. % %
  370. % % figure(2)
  371. % % plot(x_dot)
  372. % % title('height velocity')
  373. % % xlabel('time (s)');
  374. % % ylabel('velocity km/s');
  375. % %
  376. % % figure(3)
  377. % % plot(x_ddot)
  378. % % title('height acceleration')
  379. % % xlabel('time (s)');
  380. % % ylabel('acceleration km/ss');
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!