SHARE
TWEET

Peti

a guest Apr 23rd, 2019 79 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
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
 
Top