team.m

来自「从事多机器人探测控制的朋友可以参考我写的源程序」· M 代码 · 共 48 行

function [team,ddj]=team(lujing,zwd,robotnum,scanp)%-----------------队形规则:路径和机器人所在列差距越小越好，team值越大。
vvv=zwd(:,2*(1:8));
for ti=1:robotnum
    mmm=abs(vvv(ti,:)-lujing(ti));
    vvv1=max(mmm);vvv2=min(mmm);
    lll(ti,:)=abs(vvv(ti,:)-max(lujing)-5);
    lll1=max(lll);lll2=min(lll);
    for tj=1:8
        if scanp(ti,1)==0 %-----------------判断机器人不在区域内
           if scanp(ti,2)==1;%-------------------本区域未完全探测、处理。
              if vvv1==abs(zwd(ti,2*tj)-lujing(ti))%-----------------------远离目标方向运动.
                 team(ti,tj)=0;
              elseif vvv2==abs(zwd(ti,2*tj)-lujing(ti))
                  team(ti,tj)=200;
              else team(ti,tj)=30;
              end
           else %-------------------本区域完全探测、处理。
              if  lll1==abs(zwd(ti,2*tj)-max(lujing)-5)%-----------------------远离目标方向运动.
                 team(ti,tj)=0;
              elseif vvv2==abs(zwd(ti,2*tj)-max(lujing)-5)
                  team(ti,tj)=200;
              else team(ti,tj)=30;
              end
              
           end
       else%-----------------判断机器人在区域内
           if  scanp(ti,2)==1;%-------------------本区域未完全探测、处理。
              if abs(zwd(ti,2*tj)-lujing(ti))>4
                 team(ti,tj)=0;
              elseif vvv1==abs(zwd(ti,2*tj)-lujing(ti))%-----------------------远离目标方向运动.
                 team(ti,tj)=50;
              elseif vvv2==abs(zwd(ti,2*tj)-lujing(ti))
                  team(ti,tj)=70;
              else team(ti,tj)=60;
              end
           else
              if  lll1==abs(zwd(ti,2*tj)-max(lujing)-5)%-----------------------远离目标方向运动.
                 team(ti,tj)=0;
              elseif vvv2==abs(zwd(ti,2*tj)-max(lujing)-5)
                  team(ti,tj)=200;
              else team(ti,tj)=30;
              end
          end
      end
           
    end
end
        ddj=1;