Guest User

Peti

a guest
Apr 23rd, 2019
89
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. % program na hladanie kociek robotom v bludisku s prekazkami,
  2. % robot ma 3 snimace prekazok (vpredu, vlavo, vpravo), otaca sa o 90stupnov
  3. % vlavo alebo vpravo, ide iba dopredu
  4. % cielom je najst vsetky kocky v bludisku, po najdeni kocky ju priniest do
  5. % ciela do pozicie (40,40) 1. typ objektu, do pozicie (1,40) 2. typ objektu
  6.  
  7. % toto riesenie obsahuje aj radar na hladanie kocky v def. okruhu, po
  8. % najdeni kocky ju zoberie do ciela
  9.  
  10. %robot=[poziciaX poziciaY snimacLavo snimacPravo snimacVpred pohyb otocenie smer kocka prekazka cielx ciely offset_rand gain_rand]
  11.  
  12. % poziciaX - stlpec 1 az 40, poziciaY - riadok 1 az 40
  13.  
  14. % snimacLavo snimacPravo snimacVpred , 0-volne, 1-prekazka, 2-kocka,
  15. % 3-iny robot
  16.  
  17. % pohyb , 0-stoji, 1-ide
  18.  
  19. % otocenie, 0-rovno, 1-doprava, 2-dolava
  20.  
  21. % smer, 1-hore, 2-dole, 3-doprava, 4-dolava
  22.  
  23. % kocka, 0-nema kocku,hlada ju, 1-nesie kocku na ciel (40,40), 2 - ide na
  24. % definovany ciel
  25.  
  26. % prekazka, kde je prekazka 0-nie je, 1-hore, 2-dole, 3-doprava, 4-dolava,
  27. % ak su zaporne hodnoty, prekazka je v cieli
  28.  
  29. % typ kocka , 1 - objekt 1. typu (napr. jablko, hruska), 2 - objekt 2. typu (napr. banan, pomaranc, citron)
  30.  
  31. % cielX - stlpec 1 az 40, cielY - riadok 1 az 40
  32.  
  33. % param1, param2 - parametre pre random hladanie
  34.  
  35. % hlavna funkcia
  36. function [caszberu]=mainrobot(pocetkociek,pocetprekazok)
  37.  
  38. h1=figure;
  39.  
  40. % definovanie color mapy 1-pozadie, 2-prekazka, 3-robot, 4-robot s kockou,
  41. % 5- kocka 1 typ , 6- kocka 2 typ, 7 - neodkryta mapa
  42. colormaprobot=[1 1 1;0 0 0;0 1 0;1 0 0;0 0 1;1 0 1;1 1 0];
  43. mapa=ones(42);
  44. mapa(1,:)=2;
  45. mapa(42,:)=2;
  46. mapa(:,1)=2;
  47. mapa(:,42)=2;
  48.  
  49. % budovana mapa 1-pozadie, 2-prekazka, 3-robot, 4-robot s kockou,
  50. % 5- kocka 1 typ , 6- kocka 2 typ, 7 - neodkryta mapa
  51. mapazobraz=7*ones(42);
  52. mapazobraz(1,:)=2;
  53. mapazobraz(42,:)=2;
  54. mapazobraz(:,1)=2;
  55. mapazobraz(:,42)=2;
  56.  
  57. odkrytiemapy=zeros(42);
  58.  
  59. % definovanie prekazok
  60. if pocetprekazok>0
  61. mapa(2:5,8)=2;
  62. mapa(6,8:10)=2;
  63.  
  64. mapa(2:7,23)=2;
  65. mapa(8,19:23)=2;
  66.  
  67. mapa(34:41,28)=2;
  68. mapa(34,25:28)=2;
  69.  
  70. mapa(34,10:12)=2;
  71. mapa(35:41,10)=2;
  72.  
  73. mapa(20,32:41)=2;
  74. mapa(18:22,32)=2;
  75.  
  76. mapa(22,12:20)=2;
  77. mapa(19:25,16)=2;
  78.  
  79. end
  80.  
  81. stanicaX=40;
  82. stanicaY=40;
  83.  
  84. pocetrobot=3;
  85.  
  86. % maximalny cas trvania simulacie
  87. maxcashladania=120*pocetkociek;
  88.  
  89. % zoznam kociek
  90. % 1-pozicia X, 2-pozicia Y, 3 - stav kocky, 4 - typ, 5 - parameter 1, 6 - parameter 2, 7 - parameter 3
  91. zoznamkociek=[];
  92.  
  93. % definovanie parametrov robota
  94. roboti(1)=struct('poziciaX',1,'poziciaY',1,'snimacLavo',1,'snimacPravo',0,'snimacVpred',1,'pohyb',0,'otocenie',0, ...
  95. 'smer',1,'kocka',0,'prekazka',0,'typkocka',1,'cielX',40,'cielY',40,'param1',10,'param2',40,'pocitadlo',0,'maxkrok',0,'bludenie',0);
  96.  
  97. % zakreslenie robota
  98. mapa(2,2)=3;
  99.  
  100. if pocetrobot>1
  101. %roboti(2,:)=[1 40 1 0 0 0 0 1 0 0 40 40 3 20];
  102. roboti(2)=struct('poziciaX',1,'poziciaY',40,'snimacLavo',1,'snimacPravo',0,'snimacVpred',0,'pohyb',0,'otocenie',0, ...
  103. 'smer',1,'kocka',0,'prekazka',0,'typkocka',1,'cielX',40,'cielY',40,'param1',3,'param2',20,'pocitadlo',0,'maxkrok',0,'bludenie',0);
  104.  
  105. mapa(41,2)=3;
  106. if pocetrobot>2
  107. % roboti(3,:)=[40 1 1 0 0 0 0 2 0 0 40 40 8 30];
  108. roboti(3)=struct('poziciaX',40,'poziciaY',1,'snimacLavo',1,'snimacPravo',0,'snimacVpred',0,'pohyb',0,'otocenie',0, ...
  109. 'smer',2,'kocka',0,'prekazka',0,'typkocka',1,'cielX',40,'cielY',40,'param1',8,'param2',30,'pocitadlo',0,'maxkrok',0,'bludenie',0);
  110.  
  111. mapa(2,41)=3;
  112. pocetrobot=3;
  113. end
  114. end
  115.  
  116. % parametre na nahodne hladanie
  117. for ii=1:pocetrobot,
  118. roboti(ii).maxkrok=fix(40*rand);
  119. end
  120.  
  121. khladaj=zeros(1,pocetrobot);
  122. nahodahladaj=fix(40*rand(1,pocetrobot));
  123. timeout=[0 0 0];
  124.  
  125. % parametre na nahodne hladanie
  126. % khladaj=0;
  127. % nahodahladaj=fix(40*rand(1,1));
  128.  
  129. % definovanie pozicii kociek
  130. kockypos=[4 11;15 22;23 18;25 35;37 16;37 5;10 6;22 8;35 32;8 32;15 35;12 15;30 20;12 28;20 25;33 3;28 9; ...
  131. 14 9;4 21;4 28;38 24;12 38;28 14;3 16;17 13;26 4;26 26;5 6;16 5;29 31;38 12;32 36;38 20;3 35;10 11];
  132.  
  133. if pocetkociek>35
  134. pocetkociek=35;
  135. end
  136. indx=randperm(35);
  137. for i=1:pocetkociek,
  138. if rand(1)>0.5
  139. mapa(kockypos(indx(i),1),kockypos(indx(i),2))=5;
  140. else
  141. mapa(kockypos(indx(i),1),kockypos(indx(i),2))=6;
  142. end
  143. end
  144.  
  145. % pociatocne vykreslenie mapy
  146. colormap(colormaprobot);
  147. hmapa=image(mapa);
  148. k=0;
  149. pocetuloh=0;
  150. title(['ulozene kocky = ' num2str(pocetuloh)])
  151.  
  152. % % zobrazenie budovanej mapy
  153. % h2=figure;
  154. % colormap(colormaprobot);
  155. % hmapa2=image(mapazobraz);
  156.  
  157.  
  158. % hlavny cyklus
  159. while k<maxcashladania && pocetuloh~=pocetkociek;
  160. k=k+1;
  161. for rrob=1:pocetrobot,
  162.  
  163. % vyber pixelov z mapy
  164. hp=mapa(roboti(rrob).poziciaY,roboti(rrob).poziciaX+1);
  165. dp=mapa(roboti(rrob).poziciaY+2,roboti(rrob).poziciaX+1);
  166. pp=mapa(roboti(rrob).poziciaY+1,roboti(rrob).poziciaX+2);
  167. vp=mapa(roboti(rrob).poziciaY+1,roboti(rrob).poziciaX);
  168.  
  169. % nastavenie snimacov robota
  170. [vs,ps,hs]=nastavsnimace(roboti(rrob),hp,dp,pp,vp);
  171.  
  172. roboti(rrob).snimacLavo=vs;
  173. roboti(rrob).snimacPravo=ps;
  174. roboti(rrob).snimacVpred=hs;
  175.  
  176. % chod do stanice
  177. if roboti(rrob).kocka==1
  178. %novysmer=choddostanice(roboti(rrob));
  179. if roboti(rrob).typkocka==2
  180. roboti(rrob).cielX=40;
  181. roboti(rrob).cielY=1;
  182. else
  183. roboti(rrob).cielX=40;
  184. roboti(rrob).cielY=40;
  185. end
  186. [novysmer,novaprekazka]=chodnaciel(roboti(rrob));
  187. roboti(rrob).prekazka=novaprekazka;
  188. if novysmer>-1
  189. roboti(rrob).otocenie=novysmer;
  190. roboti(rrob).pohyb=1;
  191. timeout(rrob)=0;
  192. else
  193. if novaprekazka<0
  194. roboti(rrob).otocenie=0;
  195. roboti(rrob).prekazka=0;
  196. roboti(rrob).smer=abs(novaprekazka);
  197. else
  198.  
  199. roboti(rrob).pohyb=0;
  200. timeout(rrob)=timeout(rrob)+1;
  201. if timeout(rrob)>3
  202. if rem(roboti(rrob).smer,2)==1
  203. roboti(rrob).smer=roboti(rrob).smer+1;
  204. else
  205. roboti(rrob).smer=roboti(rrob).smer-1;
  206. end
  207. timeout(rrob)=0;
  208. end
  209. end
  210. end
  211.  
  212. % chod na ciel
  213. elseif roboti(rrob).kocka==2
  214. [novysmer,novaprekazka]=chodnaciel(roboti(rrob));
  215. roboti(rrob).prekazka=novaprekazka;
  216. if novysmer>-1
  217. roboti(rrob).otocenie=novysmer;
  218. roboti(rrob).pohyb=1;
  219. timeout(rrob)=0;
  220. else
  221. % ak je prekazka v cieli kocka, zober ju
  222. if novaprekazka<0
  223. roboti(rrob).otocenie=0;
  224. roboti(rrob).prekazka=0;
  225. roboti(rrob).smer=abs(novaprekazka);
  226. else
  227.  
  228. roboti(rrob).pohyb=0;
  229. if roboti(rrob).poziciaX==roboti(rrob).cielX && roboti(rrob).poziciaY==roboti(rrob).cielY
  230. roboti(rrob).kocka=0;
  231.  
  232. end
  233. % timeout(rrob)=timeout(rrob)+1;
  234. % if timeout(rrob)>5
  235. % if rem(roboti(rrob).smer,2)==1
  236. % roboti(rrob).smer=roboti(rrob).smer+1;
  237. % else
  238. % roboti(rrob).smer=roboti(rrob).smer-1;
  239. % end
  240. % timeout(rrob)=0;
  241. % end
  242.  
  243. end
  244. end
  245.  
  246. % nahodne hladanie
  247. else
  248. [novysmer,kk,nahoda]=nahodnehladanie(roboti(rrob),khladaj(rrob),nahodahladaj(rrob));
  249. % nastavenie smeru
  250. if novysmer>-1
  251. roboti(rrob).otocenie=novysmer;
  252. roboti(rrob).pohyb=1;
  253. else
  254. roboti(rrob).pohyb=0;
  255. if rem(roboti(rrob).smer,2)==1
  256. roboti(rrob).smer=roboti(rrob).smer+1;
  257. else
  258. roboti(rrob).smer=roboti(rrob).smer-1;
  259. end
  260.  
  261. end
  262. khladaj(rrob)=kk;
  263. nahodahladaj(rrob)=nahoda;
  264. end
  265.  
  266. % nastav novu poziciu robota
  267. stareX=roboti(rrob).poziciaX;
  268. stareY=roboti(rrob).poziciaY;
  269. mapa(stareY+1,stareX+1)=1;
  270.  
  271. noveX=stareX;
  272. noveY=stareY;
  273.  
  274. % ak je robot v pohybe nastav smer cesty
  275. if roboti(rrob).pohyb==1
  276. [posX,posY,otoc]=nastavsmercesty(roboti(rrob));
  277. noveX=posX;
  278. noveY=posY;
  279. if otoc>0
  280. roboti(rrob).smer=otoc;
  281. end
  282.  
  283. end
  284.  
  285. roboti(rrob).poziciaX=noveX;
  286. roboti(rrob).poziciaY=noveY;
  287.  
  288. % co vykona robot v jednotlivych stavoch
  289. [robotstav,nasielkocku,polozilkocku,zoznamkociek]=akciarobot(roboti(rrob),mapa,zoznamkociek,odkrytiemapy);
  290. roboti(rrob)=robotstav;
  291.  
  292. % kontrola ci nahodou neprejde prekazku
  293. if mapa(roboti(rrob).poziciaY+1,roboti(rrob).poziciaX+1)==2
  294. noveX=stareX;
  295. noveY=stareY;
  296. roboti(rrob).poziciaX=noveX;
  297. roboti(rrob).poziciaY=noveY;
  298. end
  299.  
  300. % zakreslenie novej pozicie robota
  301. if roboti(rrob).kocka==1
  302. mapa(noveY+1,noveX+1)=4;
  303. else
  304. mapa(noveY+1,noveX+1)=3;
  305. end
  306.  
  307. % % budovanie mapy
  308. % odkrytiemapy=budujmapu(odkrytiemapy,roboti(rrob));
  309.  
  310. % prekreslenie mapy
  311. figure(h1)
  312. set(hmapa,'CData',mapa)
  313.  
  314. % vypisy do okna
  315. if nasielkocku==1
  316. title(['ulozene kocky = ' num2str(pocetuloh) ' cas [s] = ' num2str(k)])
  317. end
  318. if polozilkocku==1
  319. pocetuloh=pocetuloh+1;
  320. title(['ulozene kocky = ' num2str(pocetuloh) ' cas [s] = ' num2str(k)])
  321. end
  322.  
  323. % na spomalenie priebehu simulacie
  324. pause(0.001)
  325.  
  326. end
  327.  
  328. % if rem(k,5)==0
  329. % figure(h2)
  330. % set(hmapa2,'CData',mapazobraz)
  331. % end
  332.  
  333. end
  334. caszberu=k;
  335. end
  336.  
  337.  
  338. % fukcia, ktora riesi akcie, co sa robot robit v urcitych stavoch
  339. % robot - struktura robota
  340. % mapab - aktualna mapa bludiska
  341. % zkvstup - zoznam najdenych kociek
  342. % odkrytiemapy1 - kde uz bol
  343. function [robotupdate,nasielkocku,polozilkocku,zkvystup]=akciarobot(robot,mapab,zkvstup,odkrytiemapy1)
  344.  
  345. dosahradaru=3;
  346. nasielkocku=0;
  347. polozilkocku=0;
  348. zkvystup=zkvstup;
  349.  
  350. % ak nasiel kocku, nastav stav zober kocku
  351. if mapab(robot.poziciaY+1,robot.poziciaX+1)==5 || mapab(robot.poziciaY+1,robot.poziciaX+1)==6
  352. robot.kocka=1;
  353. robot.smer=2;
  354. robot.otocenie=0;
  355. nasielkocku=1;
  356. robot.typkocka=1;
  357.  
  358. % doplnit rozpoznavanie pomocou NS
  359. [typkocky,paramkocky]=citajparamkocky(mapab,robot.poziciaX,robot.poziciaY);
  360. robot.typkocka=typkocky;
  361.  
  362. end
  363. %________________________________
  364. if robot.kocka==1 || robot.kocka==2
  365. [novakocka]=najdikocku(robot,mapab,dosahradaru);
  366. % ak nasiel radar ine kocky
  367. if ~(isempty(novakocka)) && robot.cielX~=novakocka(1,2) && robot.cielY~=novakocka(1,1)
  368. % ak je zoznam prazdny
  369. if isempty(zkvstup)
  370. zkvstup=[0, novakocka(1,1:2)];
  371. else
  372. % aby sa neopakovali kocky v zozname
  373. pridaj=1; % ano, mozes pridat novu kocku k zoznamu
  374. for i=1:size(zkvstup,1)
  375. for j=1:size(novakocka,1)
  376. if novakocka(j,1)==zkvstup(i,2) && novakocka(j,2)==zkvstup(i,3)
  377. pridaj=0; % kocka uz sa nachadza v zozname
  378. break;
  379. end
  380. end
  381. end
  382. % ak kocku mozno pridat
  383. if pridaj==1
  384. tmp=[0 novakocka(1,1:2)];
  385. zkvstup=[zkvstup;tmp];
  386. end
  387.  
  388. end
  389.  
  390. end
  391. end
  392. %----------------------------------------------------------------------------------------
  393.  
  394. % ak nesie kocku
  395. if robot.kocka==1
  396.  
  397. % ak priniesol kocku do ciela
  398. if robot.typkocka==1
  399. if robot.poziciaX==40 && robot.poziciaY==40
  400. robot.kocka=0;
  401. polozilkocku=1;
  402. end
  403. elseif robot.typkocka==2
  404. if robot.poziciaX==40 && robot.poziciaY==1
  405. robot.kocka=0;
  406. polozilkocku=1;
  407. end
  408. end
  409. end
  410.  
  411.  
  412.  
  413.  
  414. % ak nema kocku prehlada priestor ultrazvukom
  415. if robot.kocka==0
  416.  
  417. %--------------------------------------------------------------------------------------
  418. % ak zoznam kociek nie je parzdny, posleme prazdneho
  419. % robota pre 1. kocku
  420. test=0;
  421. if ~isempty(zkvstup)
  422. for i=1:size(zkvstup,1)
  423. if zkvstup(i,1)==0
  424. test=1;
  425. break;
  426. end
  427. end
  428. end
  429. if ~isempty(zkvstup) && test==1
  430. for i=1:size(zkvstup,1)
  431. if zkvstup(i,1)==0
  432. x=zkvstup(i,3);
  433. robot.cielX=x;
  434. y=zkvstup(i,2);
  435. robot.cielY=y;
  436. robot.kocka=2;
  437. robot.pocitadlo=0;
  438. robot.maxkrok=2*(abs(robot.poziciaX-robot.cielX)+abs(robot.poziciaY-robot.cielY));
  439. zkvstup(i,1)=1;
  440. Rx=num2str(robot.poziciaX);
  441. Ry=num2str(robot.poziciaY);
  442. X=['Robot ',Rx,' ',Ry, ' ide pre kocku c. ',num2str(i), ' so suradnicami ', num2str(x),' ',num2str(y)];
  443. disp(X);
  444. break;
  445. end
  446. end
  447.  
  448. else
  449.  
  450.  
  451. %---------------------------------------------------------------------------------------
  452.  
  453.  
  454. [najdenepozkocky]=najdikocku(robot,mapab,dosahradaru);
  455. % ak najde kocku nastavi ciel na jej poziciu
  456. if isempty(najdenepozkocky)==0
  457. robot.cielX=najdenepozkocky(1,2);
  458. robot.cielY=najdenepozkocky(1,1);
  459. robot.kocka=2;
  460. end
  461. end
  462. end
  463.  
  464. robotupdate=robot;
  465. zkvystup=zkvstup;
  466. end
  467.  
  468. % funkcia na budovanie mapy
  469. function odkrytiemapy2=budujmapu(odkrytiemapy1,robot)
  470. odkrytiemapy2=odkrytiemapy1;
  471.  
  472. % doprogramovavat
  473.  
  474.  
  475. end
  476.  
  477.  
  478. % funkcia, ktora vrati parametre objektu a jeho typ
  479. function [typkocky,paramkocky]=citajparamkocky(mapab,posX,posY)
  480.  
  481. pd=[0.91 0.07 0.91 0.03 0.5 0.4;0.75 0.15 0.77 0.06 0.57 0.27; ...
  482. 0.33 0.23 0.3 0.2 0.75 0.15;0.73 0.08 0.92 0.03 0.47 0.18;0.5 0.15 0.83 0.08 0.7 0.13];
  483.  
  484. paramkocky=zeros(1,3);
  485.  
  486. % doplnit rozpoznavanie pomocou NS
  487. if mapab(posY+1,posX+1)==6
  488. typkocky=2;
  489. druh=fix(3*rand(1))+3;
  490. else
  491. typkocky=1;
  492. druh=fix(3*rand(1))+1;
  493.  
  494. end
  495.  
  496. for i=1:3
  497. paramkocky(i)=pd(druh,2*(i-1)+1)+2*pd(druh,2*(i-1)+2)*rand(1)-pd(druh,2*(i-1)+2);
  498. end
  499. end
  500.  
  501.  
  502.  
  503. % funkcia pre riadenie robota do ciela
  504. % novysmer , 0-rovno, 1-vpravo, 2-vlavo, -1 - bez zmeny
  505. % prekazka - 0-nie je, 1-hore, 2-dole, 3-doprava, 4-dolava,
  506. % ak su zaporne hodnoty, prekazka je v cieli
  507. function [novysmer,prekazka]=chodnaciel(robot)
  508.  
  509. % suradnice ciela robota
  510. xciel=robot.cielX;
  511. yciel=robot.cielY;
  512.  
  513. % kde sa nachadza ciel od aktualnej pozicie robota, vlavo, vpravo, hore
  514. % dole
  515. if (xciel-robot.poziciaX)>0
  516. idevpravo=1;
  517. else
  518. idevpravo=0;
  519. end
  520. if (xciel-robot.poziciaX)<0
  521. idevlavo=1;
  522. else
  523. idevlavo=0;
  524. end
  525. if (yciel-robot.poziciaY)>0
  526. idedole=1;
  527. else
  528. idedole=0;
  529. end
  530. if (yciel-robot.poziciaY)<0
  531. idehore=1;
  532. else
  533. idehore=0;
  534. end
  535.  
  536. novysmer=-1;
  537. prekazka=robot.prekazka;
  538. % ak nenarazil na prekazku
  539. if prekazka==0
  540.  
  541. % ma ist smerom dole
  542. if idedole==1
  543. if robot.smer==2 && robot.snimacVpred==0
  544. novysmer=0;
  545. elseif robot.smer==3 && robot.snimacPravo==0 %ide vpravo, vpravo volno, otoc dole
  546. novysmer=1;
  547. elseif robot.smer==4 && robot.snimacLavo==0
  548. novysmer=2;
  549. elseif robot.smer==1
  550. if idevpravo==1 && robot.snimacPravo==0
  551. novysmer=1;
  552. elseif idevlavo==1 && robot.snimacLavo==0
  553. novysmer=2;
  554. end
  555. end
  556. end
  557.  
  558. if novysmer==-1
  559. % ma ist smerom hore
  560. if idehore==1
  561. if robot.smer==1 && robot.snimacVpred==0
  562. novysmer=0;
  563. elseif robot.smer==3 && robot.snimacLavo==0 %ide vpravo, vpravo volno, otoc dole
  564. novysmer=2;
  565. elseif robot.smer==4 && robot.snimacPravo==0
  566. novysmer=1;
  567. elseif robot.smer==2
  568. if idevpravo==1 && robot.snimacLavo==0
  569. novysmer=2;
  570. elseif idevlavo==1 && robot.snimacPravo==0
  571. novysmer=1;
  572. end
  573. end
  574. end
  575. end
  576.  
  577. if novysmer==-1
  578. % ma ist smerom vpravo
  579. if idevpravo==1
  580. if robot.smer==1 && robot.snimacPravo==0
  581. novysmer=1;
  582. elseif robot.smer==3 && robot.snimacVpred==0 %ide vpravo, vpravo volno, otoc dole
  583. novysmer=0;
  584. elseif robot.smer==2 && robot.snimacLavo==0
  585. novysmer=2;
  586. elseif robot.smer==4
  587. if idehore==1 && robot.snimacPravo==0
  588. novysmer=1;
  589. elseif idedole==1 && robot.snimacLavo==0
  590. novysmer=2;
  591. end
  592. end
  593. end
  594. end
  595.  
  596. if novysmer==-1
  597. % ma ist smerom vlavo
  598. if idevlavo==1
  599. if robot.smer==1 && robot.snimacLavo==0
  600. novysmer=2;
  601. elseif robot.smer==4 && robot.snimacVpred==0 %ide vpravo, vpravo volno, otoc dole
  602. novysmer=0;
  603. elseif robot.smer==2 && robot.snimacPravo==0
  604. novysmer=1;
  605. elseif robot.smer==3
  606. if idehore==1 && robot.snimacLavo==0
  607. novysmer=2;
  608. elseif idedole==1 && robot.snimacPravo==0
  609. novysmer=1;
  610. end
  611. end
  612. end
  613. end
  614.  
  615. % nie je mozny ziadny smer
  616. if novysmer==-1
  617. % ak nie je v cieli, tak nastav prekazku
  618. if xciel~=robot.poziciaX
  619. if idevpravo==1 && (xciel-robot.poziciaX)==1
  620. prekazka=-3;
  621. elseif idevpravo==1
  622. prekazka=3;
  623. elseif idevlavo==1 && (robot.poziciaX-xciel)==1
  624. prekazka=-4;
  625. elseif idevlavo==1
  626. prekazka=4;
  627. end
  628. elseif yciel~=robot.poziciaY
  629. if idehore==1 && (robot.poziciaY-yciel)==1
  630. prekazka=-1;
  631. elseif idehore==1
  632. prekazka=1;
  633. elseif idedole==1 && (yciel-robot.poziciaY)==1
  634. prekazka=-2;
  635. elseif idedole==1
  636. prekazka=2;
  637. end
  638. end
  639. end
  640.  
  641. % ak je prekazka, potom jej obchadzanie
  642. else
  643.  
  644. % prekazka vpravo
  645. if prekazka==3 && yciel<21
  646. if robot.smer==3 && robot.snimacVpred~=0 && robot.snimacPravo~=0 && robot.snimacLavo==0 % ak ide doprava,prekazka vpredu a vpravo, vlavo volno otoc dolava hore
  647. novysmer=2;
  648. elseif robot.smer==3 && robot.snimacVpred~=0 && robot.snimacPravo==0 % ak ide doprava,prekazka vpredu , vpravo volno otoc doprava hole
  649. novysmer=1;
  650. elseif robot.smer==2 && robot.snimacLavo~=0 && robot.snimacVpred==0 % ak ide dole,prekazka vlavo, vpred volno pokracuj rovno
  651. novysmer=0;
  652. elseif robot.smer==2 && robot.snimacLavo~=0 && robot.snimacVpred~=0 % ak ide dole,prekazka vlavo a rovno, otoc vpravo
  653. novysmer=1;
  654. elseif robot.smer==2 && robot.snimacLavo==0 && robot.snimacVpred==0 % ak ide dole,prekazka vlavo a rovno nieje, otoc dolava
  655. novysmer=2;
  656. elseif robot.smer==2 && robot.snimacLavo==0 && robot.snimacVpred~=0 && robot.snimacPravo==0 % ak ide dole,prekazka rovno, vlavo a vpravo je volno, otoc doprava
  657. novysmer=1;
  658. elseif robot.smer==4 && robot.snimacLavo~=0 && robot.snimacVpred==0 % ak ide dolava,prekazka vlavo, vpred je volno, chod rovno
  659. novysmer=0;
  660. elseif robot.smer==4 && robot.snimacLavo==0 % ak ide dolava, vlavo je volno, chod dolava
  661. novysmer=2;
  662. elseif robot.smer==1 % ak ide dole, toc dolava
  663. novysmer=2;
  664. elseif robot.smer==3 && robot.snimacVpred==0 && robot.poziciaX<xciel % ak ide doprava, rovno je volno, este nie je v cieli, chod rovno a vypni obchadzanie prekazky
  665. novysmer=0;
  666. prekazka=0;
  667. end
  668.  
  669. elseif prekazka==3 && yciel>20
  670. if robot.smer==3 && robot.snimacVpred~=0 && robot.snimacLavo~=0 && robot.snimacPravo==0 % ak ide doprava,prekazka vpredu a vlavo, vpravo volno otoc doprava dole
  671. novysmer=1;
  672. elseif robot.smer==3 && robot.snimacVpred~=0 && robot.snimacLavo==0 % ak ide doprava,prekazka vpredu , vlavo volno otoc dolava hole
  673. novysmer=2;
  674. elseif robot.smer==1 && robot.snimacPravo~=0 && robot.snimacVpred==0 % ak ide hore,prekazka vpravo, vpred volno pokracuj rovno
  675. novysmer=0;
  676. elseif robot.smer==1 && robot.snimacPravo~=0 && robot.snimacVpred~=0 % ak ide hore,prekazka vpravo a rovno, otoc vlavo
  677. novysmer=2;
  678. elseif robot.smer==1 && robot.snimacPravo==0 && robot.snimacVpred==0 % ak ide hore,prekazka vpravo a rovno nieje, otoc doprava
  679. novysmer=1;
  680. elseif robot.smer==1 && robot.snimacPravo==0 && robot.snimacVpred~=0 && robot.snimacLavo==0 % ak ide hore,prekazka rovno, vlavo a vpravo je volno, otoc dolava
  681. novysmer=2;
  682. elseif robot.smer==4 && robot.snimacPravo~=0 && robot.snimacVpred==0 % ak ide dolava,prekazka vpravo, vpred je volno, chod rovno
  683. novysmer=0;
  684. elseif robot.smer==4 && robot.snimacPravo==0 % ak ide dolava, vpravo je volno, chod doprava
  685. novysmer=1;
  686. elseif robot.smer==2 % ak ide dole, toc dolava
  687. novysmer=1;
  688. elseif robot.smer==3 && robot.snimacVpred==0 && robot.poziciaX<xciel % ak ide doprava, rovno je volno, este nie je v cieli, chod rovno a vypni obchadzanie prekazky
  689. novysmer=0;
  690. prekazka=0;
  691. end
  692. end
  693.  
  694. % prekazka vlavo
  695. if prekazka==4 && yciel>20
  696. if robot.smer==4 && robot.snimacVpred~=0 && robot.snimacPravo~=0 && robot.snimacLavo==0 % ak ide doprava,prekazka vpredu a vpravo, vlavo volno otoc dolava hore
  697. novysmer=2;
  698. elseif robot.smer==4 && robot.snimacVpred~=0 && robot.snimacPravo==0 % ak ide doprava,prekazka vpredu , vpravo volno otoc doprava hole
  699. novysmer=1;
  700. elseif robot.smer==1 && robot.snimacLavo~=0 && robot.snimacVpred==0 % ak ide dole,prekazka vlavo, vpred volno pokracuj rovno
  701. novysmer=0;
  702. elseif robot.smer==1 && robot.snimacLavo~=0 && robot.snimacVpred~=0 % ak ide dole,prekazka vlavo a rovno, otoc vpravo
  703. novysmer=1;
  704. elseif robot.smer==1 && robot.snimacLavo==0 && robot.snimacVpred==0 % ak ide dole,prekazka vlavo a rovno nieje, otoc dolava
  705. novysmer=2;
  706. elseif robot.smer==1 && robot.snimacLavo==0 && robot.snimacVpred~=0 && robot.snimacPravo==0 % ak ide dole,prekazka rovno, vlavo a vpravo je volno, otoc doprava
  707. novysmer=1;
  708. elseif robot.smer==3 && robot.snimacLavo~=0 && robot.snimacVpred==0 % ak ide dolava,prekazka vlavo, vpred je volno, chod rovno
  709. novysmer=0;
  710. elseif robot.smer==3 && robot.snimacLavo==0 % ak ide dolava, vlavo je volno, chod dolava
  711. novysmer=2;
  712. elseif robot.smer==2 % ak ide dole, toc dolava
  713. novysmer=2;
  714. elseif robot.smer==4 && robot.snimacVpred==0 && robot.poziciaX>xciel % ak ide doprava, rovno je volno, este nie je v cieli, chod rovno a vypni obchadzanie prekazky
  715. novysmer=0;
  716. prekazka=0;
  717. end
  718.  
  719. elseif prekazka==4 && yciel<21
  720. if robot.smer==4 && robot.snimacVpred~=0 && robot.snimacLavo~=0 && robot.snimacPravo==0 % ak ide doprava,prekazka vpredu a vlavo, vpravo volno otoc doprava dole
  721. novysmer=1;
  722. elseif robot.smer==4 && robot.snimacVpred~=0 && robot.snimacLavo==0 % ak ide doprava,prekazka vpredu , vlavo volno otoc dolava hole
  723. novysmer=2;
  724. elseif robot.smer==2 && robot.snimacPravo~=0 && robot.snimacVpred==0 % ak ide hore,prekazka vpravo, vpred volno pokracuj rovno
  725. novysmer=0;
  726. elseif robot.smer==2 && robot.snimacPravo~=0 && robot.snimacVpred~=0 % ak ide hore,prekazka vpravo a rovno, otoc vlavo
  727. novysmer=2;
  728. elseif robot.smer==2 && robot.snimacPravo==0 && robot.snimacVpred==0 % ak ide hore,prekazka vpravo a rovno nieje, otoc doprava
  729. novysmer=1;
  730. elseif robot.smer==2 && robot.snimacPravo==0 && robot.snimacVpred~=0 && robot.snimacLavo==0 % ak ide hore,prekazka rovno, vlavo a vpravo je volno, otoc dolava
  731. novysmer=2;
  732. elseif robot.smer==3 && robot.snimacPravo~=0 && robot.snimacVpred==0 % ak ide dolava,prekazka vpravo, vpred je volno, chod rovno
  733. novysmer=0;
  734. elseif robot.smer==3 && robot.snimacPravo==0 % ak ide dolava, vpravo je volno, chod doprava
  735. novysmer=1;
  736. elseif robot.smer==1 % ak ide dole, toc dolava
  737. novysmer=1;
  738. elseif robot.smer==4 && robot.snimacVpred==0 && robot.poziciaX>xciel % ak ide doprava, rovno je volno, este nie je v cieli, chod rovno a vypni obchadzanie prekazky
  739. novysmer=0;
  740. prekazka=0;
  741. end
  742.  
  743. end
  744.  
  745. % prekazka hore
  746. if prekazka==1 %&& xciel>20
  747. if robot.smer==1 && robot.snimacVpred~=0 && robot.snimacLavo~=0 && robot.snimacPravo==0 % ak ide doprava,prekazka vpredu a vpravo, vlavo volno otoc dolava hore
  748. novysmer=1;
  749. elseif robot.smer==1 && robot.snimacVpred~=0 && robot.snimacLavo==0 % ak ide doprava,prekazka vpredu , vpravo volno otoc doprava hole
  750. novysmer=2;
  751. elseif robot.smer==4 && robot.snimacPravo~=0 && robot.snimacVpred==0 % ak ide dole,prekazka vlavo, vpred volno pokracuj rovno
  752. novysmer=0;
  753. elseif robot.smer==4 && robot.snimacPravo~=0 && robot.snimacVpred~=0 % ak ide dole,prekazka vlavo a rovno, otoc vpravo
  754. novysmer=2;
  755. elseif robot.smer==4 && robot.snimacPravo==0 && robot.snimacVpred==0 % ak ide dole,prekazka vlavo a rovno nieje, otoc dolava
  756. novysmer=1;
  757. elseif robot.smer==4 && robot.snimacLavo==0 && robot.snimacVpred~=0 && robot.snimacPravo==0 % ak ide dole,prekazka rovno, vlavo a vpravo je volno, otoc doprava
  758. novysmer=2;
  759. elseif robot.smer==2 && robot.snimacPravo~=0 && robot.snimacVpred==0 % ak ide dolava,prekazka vlavo, vpred je volno, chod rovno
  760. novysmer=0;
  761. elseif robot.smer==2 && robot.snimacPravo==0 % ak ide dolava, vlavo je volno, chod dolava
  762. novysmer=1;
  763. elseif robot.smer==3 % ak ide dole, toc dolava
  764. novysmer=1;
  765. elseif robot.smer==1 && robot.snimacVpred==0 && robot.poziciaY>yciel % ak ide doprava, rovno je volno, este nie je v cieli, chod rovno a vypni obchadzanie prekazky
  766. novysmer=0;
  767. prekazka=0;
  768. end
  769.  
  770. end
  771.  
  772. % prekazka dole
  773. if prekazka==2 %&& xciel>20
  774. if robot.smer==2 && robot.snimacVpred~=0 && robot.snimacPravo~=0 && robot.snimacLavo==0 % ak ide doprava,prekazka vpredu a vpravo, vlavo volno otoc dolava hore
  775. novysmer=2;
  776. elseif robot.smer==2 && robot.snimacVpred~=0 && robot.snimacPravo==0 % ak ide doprava,prekazka vpredu , vpravo volno otoc doprava hole
  777. novysmer=1;
  778. elseif robot.smer==4 && robot.snimacLavo~=0 && robot.snimacVpred==0 % ak ide dole,prekazka vlavo, vpred volno pokracuj rovno
  779. novysmer=0;
  780. elseif robot.smer==4 && robot.snimacLavo~=0 && robot.snimacVpred~=0 % ak ide dole,prekazka vlavo a rovno, otoc vpravo
  781. novysmer=1;
  782. elseif robot.smer==4 && robot.snimacLavo==0 && robot.snimacVpred==0 % ak ide dole,prekazka vlavo a rovno nieje, otoc dolava
  783. novysmer=2;
  784. elseif robot.smer==4 && robot.snimacLavo==0 && robot.snimacVpred~=0 && robot.snimacPravo==0 % ak ide dole,prekazka rovno, vlavo a vpravo je volno, otoc doprava
  785. novysmer=1;
  786. elseif robot.smer==1 && robot.snimacLavo~=0 && robot.snimacVpred==0 % ak ide dolava,prekazka vlavo, vpred je volno, chod rovno
  787. novysmer=0;
  788. elseif robot.smer==1 && robot.snimacLavo==0 % ak ide dolava, vlavo je volno, chod dolava
  789. novysmer=2;
  790. elseif robot.smer==3 % ak ide dole, toc dolava
  791. novysmer=2;
  792. elseif robot.smer==2 && robot.snimacVpred==0 && robot.poziciaY<yciel % ak ide doprava, rovno je volno, este nie je v cieli, chod rovno a vypni obchadzanie prekazky
  793. novysmer=0;
  794. prekazka=0;
  795. end
  796.  
  797. end
  798.  
  799.  
  800. end
  801.  
  802. end
  803.  
  804. % funkcia na nahodne prehladavanie
  805. % novysmer , 0-rovno, 1-vpravo, 2-vlavo, -1 - bez zmeny
  806. % kki, kko - pocitadlo
  807. % nahodai, nahodao - kedy ma byt vykonana zmena smeru
  808. function [novysmer,kko,nahodao]=nahodnehladanie(robot,kki,nahodai)
  809. if robot.snimacPravo==2 % ak je vpravo kocka, chod doprava
  810. novysmer=1;
  811. elseif robot.snimacLavo==2 % ak je vlavo kocka, chod dolava
  812. novysmer=2;
  813. elseif robot.snimacVpred==0 || robot.snimacVpred==2 % ak je rovno kocka alebo volno chod rovno
  814. novysmer=0;
  815. elseif robot.snimacVpred~=0 && robot.snimacPravo==0 && robot.snimacLavo==0 % ak je rovno prekazka, vpravo aj vlavo volno, vyber si vlavo alebo vpravo
  816. if rand<0.4
  817. novysmer=2;
  818. else
  819. novysmer=1;
  820. end
  821. elseif robot.snimacVpred~=0 && robot.snimacPravo==0 % ak je rovno prekazka, vpravo volno, chod vpravo
  822. novysmer=1;
  823. elseif robot.snimacVpred~=0 && robot.snimacLavo==0
  824. novysmer=2;
  825. else
  826. novysmer=-1;
  827. end
  828.  
  829. % nahodna zmena smeru
  830. if kki==nahodai
  831. % ak ide rovno, zmen smer
  832. if novysmer==0 && robot.snimacVpred~=2
  833. % ak je vlavo volno, chod vlavo
  834. if robot.snimacLavo==0
  835. novysmer=2;
  836. elseif robot.snimacPravo==0 % ak je vpravo volno, chod vpravo
  837. novysmer=1;
  838. end
  839. end
  840. % nahodne generovanie casu na zmenu smeru
  841. nahodai=fix(robot.param2*rand+robot.param1);
  842. kko=0;
  843. else
  844. kko=kki+1;
  845. end
  846. nahodao=nahodai;
  847. end
  848.  
  849. % funkcia na nastavenie smeru cesty, podla toho ktorym smerom je otoceny
  850. % vypocita poziciu robota a ktorym smerom sa ma otocit
  851. function [posX,posY,otoc]=nastavsmercesty(robot)
  852. stareX=robot.poziciaX;
  853. stareY=robot.poziciaY;
  854. posX=robot.poziciaX;
  855. posY=robot.poziciaY;
  856. otoc=0;
  857. % nastavenie smeru cesty
  858. if robot.pohyb==1
  859. if robot.otocenie==0 && robot.smer==1 % hore
  860. posX=stareX;
  861. posY=stareY-1;
  862. elseif robot.otocenie==0 && robot.smer==2 % dole
  863. posX=stareX;
  864. posY=stareY+1;
  865. elseif robot.otocenie==0 && robot.smer==3 % vpravo
  866. posX=stareX+1;
  867. posY=stareY;
  868. elseif robot.otocenie==0 && robot.smer==4 % vlavo
  869. posX=stareX-1;
  870. posY=stareY;
  871.  
  872. elseif robot.otocenie==1 && robot.smer==1 % otocenie vpravo
  873. posX=stareX+1;
  874. posY=stareY;
  875. otoc=3;
  876. elseif robot.otocenie==1 && robot.smer==3 % otocenie dole
  877. posX=stareX;
  878. posY=stareY+1;
  879. otoc=2;
  880. elseif robot.otocenie==1 && robot.smer==2 % otocenie vlavo
  881. posX=stareX-1;
  882. posY=stareY;
  883. otoc=4;
  884. elseif robot.otocenie==1 && robot.smer==4 % otocenie hore
  885. posX=stareX;
  886. posY=stareY-1;
  887. otoc=1;
  888.  
  889. elseif robot.otocenie==2 && robot.smer==1 % otocenie vlavo
  890. posX=stareX-1;
  891. posY=stareY;
  892. otoc=4;
  893. elseif robot.otocenie==2 && robot.smer==3 % otocenie hore
  894. posX=stareX;
  895. posY=stareY-1;
  896. otoc=1;
  897. elseif robot.otocenie==2 && robot.smer==2 % otocenie vpravo
  898. posX=stareX+1;
  899. posY=stareY;
  900. otoc=3;
  901. elseif robot.otocenie==2 && robot.smer==4 % otocenie dole
  902. posX=stareX;
  903. posY=stareY+1;
  904. otoc=2;
  905. end
  906. end
  907. if posY<1
  908. posY=1;
  909. end
  910. if posX<1
  911. posX=1;
  912. end
  913. if posX>40
  914. posX=40;
  915. end
  916. if posY>40
  917. posY=40;
  918. end
  919.  
  920. end
  921.  
  922. % nastavenie stavu snimacov, podla pixelov v mape
  923. function [vs,ps,hs]=nastavsnimace(robot,horepole,dolepole,vpravopole,vlavopole)
  924.  
  925. % smer hore
  926. if robot.smer==1
  927. if horepole==2
  928. hs=1;
  929. elseif horepole==5 || horepole==6
  930. hs=2;
  931. elseif horepole==3 || horepole==4
  932. hs=3;
  933. else
  934. hs=0;
  935. end
  936. if vlavopole==2
  937. vs=1;
  938. elseif vlavopole==5 || vlavopole==6
  939. vs=2;
  940. elseif vlavopole==3 || vlavopole==4
  941. vs=3;
  942. else
  943. vs=0;
  944. end
  945. if vpravopole==2
  946. ps=1;
  947. elseif vpravopole==5 || vpravopole==6
  948. ps=2;
  949. elseif vpravopole==3 || vpravopole==4
  950. ps=3;
  951. else
  952. ps=0;
  953. end
  954.  
  955. end
  956. % smer dole
  957. if robot.smer==2
  958. if dolepole==2
  959. hs=1;
  960. elseif dolepole==5 || dolepole==6
  961. hs=2;
  962. elseif dolepole==3 || dolepole==4
  963. hs=3;
  964. else
  965. hs=0;
  966. end
  967. if vpravopole==2
  968. vs=1;
  969. elseif vpravopole==5 || vpravopole==6
  970. vs=2;
  971. elseif vpravopole==3 || vpravopole==4
  972. vs=3;
  973. else
  974. vs=0;
  975. end
  976. if vlavopole==2
  977. ps=1;
  978. elseif vlavopole==5 || vlavopole==6
  979. ps=2;
  980. elseif vlavopole==3 || vlavopole==4
  981. ps=3;
  982. else
  983. ps=0;
  984. end
  985.  
  986. end
  987. % smer vpravo
  988. if robot.smer==3
  989. if vpravopole==2
  990. hs=1;
  991. elseif vpravopole==5 || vpravopole==6
  992. hs=2;
  993. elseif vpravopole==3 || vpravopole==4
  994. hs=3;
  995. else
  996. hs=0;
  997. end
  998. if horepole==2
  999. vs=1;
  1000. elseif horepole==5 || horepole==6
  1001. vs=2;
  1002. elseif horepole==3 || horepole==4
  1003. vs=3;
  1004. else
  1005. vs=0;
  1006. end
  1007. if dolepole==2
  1008. ps=1;
  1009. elseif dolepole==5 || dolepole==6
  1010. ps=2;
  1011. elseif dolepole==3 || dolepole==4
  1012. ps=3;
  1013. else
  1014. ps=0;
  1015. end
  1016.  
  1017. end
  1018. % smer vlavo
  1019. if robot.smer==4
  1020. if vlavopole==2
  1021. hs=1;
  1022. elseif vlavopole==5 || vlavopole==6
  1023. hs=2;
  1024. elseif vlavopole==3 || vlavopole==4
  1025. hs=3;
  1026. else
  1027. hs=0;
  1028. end
  1029. if dolepole==2
  1030. vs=1;
  1031. elseif dolepole==5 || dolepole==6
  1032. vs=2;
  1033. elseif dolepole==3 || dolepole==4
  1034. vs=3;
  1035. else
  1036. vs=0;
  1037. end
  1038. if horepole==2
  1039. ps=1;
  1040. elseif horepole==5 || horepole==6
  1041. ps=2;
  1042. elseif horepole==3 || horepole==4
  1043. ps=3;
  1044. else
  1045. ps=0;
  1046. end
  1047.  
  1048. end
  1049.  
  1050. end
  1051.  
  1052. % funkcia na prehladanie okolia robota radarom
  1053. % vrati pozicie kociek [Y X] - [riadok stlpec] v mape
  1054. function [novapozkocky]=najdikocku(robot,celamapa,dosahsenzora)
  1055. % def. rozsahu senzora
  1056. rmin=robot.poziciaY-dosahsenzora+1;
  1057. rmax=robot.poziciaY+dosahsenzora+1;
  1058. smin=robot.poziciaX-dosahsenzora+1;
  1059. smax=robot.poziciaX+dosahsenzora+1;
  1060.  
  1061. % ohranicenie mapy
  1062. if rmin<2
  1063. rmin=2;
  1064. end
  1065. if smin<1
  1066. smin=1;
  1067. end
  1068. if rmax>41
  1069. rmax=41;
  1070. end
  1071. if smax>41
  1072. smax=41;
  1073. end
  1074.  
  1075. % ohranicenie prekazkov, radar cez prekaz nevidi
  1076. ss=robot.poziciaX+1;
  1077. for rr=rmin:robot.poziciaY+1,
  1078. if celamapa(rr,ss)==2
  1079. rmin=rr;
  1080. break;
  1081. end
  1082. end
  1083. rr=robot.poziciaY+1;
  1084. for ss=smin:robot.poziciaX+1,
  1085. if celamapa(rr,ss)==2
  1086. smin=ss;
  1087. break;
  1088. end
  1089. end
  1090. ss=robot.poziciaX+1;
  1091. for rr=robot.poziciaY+1:rmax,
  1092. if celamapa(rr,ss)==2
  1093. rmax=rr;
  1094. break;
  1095. end
  1096. end
  1097. rr=robot.poziciaY+1;
  1098. for ss=robot.poziciaX+1:smax,
  1099. if celamapa(rr,ss)==2
  1100. smax=ss;
  1101. break;
  1102. end
  1103. end
  1104. novapozkocky=[];
  1105.  
  1106. % hladanie kocky v ohranicenom priestore
  1107. for rr=rmin:rmax,
  1108. for ss=smin:smax,
  1109. if celamapa(rr,ss)==5
  1110. novapozkocky=[novapozkocky;rr-1 ss-1 1];
  1111. elseif celamapa(rr,ss)==6
  1112. novapozkocky=[novapozkocky;rr-1 ss-1 2];
  1113. end
  1114. end
  1115. end
  1116.  
  1117. end
RAW Paste Data