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
정보
이 질문은 마감되었습니다. 편집하거나 답변을 올리려면 질문을 다시 여십시오.
이전 댓글 표시
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
댓글 수: 0
답변 (1개)
Image Analyst
2015년 11월 28일
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
댓글 수: 0
이 질문은 마감되었습니다.
마감:
2021년 8월 20일
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!
Translated by 
