Iteration is not stopping. Can you please help thanks.

clf
clc
clear all
close all

%Obstacle #1
Obpf1= 4.5;
Obs1 = [275 0];

%Obstacle #2
Obpf2= 4;
Obs2 = [200 90];

%Obstacle #3
Obpf3= 3;
Obs3 = [200 190];

%Obstacle #4
Obpf4= 2;
Obs4 = [200 290];

%Moving Obstacle
Obpf5 =2;
Obs5 = [125 0];

%Initial Goal 1
Gpf1 =1;
Gi1 = [170 90];

%Initial Goal 2
Gpf2 =1;
Gi2 = [170 190];

%Initial Goal 3
Gpf3 =1;
Gi3 = [170 290];

%Secondary Goal 1
Gsf1 = 1.5;
Gs1 = [0 145];

%Secondary Goal 2
Gsf2 = 1.5;
Gs2 = [0 195];

%Secondary Goal 3
Gsf3 = 1.5;
Gs3 = [0 245];

%Robot 1
R1pf = 1;
R1 = [20 372];

%Robot 2
R2pf = 1;
R2 = [50 372];

%Robot 3
R3pf = 1;
R3 = [80 372];

%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Initial Robot Environment
%%%%%%%%%%%%%%%%%%%%%%%%%%%

rectangle('position',[275 0 150 400],'Facecolor','red','edgecolor','black','linewidth',1.5)  % sortation area 
hold on
 
rectangle('position',[200 90 75 20],'Facecolor','yellow')  %CHUTE NUMBER 1
rectangle('position',[200 190 75 20],'Facecolor','yellow')  %CHUTE NUMBER 2
rectangle('position',[200 290 75 20],'Facecolor','yellow')  %CHUTE NUMBER 3
hold on
 
rectangle('position',[168 88 20 20],'curvature',[1,1],'Facecolor','black')  %static robot 1
rectangle('position',[170 190 20 20],'curvature',[1,1],'Facecolor','black') %static robot 2
rectangle('position',[170 290 20 20],'curvature',[1,1],'Facecolor','black')  %static robot 3
hold on 
 
rectangle('position',[0 145 20 18],'facecolor','b') %truck station 1
rectangle('position',[0 195 20 18],'facecolor','b') %truck station 2
rectangle('position',[0 245 20 18],'facecolor','b') %truck station 3
hold on

rectangle('position',[125 0 5 5],'facecolor','b')

%human_obst(125,0,5,1)
auto_robot(20,372.5,10,1)
auto_robot(50,372.5,10,1)
auto_robot(80,372.5,10,1)
 
title('Autonomous Warehouse Environment')
 
axis([0 400 0 400]);%whole environment area 

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 % Create Repulsive and attractive potential field forces %
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %Matrix X x Y of  400 by 400
 i=1:400;
 j=1:400;
 [I,J] = meshgrid(i,j);
 Z = []; %empty matrix for z-axis plot

 for i=1:400 
    for j=1:400 
        pos = [j/10 i/10]; % scaled to fit 400 by 400 matrix
        z=0;    %accumulator
        
        %Obstacles with exponential curve while goal is by vector  
        z=z+exp(-norm(pos-Obs1)/Obpf1)+exp(-norm(pos-Obs2)/Obpf2)+ ...
            exp(-norm(pos-Obs3)/Obpf3)+exp(-norm(pos-Obs4))/Obpf4+ ...
            exp(-norm(pos-Obs5))/Obpf5+ ...
           Gpf1*(norm(Gi1-pos))+ Gpf2*(norm(Gi2-pos))+Gpf3*(norm(Gi3-pos)+...
           Gsf1*(norm(Gs1-pos))+ Gsf2*(norm(Gs2-pos))+Gsf3*(norm(Gs3-pos)));
    % The potential functions chosen are similar to the function 
    % proposed in the document 'apf.pdf'
        Z(i,j) = z; %store in matrix
            
    end
 end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%
 % Path planning for Robot % 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%

   
 %Plot current robot motion
 t1=(abs(R1(1)-Gi1(1))+abs(R1(2)-Gi1(2))); 
 t2=(abs(R2(1)-Gi2(1))+abs(R2(2)-Gi2(2)));
 t3=(abs(R3(1)-Gi3(1))+abs(R3(2)-Gi3(2)));
 
 count=1;
while t1>0.5 || t2 >0.5 || t3 > 0.5 %check if the robot had reached the specified target;  
             %stop when reach target
             
   dx1=[0 0];  %accumulation matrix robot1
   dx2=[0 0];  %accumulation matrix robot2
   dx3=[0 0];  %accumulation matrix robot3
 
   %Obstacles Accumulate vector sum  %Goal Accumulate - Robot 1
   dx1= dx1 + (R1-R2)*exp((-norm(R1-R2))/R2pf) + ...
        (R1-R3)*exp((-norm(R1-R3))/R3pf) + ...
        (R1-Obs1)*exp((-norm(R1-Obs1))/Obpf1)+ ...
        (R1-Obs2)*exp((-norm(R1-Obs2))/Obpf2)+ ...
        (R1-Obs3)*exp((-norm(R1-Obs3))/Obpf3)+ ...
        (R1-Obs4)*exp((-norm(R1-Obs4))/Obpf4)+ ...
        (R1-Obs5)*exp((-norm(R1-Obs5))/Obpf5)+ ...
        Gpf1*(Gi1-R1)/norm(Gi1-R1);
    
    
    %Obstacles Accumulate vector sum  %Goal Accumulate - Robot 2
    
    dx2= dx2 + (R2-R1)*exp((-norm(R2-R1))/R2pf) + ...
        (R2-R3)*exp((-norm(R2-R3))/R3pf) + ...
        (R2-Obs1)*exp((-norm(R2-Obs1))/Obpf1)+ ...
        (R2-Obs2)*exp((-norm(R2-Obs2))/Obpf2)+ ...
        (R2-Obs3)*exp((-norm(R2-Obs3))/Obpf3)+ ...
        (R2-Obs4)*exp((-norm(R2-Obs4))/Obpf4)+ ...
        (R2-Obs5)*exp((-norm(R2-Obs5))/Obpf5)+ ...
        Gpf2*(Gi2-R2)/norm(Gi2-R2);
    
    %Obstacles Accumulate vector sum  %Goal Accumulate - Robot 3
    
    dx3= dx3 + (R3-R1)*exp((-norm(R3-R1))/R1pf) + ...
        (R3-R2)*exp((-norm(R3-R2))/R2pf) + ...
        (R3-Obs1)*exp((-norm(R3-Obs1))/Obpf1)+ ...
        (R3-Obs2)*exp((-norm(R3-Obs2))/Obpf2)+ ...
        (R3-Obs3)*exp((-norm(R3-Obs3))/Obpf3)+ ...
        (R3-Obs4)*exp((-norm(R3-Obs4))/Obpf4)+ ...
        (R3-Obs5)*exp((-norm(R3-Obs5))/Obpf5)+ ...
        Gpf3*(Gi3-R3)/norm(Gi3-R3);
    
    R1=R1+dx1;
    R2=R2+dx2;
    R3=R3+dx3;
    
   t1=(abs(R1(1)-Gi1(1))+abs(R1(2)-Gi1(2))); 
   t2=(abs(R2(1)-Gi2(1))+abs(R2(2)-Gi2(2))); 
   t3=(abs(R3(1)-Gi3(1))+abs(R3(2)-Gi3(2))); 
   count=count+1; 
   
    plot3(R1(1,1), R1(1,2), Z(round(R1(1,2)), round(R1(1,1))),'K.','MarkerSize',10);
    plot3(R2(1,1), R2(1,2), Z(round(R2(1,2)), round(R2(1,1))),'R.','MarkerSize',10);
    plot3(R3(1,1), R3(1,2), Z(round(R3(1,2)), round(R3(1,1))),'G.','MarkerSize',10);

    
   title(sprintf('Iteration: %d',count));
   refresh;
   drawnow;
   
end

t4=(abs(R1(1)-Gs1(1))+abs(R1(2)-Gs1(2))); 
t5=(abs(R2(1)-Gs2(1))+abs(R2(2)-Gs2(2)));
t6=(abs(R3(1)-Gs3(1))+abs(R3(2)-Gs3(2)));

while t4>0.3 || t5 >0.3|| t6 > 0.3 %check if the robot had reached the specified target;  
             %stop when reach target
             
   dx4=[0 0];  %accumulation matrix robot1
   dx5=[0 0];  %accumulation matrix robot2
   dx6=[0 0];  %accumulation matrix robot3
 
   %Obstacles Accumulate vector sum  %Goal Accumulate - Robot 1
   dx4= dx4 + (R1-R2)*exp((-norm(R1-R2))/R2pf) + ...
        (R1-R3)*exp((-norm(R1-R3))/R3pf) + ...
        (R1-Obs1)*exp((-norm(R1-Obs1))/Obpf1)+ ...
        (R1-Obs2)*exp((-norm(R1-Obs2))/Obpf2)+ ...
        (R1-Obs3)*exp((-norm(R1-Obs3))/Obpf3)+ ...
        (R1-Obs4)*exp((-norm(R1-Obs4))/Obpf4)+ ...
        (R1-Obs5)*exp((-norm(R1-Obs5))/Obpf5)+ ...
        Gsf1*(Gs1-R1)/norm(Gs1-R1);
    
    
    %Obstacles Accumulate vector sum  %Goal Accumulate - Robot 2
    
    dx5= dx5 + (R2-R1)*exp((-norm(R2-R1))/R2pf) + ...
        (R2-R3)*exp((-norm(R2-R3))/R3pf) + ...
        (R2-Obs1)*exp((-norm(R2-Obs1))/Obpf1)+ ...
        (R2-Obs2)*exp((-norm(R2-Obs2))/Obpf2)+ ...
        (R2-Obs3)*exp((-norm(R2-Obs3))/Obpf3)+ ...
        (R2-Obs4)*exp((-norm(R2-Obs4))/Obpf4)+ ...
        (R2-Obs5)*exp((-norm(R2-Obs5))/Obpf5)+ ...
        Gsf2*(Gs2-R2)/norm(Gs2-R2);
    
    %Obstacles Accumulate vector sum  %Goal Accumulate - Robot 3
    
    dx6= dx6 + (R3-R1)*exp((-norm(R3-R1))/R1pf) + ...
        (R3-R2)*exp((-norm(R3-R2))/R2pf) + ...
        (R3-Obs1)*exp((-norm(R3-Obs1))/Obpf1)+ ...
        (R3-Obs2)*exp((-norm(R3-Obs2))/Obpf2)+ ...
        (R3-Obs3)*exp((-norm(R3-Obs3))/Obpf3)+ ...
        (R3-Obs4)*exp((-norm(R3-Obs4))/Obpf4)+ ...
        (R3-Obs5)*exp((-norm(R3-Obs5))/Obpf5)+ ...
        Gsf3*(Gs3-R3)/norm(Gs3-R3);
    
    R1=R1+dx4;
    R2=R2+dx5;
    R3=R3+dx6;
    
   t4=(abs(R1(1)-Gs1(1))+abs(R1(2)-Gs1(2))); 
   t5=(abs(R2(1)-Gs2(1))+abs(R2(2)-Gs2(2))); 
   t6=(abs(R3(1)-Gs3(1))+abs(R3(2)-Gs3(2))); 
   
   count=count+1; 
   
   % assuming arrays are 400x400
thisr = min(max(round(R1),1),200);
plot3(R1(1,1), R1(1,2), Z(thisr(2),thisr(1)),'K.','MarkerSize',10);
thisr = min(max(round(R2),1),200);
plot3(R2(1,1), R2(1,2), Z(thisr(2),thisr(1)),'R.','MarkerSize',10);
thisr = min(max(round(R3),1),200);
plot3(R3(1,1), R3(1,2), Z(thisr(2),thisr(1)),'G.','MarkerSize',10);
    
   title(sprintf('Iteration: %d',count));
   refresh;
   drawnow;
 
end

the used function

function auto_robot(x,y,r,n)
theta= linspace(0, 2*pi,n);
x1= x+r*cos(theta);
y1= y+r*sin(theta);
plot(x,y,'o','markerfacecolor','c','markersize',6),hold on
plot(x1,y1,'r-')
axis equal
end

Answer 1

[Adam]

Which iteration is not finishing? You have a nested for loop and two while loops in that code. I assume you mean one of the while loops.

I would suggest using the debugger to understand what is happening within the loop.

while t1>0.5 || t2 >0.5 || t3 > 0.5

is inherently an infinite loop, depending entirely on you updating t1, t2 and t3 within the loop to eventually become values for which that evaluates to false. If that never happens then the loop will indeed be infinite.

[JIBIN GEORGE]

the first while loop works as I required however the second while loop is not stopping.is there any way I can end the second while loop when its reached its target