Info

This question is closed. Reopen it to edit or answer.

I am doing two differnt points moving at same time with different position and velocities.In same loop am plotting two points,but in this two points are moving ata same time ,but after reaching of first point to destination ,other points also

1 view (last 30 days)
Swathy
Swathy on 28 Nov 2015
Closed: MATLAB Answer Bot on 20 Aug 2021
if w==2
T=5;
delta=0.5;
ki=0.094;
kp=0.406;
r=0
r1=0
%Position of Target
target=[startx,starty;posx1,posy1;];
target1=[startx1,starty1;posx11,posy11;];
finalStep=2000;
for ii=2:size(target,1)
startx=target(ii-1,1);
starty=target(ii-1,2);
xg=target(ii,1);
yg=target(ii,2);
startx1=target1(ii-1,1);
starty1=target1(ii-1,2);
xg1=target1(ii,1);
yg1=target1(ii,2);
x=zeros(1,finalStep);
y=zeros(1,finalStep);
theta=zeros(1,finalStep);
turning=zeros(1,finalStep);
err=zeros(1,finalStep);
u=zeros(1,finalStep);
x1=zeros(1,finalStep);
y1=zeros(1,finalStep);
theta1=zeros(1,finalStep);
turning1=zeros(1,finalStep);
err1=zeros(1,finalStep);
u1=zeros(1,finalStep);
for t=1:finalStep
if t==1
x(t)=startx;
y(t)=starty;
x1(t)=startx1;
y1(t)=starty1;
theta(t)=0;
turning(t)=0;
theta1(t)=0;
turning1(t)=0;
si=atan((xg-x(t))/(yg-y(t)));
si1=atan((xg1-x1(t))/(yg1-y1(t)));
err(t)=si-theta(t);
u(t)=ki*err(t)*delta/2;
r=kp*err(t)+u(t);
err1(t)=si1-theta1(t);
u1(t)=ki*err1(t)*delta/2;
r1=kp*err1(t)+u1(t);
else if t>1
if (xg>0&&yg>0)
x(t)=x(t-1)+delta*v*sin(theta(t-1));
y(t)=y(t-1)+delta*v*cos(theta(t-1));
theta(t)=theta(t-1)+delta*turning(t-1);
turning(t)=turning(t-1)+(delta*(r-turning(t-1)))/T;
si=atan((xg-x(t))/(yg-y(t)));
err(t)=si-theta(t);
u(t)=ki*(err(t)+err(t-1))*delta/2;
r=kp*err(t)+u(t);
end
if(xg<0&&yg>0)
x(t)=x(t-1)+delta*v*sin(theta(t-1));
y(t)=y(t-1)+delta*v*cos(theta(t-1));
theta(t)=theta(t-1)+delta*turning(t-1);
turning(t)=turning(t-1)+(delta*(r-turning(t-1)))/T;
si=atan((xg-x(t))/(yg-y(t)));
err(t)=si-theta(t);
u(t)=ki*(err(t)+err(t-1))*delta/2;
r=kp*err(t)+u(t);
end
if(xg>0&&yg<0)
x(t)=x(t-1)-delta*v*sin(theta(t-1));
y(t)=y(t-1)-delta*v*cos(theta(t-1));
theta(t)=theta(t-1)+delta*turning(t-1);
turning(t)=turning(t-1)+(delta*(r-turning(t-1)))/T;
si=atan((xg-x(t))/(yg-y(t)));
err(t)=si-theta(t);
u(t)=ki*(err(t)+err(t-1))*delta/2;
r=kp*err(t)+u(t);
end
if (xg<0&&yg<0)
x(t)=x(t-1)-delta*v*sin(theta(t-1));
y(t)=y(t-1)-delta*v*cos(theta(t-1));
theta(t)=theta(t-1)+delta*turning(t-1);
turning(t)=turning(t-1)+(delta*(r-turning(t-1)))/T;
si=atan((xg-x(t))/(yg-y(t)));
err(t)=si-theta(t);
u(t)=ki*(err(t)+err(t-1))*delta/2;
r=kp*err(t)+u(t);
end
if(xg1>0&&yg1>0)
x1(t)=x1(t-1)+delta*v1*sin(theta1(t-1));
y1(t)=y1(t-1)+delta*v1*cos(theta1(t-1));
theta1(t)=theta1(t-1)+delta*turning1(t-1);
turning1(t)=turning1(t-1)+(delta*(r1-turning1(t-1)))/T;
si1=atan((xg1-x1(t))/(yg1-y1(t)));
err1(t)=si1-theta1(t);
u1(t)=ki*(err1(t)+err1(t-1))*delta/2;
r1=kp*err1(t)+u1(t);
end
if(xg1>0&&yg1<0)
x1(t)=x1(t-1)-delta*v1*sin(theta1(t-1));
y1(t)=y1(t-1)-delta*v1*cos(theta1(t-1));
theta1(t)=theta1(t-1)+delta*turning1(t-1);
turning1(t)=turning1(t-1)+(delta*(r1-turning1(t-1)))/T;
si1=atan((xg1-x1(t))/(yg1-y1(t)));
err1(t)=si1-theta1(t);
u1(t)=ki*(err1(t)+err1(t-1))*delta/2;
r1=kp*err1(t)+u1(t);
end
if(xg1<0&&yg1>0)
x1(t)=x1(t-1)+delta*v1*sin(theta1(t-1));
y1(t)=y1(t-1)+delta*v1*cos(theta1(t-1));
theta1(t)=theta1(t-1)+delta*turning1(t-1);
turning1(t)=turning1(t-1)+(delta*(r1-turning1(t-1)))/T;
si1=atan((xg1-x1(t))/(yg1-y1(t)));
err1(t)=si1-theta1(t);
u1(t)=ki*(err1(t)+err1(t-1))*delta/2;
r1=kp*err1(t)+u1(t);
end
if (xg1<0&&yg1<0)
x1(t)=x1(t-1)-delta*v1*sin(theta1(t-1));
y1(t)=y1(t-1)-delta*v1*cos(theta1(t-1));
theta1(t)=theta1(t-1)+delta*turning1(t-1);
turning1(t)=turning1(t-1)+(delta*(r1-turning1(t-1)))/T;
si1=atan((xg1-x1(t))/(yg1-y1(t)));
err1(t)=si1-theta1(t);
u1(t)=ki*(err1(t)+err1(t-1))*delta/2;
r1=kp*err1(t)+u1(t);
end
end
end
figure(1);
%set(text,'String',{'',['Vx '],' ',' ',['Vy '], ' ' ,' ',['PosX '],' ' ,' ',['PosY '],' ' ,' ',['Time ']});
plot (startx,starty,'--rs', xg,yg,'--rs',x(t),y(t),'-')
plot (startx1,starty1,'--rs', xg1,yg1,'--rs',x1(t),y1(t),'-')
hold on
drawnow
axis([-5000 5000 -5000 5000])
labels = cellstr( 'T2' );
labels1 = cellstr( 'T1' );
text(startx,starty, labels1, 'horizontal','left', 'vertical','top');
text(startx1,starty1, labels, 'horizontal','left', 'vertical','top');
% set(text11,'String',{'',['posx '],'',['posy '], '',['V '],' ',['t '],' ' });
% hold on
if xg>0
if x(t)>xg
if(posx1==xg)
disp('posx4444')
v=p2(3);
end
if(posx2==xg)
disp('posx2222')
v=p3(3);
end
if(posx3==xg)
disp('posx2222')
v=p4(3);
end
disp('Simulation1 End')
break
%stop
end
end
if xg<0
if x(t)<xg
if(posx1==xg)
disp('posx4444')
v=p2(3);
end
if(posx2==xg)
disp('posx2222')
v=p3(3);
end
if(posx3==xg)
disp('posx2222')
v=p4(3);
end
disp('Simulation2 End')
break
end
end
if xg1>0
if x1(t)>xg1
if(posx11==xg1)
disp('posx4444')
v=p21(3);
end
if(posx21==xg1)
disp('posx2222')
v=p31(3);
end
if(posx21==xg1)
disp('posx2222')
v=p41(3);
end
disp('Simulation12 End')
break
end
end
if xg1<0
if x1(t)<xg1
if(posx11==xg1)
disp('posx4444')
v=p21(3);
end
if(posx21==xg1)
disp('posx2222')
v=p31(3);
end
if(posx31==xg1)
disp('posx2222')
v=p41(3);
end
disp('Simulation21 End')
break
end
end
end
end

Sign in to answer this question.

Answers (1)

Image Analyst
Image Analyst on 28 Nov 2015
Try this: http://blogs.mathworks.com/videos/2012/07/03/debugging-in-matlab/
Or else this:
% Loop 1 that you break out of.
for k = 1 : 40
if someCondition
break
end
end
% Now do the next loop
for k = 1 : 13
% code
end
% Now do the second loop
for k = 1 : 42
% code
end

Tags

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!