need help in looping

Constantine

Dear all,

I have writen a code for simple ship control system where a ship can move from an initial point to the target point. When there is only 1 target, my code run smoothly. However, I have to make it in two target, where the ship reach target 1 then continue to move to next target. My code is as below:-

clc
clear

%Parameter Constant
T=5;
v=10;
delta=0.5;

%Position of Target
xg=1650;
yg=1250;

%Gain of PID controller
ki=0.094;
kp=0.406;

for t=1:2000
if t==1
x(t)=0;
y(t)=500;
theta(t)=0;
turning(t)=0;
si=atan((xg-x(t))/(yg-y(t)));
err(t)=si-theta(t);
u(t)=ki*err(t)*delta/2;
r=kp*err(t)+u(t);

else if t>1
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
end

figure(1)
plot (0,500,'x',1650,1250,'x',x(t),y(t),'-')
axis([0 2000 0 1500])
hold on

if x(t)>1650
disp('Simulation End')
break
end
end

This code run smoothly. If the target yg less than initial y , then the equation
x(t)=x(t-1)+delta*v*sin(theta(t-1));
y(t)=y(t-1)+delta*v*cos(theta(t-1));
becomes
x(t)=x(t-1)-delta*v*sin(theta(t-1));
y(t)=y(t-1)-delta*v*cos(theta(t-1));
position xg (x of target) is always greater than x.

I wish to set my first target at xg1=800, yg1=1500. My second target at xg2=1650,yg2=1250, it is possible to modified my code??
I have try to add while loop but it juz didn't go right. Can someone help me in this problem??? thank you very much for your help.

drasnor

The typical approach here is to put your waypoints in a list and establish some sort of criteria for achieving a waypoint. That criteria can be as simple as "increment target waypoint index if current position is within some range of target waypoint." There is a large body of published work out there on this topic.