Iteration is not stopping. Can you please help thanks.

Question

JIBIN GEORGE on 2 Feb 2022

0
Link

Direct link to this question

https://in.mathworks.com/matlabcentral/answers/1641470-iteration-is-not-stopping-can-you-please-help-thanks

Commented: JIBIN GEORGE on 2 Feb 2022

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

0 Comments
Show -2 older commentsHide -2 older comments

Sign in to comment.

Sign in to answer this question.

Answer 1

Adam on 2 Feb 2022

0
Link

Direct link to this answer

https://in.mathworks.com/matlabcentral/answers/1641470-iteration-is-not-stopping-can-you-please-help-thanks#answer_887285

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.

1 Comment
Show -1 older commentsHide -1 older comments

JIBIN GEORGE on 2 Feb 2022

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

Sign in to comment.

Iteration is not stopping. Can you please help thanks.

0 Comments
Show -2 older commentsHide -2 older comments

Answers (1)

1 Comment
Show -1 older commentsHide -1 older comments

See Also

Categories

Tags

Community Treasure Hunt

Iteration is not stopping. Can you please help thanks.

0 Comments Show -2 older commentsHide -2 older comments

Answers (1)

1 Comment Show -1 older commentsHide -1 older comments

See Also

Categories

Tags

Community Treasure Hunt

0 Comments
Show -2 older commentsHide -2 older comments

1 Comment
Show -1 older commentsHide -1 older comments