API tools faq
paste
Advertisement
Guest User

RobociChoboci

a guest
Apr 25th, 2019
86
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
MatLab 38.94 KB | None | 0 0
raw download clone embed print report
  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. %             for i=1:size(zkvystup)
  358. %                 pozicia_kocky=[robot.poziciaY+1, robot.poziciaX+1, 1];
  359. %                 [tmp, indx] = ismember(pozicia_kocky, zkvystup(i,:));
  360. %                 if(sum(tmp)==3)
  361. %                     zkvystup(indx,:)=[];
  362. %                 end
  363. %             end
  364.            
  365.             % doplnit rozpoznavanie pomocou NS
  366.             % [typkocky,paramkocky]=citajparamkocky(mapab,robot.poziciaX,robot.poziciaY);
  367.            
  368.         end
  369.  
  370.         % ak nesie kocku
  371.         if robot.kocka==1
  372.             [najdenepozkocky]=najdikocku(robot,mapab,dosahradaru);
  373.             if (isempty(najdenepozkocky) == 0)
  374.                 for i=1:size(najdenepozkocky)
  375.                     if(sum(ismember(najdenepozkocky(i,:), zkvystup)) == 0) % < 3
  376.                         zkvystup = [zkvystup; najdenepozkocky(i,:)];
  377.                     end
  378.                 end
  379.             end
  380.             % ak priniesol kocku do ciela
  381.             if robot.typkocka==1
  382.                 if robot.poziciaX==40 && robot.poziciaY==40
  383.                     polozilkocku=1;
  384.                     if (isempty(zkvystup)==0)
  385.                         robot.cielX=zkvystup(1,2);
  386.                         robot.cielY=zkvystup(1,1);
  387.                         zkvystup(1,:) = [];
  388.                         robot.kocka = 2;
  389.                     else
  390.                         robot.kocka = 0;
  391.                     end
  392.                 end
  393.             elseif robot.typkocka==2
  394.                 if robot.poziciaX==40 && robot.poziciaY==1
  395.                     polozilkocku=1;
  396.                     if (isempty(zkvystup)==0)
  397.                         robot.cielX=zkvystup(1,2);
  398.                         robot.cielY=zkvystup(1,1);
  399.                         zkvystup(1,:) = [];
  400.                         robot.kocka = 2;
  401.                     else
  402.                         robot.kocka = 0;
  403.                     end
  404.                 end                
  405.             end
  406.         end
  407.  
  408.         % ak nema kocku prehlada priestor ultrazvukom
  409.         if robot.kocka==0
  410.             [najdenepozkocky]=najdikocku(robot,mapab,dosahradaru);
  411.             % ak najde kocku nastavi ciel na jej poziciu
  412.             if isempty(najdenepozkocky)==0
  413.                 robot.cielX=najdenepozkocky(1,2);
  414.                 robot.cielY=najdenepozkocky(1,1);
  415.                 robot.kocka=2;
  416.             end
  417.         end
  418. %         zkvystup
  419.         robotupdate=robot;
  420. end
  421.  
  422.  
  423. % funkcia na budovanie mapy
  424. function odkrytiemapy2=budujmapu(odkrytiemapy1,robot)
  425.     odkrytiemapy2=odkrytiemapy1;
  426.    
  427.     % doprogramovavat
  428.  
  429.  
  430. end
  431.  
  432.  
  433. % funkcia, ktora vrati parametre objektu a jeho typ
  434. function [typkocky,paramkocky]=citajparamkocky(mapab,posX,posY)
  435.  
  436. 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; ...
  437. 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];
  438.  
  439. paramkocky=zeros(1,3);
  440.  
  441.             % doplnit rozpoznavanie pomocou NS
  442.             if mapab(posY+1,posX+1)==6
  443.                typkocky=2;
  444.                druh=fix(3*rand(1))+3;
  445.             else
  446.                typkocky=1;
  447.                druh=fix(3*rand(1))+1;
  448.  
  449.             end
  450.            
  451.             for i=1:3
  452.                   paramkocky(i)=pd(druh,2*(i-1)+1)+2*pd(druh,2*(i-1)+2)*rand(1)-pd(druh,2*(i-1)+2);
  453.             end
  454. end
  455.  
  456.  
  457.  
  458. % funkcia pre riadenie robota do ciela
  459. % novysmer , 0-rovno, 1-vpravo, 2-vlavo, -1 - bez zmeny
  460. % prekazka - 0-nie je, 1-hore, 2-dole, 3-doprava, 4-dolava,
  461. % ak su zaporne hodnoty, prekazka je v cieli
  462. function [novysmer,prekazka]=chodnaciel(robot)
  463.    
  464.     % suradnice ciela robota
  465.     xciel=robot.cielX;
  466.     yciel=robot.cielY;
  467.    
  468.     % kde sa nachadza ciel od aktualnej pozicie robota, vlavo, vpravo, hore
  469.     % dole
  470.     if (xciel-robot.poziciaX)>0
  471.         idevpravo=1;
  472.     else
  473.         idevpravo=0;
  474.     end
  475.     if (xciel-robot.poziciaX)<0
  476.         idevlavo=1;
  477.     else
  478.         idevlavo=0;
  479.     end
  480.     if (yciel-robot.poziciaY)>0
  481.         idedole=1;
  482.     else
  483.         idedole=0;
  484.     end
  485.     if (yciel-robot.poziciaY)<0
  486.         idehore=1;
  487.     else
  488.         idehore=0;
  489.     end
  490.  
  491.     novysmer=-1;
  492.     prekazka=robot.prekazka;
  493.     % ak nenarazil na prekazku
  494.     if prekazka==0
  495.        
  496.         % ma ist smerom dole
  497.         if idedole==1
  498.            if robot.smer==2 && robot.snimacVpred==0
  499.                 novysmer=0;
  500.            elseif robot.smer==3 && robot.snimacPravo==0    %ide vpravo, vpravo volno, otoc dole
  501.                 novysmer=1;
  502.            elseif robot.smer==4 && robot.snimacLavo==0
  503.                 novysmer=2;
  504.            elseif robot.smer==1
  505.                 if idevpravo==1 && robot.snimacPravo==0
  506.                     novysmer=1;
  507.                 elseif idevlavo==1 && robot.snimacLavo==0
  508.                     novysmer=2;
  509.                 end
  510.            end
  511.         end
  512.    
  513.     if novysmer==-1
  514.         % ma ist smerom hore
  515.         if idehore==1
  516.            if robot.smer==1 && robot.snimacVpred==0
  517.                 novysmer=0;
  518.            elseif robot.smer==3 && robot.snimacLavo==0    %ide vpravo, vpravo volno, otoc dole
  519.                 novysmer=2;
  520.            elseif robot.smer==4 && robot.snimacPravo==0
  521.                 novysmer=1;
  522.            elseif robot.smer==2
  523.                 if idevpravo==1 && robot.snimacLavo==0
  524.                     novysmer=2;
  525.                 elseif idevlavo==1 && robot.snimacPravo==0
  526.                     novysmer=1;
  527.                 end
  528.            end            
  529.         end
  530.     end
  531.                    
  532.     if novysmer==-1
  533.         % ma ist smerom vpravo
  534.         if idevpravo==1
  535.            if robot.smer==1 && robot.snimacPravo==0
  536.                 novysmer=1;
  537.            elseif robot.smer==3 && robot.snimacVpred==0    %ide vpravo, vpravo volno, otoc dole
  538.                 novysmer=0;
  539.            elseif robot.smer==2 && robot.snimacLavo==0
  540.                 novysmer=2;
  541.            elseif robot.smer==4
  542.                 if idehore==1 && robot.snimacPravo==0
  543.                     novysmer=1;
  544.                 elseif idedole==1 && robot.snimacLavo==0
  545.                     novysmer=2;
  546.                 end
  547.            end            
  548.         end
  549.     end
  550.                
  551.     if novysmer==-1
  552.         % ma ist smerom vlavo
  553.         if idevlavo==1
  554.            if robot.smer==1 && robot.snimacLavo==0
  555.                 novysmer=2;
  556.            elseif robot.smer==4 && robot.snimacVpred==0    %ide vpravo, vpravo volno, otoc dole
  557.                 novysmer=0;
  558.            elseif robot.smer==2 && robot.snimacPravo==0
  559.                 novysmer=1;
  560.            elseif robot.smer==3
  561.                 if idehore==1 && robot.snimacLavo==0
  562.                     novysmer=2;
  563.                 elseif idedole==1 && robot.snimacPravo==0
  564.                     novysmer=1;
  565.                 end
  566.            end            
  567.         end
  568.     end
  569.    
  570.     % nie je mozny ziadny smer
  571.     if novysmer==-1
  572.         % ak nie je v cieli, tak nastav prekazku
  573.         if xciel~=robot.poziciaX
  574.             if idevpravo==1 && (xciel-robot.poziciaX)==1
  575.                 prekazka=-3;
  576.             elseif idevpravo==1
  577.                 prekazka=3;
  578.             elseif idevlavo==1 && (robot.poziciaX-xciel)==1
  579.                 prekazka=-4;                
  580.             elseif idevlavo==1
  581.                 prekazka=4;
  582.             end  
  583.         elseif yciel~=robot.poziciaY
  584.             if idehore==1 && (robot.poziciaY-yciel)==1
  585.                 prekazka=-1;
  586.             elseif idehore==1
  587.                 prekazka=1;
  588.             elseif idedole==1 && (yciel-robot.poziciaY)==1
  589.                 prekazka=-2;
  590.             elseif idedole==1
  591.                 prekazka=2;
  592.             end                      
  593.         end        
  594.     end
  595.    
  596.     % ak je prekazka, potom jej obchadzanie
  597.     else
  598.        
  599.         % prekazka vpravo
  600.         if prekazka==3 && yciel<21
  601.            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
  602.                 novysmer=2;
  603.            elseif robot.smer==3 && robot.snimacVpred~=0 && robot.snimacPravo==0 % ak ide doprava,prekazka vpredu , vpravo volno otoc doprava hole
  604.                 novysmer=1;                
  605.            elseif robot.smer==2 && robot.snimacLavo~=0 && robot.snimacVpred==0 % ak ide dole,prekazka vlavo, vpred volno pokracuj rovno
  606.                 novysmer=0;
  607.            elseif robot.smer==2 && robot.snimacLavo~=0 && robot.snimacVpred~=0 % ak ide dole,prekazka vlavo a rovno, otoc vpravo
  608.                 novysmer=1;                
  609.            elseif robot.smer==2 && robot.snimacLavo==0 && robot.snimacVpred==0  % ak ide dole,prekazka vlavo a rovno nieje, otoc dolava
  610.                 novysmer=2;
  611.            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
  612.                 novysmer=1;              
  613.            elseif robot.smer==4 && robot.snimacLavo~=0 && robot.snimacVpred==0 % ak ide dolava,prekazka vlavo, vpred je volno, chod rovno
  614.                 novysmer=0;
  615.            elseif robot.smer==4 && robot.snimacLavo==0 % ak ide dolava, vlavo je volno, chod dolava
  616.                 novysmer=2;        
  617.            elseif robot.smer==1 % ak ide dole, toc dolava
  618.                 novysmer=2;
  619.            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  
  620.                 novysmer=0;
  621.                 prekazka=0;
  622.            end              
  623.            
  624.         elseif prekazka==3 && yciel>20
  625.            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
  626.                 novysmer=1;
  627.            elseif robot.smer==3 && robot.snimacVpred~=0 && robot.snimacLavo==0 % ak ide doprava,prekazka vpredu , vlavo volno otoc dolava hole
  628.                 novysmer=2;                
  629.            elseif robot.smer==1 && robot.snimacPravo~=0 && robot.snimacVpred==0 % ak ide hore,prekazka vpravo, vpred volno pokracuj rovno
  630.                 novysmer=0;
  631.            elseif robot.smer==1 && robot.snimacPravo~=0 && robot.snimacVpred~=0 % ak ide hore,prekazka vpravo a rovno, otoc vlavo
  632.                 novysmer=2;                
  633.            elseif robot.smer==1 && robot.snimacPravo==0 && robot.snimacVpred==0  % ak ide hore,prekazka vpravo a rovno nieje, otoc doprava
  634.                 novysmer=1;
  635.            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
  636.                 novysmer=2;              
  637.            elseif robot.smer==4 && robot.snimacPravo~=0 && robot.snimacVpred==0 % ak ide dolava,prekazka vpravo, vpred je volno, chod rovno
  638.                 novysmer=0;
  639.            elseif robot.smer==4 && robot.snimacPravo==0 % ak ide dolava, vpravo je volno, chod doprava
  640.                 novysmer=1;        
  641.            elseif robot.smer==2 % ak ide dole, toc dolava
  642.                 novysmer=1;
  643.            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  
  644.                 novysmer=0;
  645.                 prekazka=0;
  646.            end              
  647.         end
  648.        
  649.         % prekazka vlavo
  650.         if prekazka==4 && yciel>20
  651.            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
  652.                 novysmer=2;
  653.            elseif robot.smer==4 && robot.snimacVpred~=0 && robot.snimacPravo==0 % ak ide doprava,prekazka vpredu , vpravo volno otoc doprava hole
  654.                 novysmer=1;                
  655.            elseif robot.smer==1 && robot.snimacLavo~=0 && robot.snimacVpred==0 % ak ide dole,prekazka vlavo, vpred volno pokracuj rovno
  656.                 novysmer=0;
  657.            elseif robot.smer==1 && robot.snimacLavo~=0 && robot.snimacVpred~=0 % ak ide dole,prekazka vlavo a rovno, otoc vpravo
  658.                 novysmer=1;                
  659.            elseif robot.smer==1 && robot.snimacLavo==0 && robot.snimacVpred==0  % ak ide dole,prekazka vlavo a rovno nieje, otoc dolava
  660.                 novysmer=2;
  661.            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
  662.                 novysmer=1;              
  663.            elseif robot.smer==3 && robot.snimacLavo~=0 && robot.snimacVpred==0 % ak ide dolava,prekazka vlavo, vpred je volno, chod rovno
  664.                 novysmer=0;
  665.            elseif robot.smer==3 && robot.snimacLavo==0 % ak ide dolava, vlavo je volno, chod dolava
  666.                 novysmer=2;        
  667.            elseif robot.smer==2 % ak ide dole, toc dolava
  668.                 novysmer=2;
  669.            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  
  670.                 novysmer=0;
  671.                 prekazka=0;
  672.            end              
  673.            
  674.         elseif prekazka==4 && yciel<21
  675.            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
  676.                 novysmer=1;
  677.            elseif robot.smer==4 && robot.snimacVpred~=0 && robot.snimacLavo==0 % ak ide doprava,prekazka vpredu , vlavo volno otoc dolava hole
  678.                 novysmer=2;                
  679.            elseif robot.smer==2 && robot.snimacPravo~=0 && robot.snimacVpred==0 % ak ide hore,prekazka vpravo, vpred volno pokracuj rovno
  680.                 novysmer=0;
  681.            elseif robot.smer==2 && robot.snimacPravo~=0 && robot.snimacVpred~=0 % ak ide hore,prekazka vpravo a rovno, otoc vlavo
  682.                 novysmer=2;                
  683.            elseif robot.smer==2 && robot.snimacPravo==0 && robot.snimacVpred==0  % ak ide hore,prekazka vpravo a rovno nieje, otoc doprava
  684.                 novysmer=1;
  685.            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
  686.                 novysmer=2;              
  687.            elseif robot.smer==3 && robot.snimacPravo~=0 && robot.snimacVpred==0 % ak ide dolava,prekazka vpravo, vpred je volno, chod rovno
  688.                 novysmer=0;
  689.            elseif robot.smer==3 && robot.snimacPravo==0 % ak ide dolava, vpravo je volno, chod doprava
  690.                 novysmer=1;        
  691.            elseif robot.smer==1 % ak ide dole, toc dolava
  692.                 novysmer=1;
  693.            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  
  694.                 novysmer=0;
  695.                 prekazka=0;
  696.            end              
  697.            
  698.         end
  699.  
  700.         % prekazka hore
  701.         if prekazka==1      %&& xciel>20
  702.            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
  703.                 novysmer=1;
  704.            elseif robot.smer==1 && robot.snimacVpred~=0 && robot.snimacLavo==0 % ak ide doprava,prekazka vpredu , vpravo volno otoc doprava hole
  705.                 novysmer=2;                
  706.            elseif robot.smer==4 && robot.snimacPravo~=0 && robot.snimacVpred==0 % ak ide dole,prekazka vlavo, vpred volno pokracuj rovno
  707.                 novysmer=0;
  708.            elseif robot.smer==4 && robot.snimacPravo~=0 && robot.snimacVpred~=0 % ak ide dole,prekazka vlavo a rovno, otoc vpravo
  709.                 novysmer=2;                
  710.            elseif robot.smer==4 && robot.snimacPravo==0 && robot.snimacVpred==0  % ak ide dole,prekazka vlavo a rovno nieje, otoc dolava
  711.                 novysmer=1;
  712.            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
  713.                 novysmer=2;              
  714.            elseif robot.smer==2 && robot.snimacPravo~=0 && robot.snimacVpred==0 % ak ide dolava,prekazka vlavo, vpred je volno, chod rovno
  715.                 novysmer=0;
  716.            elseif robot.smer==2 && robot.snimacPravo==0 % ak ide dolava, vlavo je volno, chod dolava
  717.                 novysmer=1;        
  718.            elseif robot.smer==3 % ak ide dole, toc dolava
  719.                 novysmer=1;
  720.            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  
  721.                 novysmer=0;
  722.                 prekazka=0;
  723.            end              
  724.                      
  725.         end
  726.  
  727.         % prekazka dole
  728.         if prekazka==2  %&& xciel>20
  729.            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
  730.                 novysmer=2;
  731.            elseif robot.smer==2 && robot.snimacVpred~=0 && robot.snimacPravo==0 % ak ide doprava,prekazka vpredu , vpravo volno otoc doprava hole
  732.                 novysmer=1;                
  733.            elseif robot.smer==4 && robot.snimacLavo~=0 && robot.snimacVpred==0 % ak ide dole,prekazka vlavo, vpred volno pokracuj rovno
  734.                 novysmer=0;
  735.            elseif robot.smer==4 && robot.snimacLavo~=0 && robot.snimacVpred~=0 % ak ide dole,prekazka vlavo a rovno, otoc vpravo
  736.                 novysmer=1;                
  737.            elseif robot.smer==4 && robot.snimacLavo==0 && robot.snimacVpred==0  % ak ide dole,prekazka vlavo a rovno nieje, otoc dolava
  738.                 novysmer=2;
  739.            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
  740.                 novysmer=1;              
  741.            elseif robot.smer==1 && robot.snimacLavo~=0 && robot.snimacVpred==0 % ak ide dolava,prekazka vlavo, vpred je volno, chod rovno
  742.                 novysmer=0;
  743.            elseif robot.smer==1 && robot.snimacLavo==0 % ak ide dolava, vlavo je volno, chod dolava
  744.                 novysmer=2;        
  745.            elseif robot.smer==3 % ak ide dole, toc dolava
  746.                 novysmer=2;
  747.            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  
  748.                 novysmer=0;
  749.                 prekazka=0;
  750.            end              
  751.                      
  752.         end        
  753.        
  754.        
  755.     end
  756.                
  757. end
  758.  
  759. % funkcia na nahodne prehladavanie
  760. % novysmer , 0-rovno, 1-vpravo, 2-vlavo, -1 - bez zmeny
  761. % kki, kko - pocitadlo
  762. % nahodai, nahodao - kedy ma byt vykonana zmena smeru
  763. function [novysmer,kko,nahodao]=nahodnehladanie(robot,kki,nahodai)
  764.         if robot.snimacPravo==2  % ak je vpravo kocka, chod doprava
  765.            novysmer=1;
  766.         elseif robot.snimacLavo==2  % ak je vlavo kocka, chod dolava
  767.            novysmer=2;
  768.         elseif robot.snimacVpred==0 || robot.snimacVpred==2 % ak je rovno kocka alebo volno chod rovno
  769.            novysmer=0;
  770.         elseif robot.snimacVpred~=0 && robot.snimacPravo==0 && robot.snimacLavo==0 % ak je rovno prekazka, vpravo aj vlavo volno, vyber si vlavo alebo vpravo
  771.            if rand<0.4
  772.                novysmer=2;
  773.            else
  774.                novysmer=1;
  775.            end
  776.         elseif robot.snimacVpred~=0 && robot.snimacPravo==0   % ak je rovno prekazka, vpravo  volno, chod vpravo
  777.            novysmer=1;
  778.         elseif robot.snimacVpred~=0 && robot.snimacLavo==0
  779.            novysmer=2;
  780.         else
  781.             novysmer=-1;
  782.         end
  783.        
  784.         % nahodna zmena smeru
  785.         if kki==nahodai
  786.            % ak ide rovno, zmen smer
  787.            if novysmer==0 && robot.snimacVpred~=2
  788.                % ak je vlavo volno, chod vlavo
  789.                if robot.snimacLavo==0
  790.                   novysmer=2;
  791.                elseif robot.snimacPravo==0  % ak je vpravo volno, chod vpravo
  792.                   novysmer=1;
  793.                end
  794.            end
  795.            % nahodne generovanie casu na zmenu smeru
  796.            nahodai=fix(robot.param2*rand+robot.param1);
  797.            kko=0;
  798.         else
  799.             kko=kki+1;
  800.         end
  801.         nahodao=nahodai;    
  802. end
  803.  
  804. % funkcia na nastavenie smeru cesty, podla toho ktorym smerom je otoceny
  805. % vypocita poziciu robota a ktorym smerom sa ma otocit
  806. function [posX,posY,otoc]=nastavsmercesty(robot)
  807.     stareX=robot.poziciaX;
  808.     stareY=robot.poziciaY;
  809.     posX=robot.poziciaX;
  810.     posY=robot.poziciaY;
  811.     otoc=0;
  812.     % nastavenie smeru cesty
  813.     if robot.pohyb==1
  814.         if robot.otocenie==0 && robot.smer==1 % hore
  815.             posX=stareX;
  816.             posY=stareY-1;
  817.         elseif robot.otocenie==0 && robot.smer==2 % dole
  818.             posX=stareX;
  819.             posY=stareY+1;
  820.         elseif robot.otocenie==0 && robot.smer==3 % vpravo
  821.             posX=stareX+1;
  822.             posY=stareY;
  823.         elseif robot.otocenie==0 && robot.smer==4 % vlavo
  824.             posX=stareX-1;
  825.             posY=stareY;
  826.            
  827.         elseif robot.otocenie==1 && robot.smer==1 % otocenie vpravo
  828.             posX=stareX+1;
  829.             posY=stareY;
  830.             otoc=3;
  831.          elseif robot.otocenie==1 && robot.smer==3 % otocenie dole
  832.             posX=stareX;
  833.             posY=stareY+1;
  834.             otoc=2;
  835.          elseif robot.otocenie==1 && robot.smer==2 % otocenie vlavo
  836.             posX=stareX-1;
  837.             posY=stareY;
  838.             otoc=4;
  839.          elseif robot.otocenie==1 && robot.smer==4 % otocenie hore
  840.             posX=stareX;
  841.             posY=stareY-1;
  842.             otoc=1;
  843.            
  844.         elseif robot.otocenie==2 && robot.smer==1 % otocenie vlavo
  845.             posX=stareX-1;
  846.             posY=stareY;
  847.             otoc=4;
  848.          elseif robot.otocenie==2 && robot.smer==3 % otocenie hore
  849.             posX=stareX;
  850.             posY=stareY-1;
  851.             otoc=1;
  852.          elseif robot.otocenie==2 && robot.smer==2 % otocenie vpravo
  853.             posX=stareX+1;
  854.             posY=stareY;
  855.             otoc=3;
  856.          elseif robot.otocenie==2 && robot.smer==4 % otocenie dole
  857.             posX=stareX;
  858.             posY=stareY+1;
  859.             otoc=2;
  860.         end
  861.     end
  862.     if posY<1
  863.         posY=1;
  864.     end
  865.     if posX<1
  866.         posX=1;
  867.     end    
  868.     if posX>40
  869.         posX=40;
  870.     end    
  871.     if posY>40
  872.         posY=40;
  873.     end
  874.  
  875. end
  876.  
  877. % nastavenie stavu snimacov, podla pixelov v mape
  878. function [vs,ps,hs]=nastavsnimace(robot,horepole,dolepole,vpravopole,vlavopole)
  879.    
  880.     % smer hore
  881.     if robot.smer==1
  882.         if horepole==2
  883.             hs=1;
  884.         elseif horepole==5 || horepole==6
  885.             hs=2;  
  886.         elseif horepole==3 || horepole==4
  887.             hs=3;              
  888.         else
  889.             hs=0;            
  890.         end
  891.         if vlavopole==2
  892.             vs=1;
  893.         elseif vlavopole==5 || vlavopole==6
  894.             vs=2;
  895.         elseif vlavopole==3 || vlavopole==4
  896.             vs=3;                                                          
  897.         else
  898.             vs=0;                      
  899.         end
  900.         if vpravopole==2
  901.             ps=1;
  902.         elseif vpravopole==5 || vpravopole==6
  903.             ps=2;                                                
  904.         elseif vpravopole==3 || vpravopole==4
  905.             ps=3;                                                          
  906.         else
  907.             ps=0;                      
  908.         end
  909.        
  910.     end
  911.     % smer dole
  912.     if robot.smer==2
  913.         if dolepole==2
  914.             hs=1;
  915.         elseif dolepole==5 || dolepole==6
  916.             hs=2;
  917.         elseif dolepole==3 || dolepole==4
  918.             hs=3;                                                          
  919.         else
  920.             hs=0;            
  921.         end
  922.         if vpravopole==2
  923.             vs=1;
  924.         elseif vpravopole==5 || vpravopole==6
  925.             vs=2;    
  926.         elseif vpravopole==3 || vpravopole==4
  927.             vs=3;                                              
  928.         else
  929.             vs=0;                      
  930.         end
  931.         if vlavopole==2
  932.             ps=1;
  933.         elseif vlavopole==5 || vlavopole==6
  934.             ps=2;
  935.         elseif vlavopole==3 || vlavopole==4
  936.             ps=3;                                              
  937.         else
  938.             ps=0;                      
  939.         end
  940.        
  941.     end
  942.      % smer vpravo
  943.     if robot.smer==3
  944.         if vpravopole==2
  945.             hs=1;
  946.         elseif vpravopole==5 || vpravopole==6
  947.             hs=2;
  948.         elseif vpravopole==3 || vpravopole==4
  949.             hs=3;                                  
  950.         else
  951.             hs=0;            
  952.         end
  953.         if horepole==2
  954.             vs=1;
  955.         elseif horepole==5 || horepole==6
  956.             vs=2;
  957.         elseif horepole==3 || horepole==4
  958.             vs=3;                                                          
  959.         else
  960.             vs=0;                      
  961.         end
  962.         if dolepole==2
  963.             ps=1;
  964.         elseif dolepole==5 || dolepole==6
  965.             ps=2;
  966.         elseif dolepole==3 || dolepole==4
  967.             ps=3;                                              
  968.         else
  969.             ps=0;                      
  970.         end
  971.        
  972.     end
  973.      % smer vlavo
  974.     if robot.smer==4
  975.         if vlavopole==2
  976.             hs=1;
  977.         elseif vlavopole==5 || vlavopole==6
  978.             hs=2;
  979.         elseif vlavopole==3 || vlavopole==4
  980.             hs=3;                      
  981.         else
  982.             hs=0;            
  983.         end
  984.         if dolepole==2
  985.             vs=1;
  986.         elseif dolepole==5 || dolepole==6
  987.             vs=2;  
  988.         elseif dolepole==3 || dolepole==4
  989.             vs=3;                                  
  990.         else
  991.             vs=0;                      
  992.         end
  993.         if horepole==2
  994.             ps=1;
  995.         elseif horepole==5 || horepole==6
  996.             ps=2;
  997.         elseif horepole==3 || horepole==4
  998.             ps=3;                                              
  999.         else
  1000.             ps=0;                      
  1001.         end
  1002.        
  1003.     end
  1004.  
  1005. end
  1006.  
  1007. % funkcia na prehladanie okolia robota radarom
  1008. % vrati pozicie kociek [Y X] - [riadok stlpec] v mape
  1009. function [novapozkocky]=najdikocku(robot,celamapa,dosahsenzora)
  1010.     % def. rozsahu senzora
  1011.     rmin=robot.poziciaY-dosahsenzora+1;
  1012.     rmax=robot.poziciaY+dosahsenzora+1;
  1013.     smin=robot.poziciaX-dosahsenzora+1;
  1014.     smax=robot.poziciaX+dosahsenzora+1;
  1015.    
  1016.     % ohranicenie mapy
  1017.     if rmin<2
  1018.         rmin=2;
  1019.     end
  1020.     if smin<1
  1021.         smin=1;
  1022.     end
  1023.     if rmax>41
  1024.         rmax=41;
  1025.     end
  1026.     if smax>41
  1027.         smax=41;
  1028.     end
  1029.    
  1030.     % ohranicenie prekazkov, radar cez prekaz nevidi
  1031.     ss=robot.poziciaX+1;
  1032.     for rr=rmin:robot.poziciaY+1,
  1033.         if celamapa(rr,ss)==2
  1034.             rmin=rr;
  1035.             break;
  1036.         end
  1037.     end
  1038.     rr=robot.poziciaY+1;
  1039.     for ss=smin:robot.poziciaX+1,
  1040.         if celamapa(rr,ss)==2
  1041.             smin=ss;
  1042.             break;
  1043.         end
  1044.     end
  1045.     ss=robot.poziciaX+1;
  1046.     for rr=robot.poziciaY+1:rmax,
  1047.         if celamapa(rr,ss)==2
  1048.             rmax=rr;
  1049.             break;
  1050.         end
  1051.     end
  1052.     rr=robot.poziciaY+1;
  1053.     for ss=robot.poziciaX+1:smax,
  1054.         if celamapa(rr,ss)==2
  1055.             smax=ss;
  1056.             break;
  1057.         end
  1058.     end    
  1059.     novapozkocky=[];
  1060.    
  1061.     % hladanie kocky v ohranicenom priestore
  1062.     for rr=rmin:rmax,
  1063.         for ss=smin:smax,
  1064.             if celamapa(rr,ss)==5
  1065.                 novapozkocky=[novapozkocky;rr-1 ss-1 1];
  1066.             elseif celamapa(rr,ss)==6
  1067.                 novapozkocky=[novapozkocky;rr-1 ss-1 2];
  1068.             end
  1069.         end
  1070.     end
  1071.            
  1072. end
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!