"Constantine Ong" wrote in message <inkpgt$ppk$1@fred.mathworks.com>...
> 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((xgx(t))/(ygy(t)));
> err(t)=sitheta(t);
> u(t)=ki*err(t)*delta/2;
> r=kp*err(t)+u(t);
>
> else if t>1
> x(t)=x(t1)+delta*v*sin(theta(t1));
> y(t)=y(t1)+delta*v*cos(theta(t1));
> theta(t)=theta(t1)+delta*turning(t1);
> turning(t)=turning(t1)+(delta*(rturning(t1)))/T;
> si=atan((xgx(t))/(ygy(t)));
> err(t)=sitheta(t);
> u(t)=ki*(err(t)+err(t1))*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(t1)+delta*v*sin(theta(t1));
> y(t)=y(t1)+delta*v*cos(theta(t1));
> becomes
> x(t)=x(t1)delta*v*sin(theta(t1));
> y(t)=y(t1)delta*v*cos(theta(t1));
> position xg (x of target) is always greater than x.
>
> I wish to set my first target at xg1=800, yg1=1200. 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.
Hi,
You should have a look at documents dealing with preallocation in matlab. You were increasing the size of 4 or 5 vectors inside a loop (it's not a good habit). Also you should check it this is what you really want : if your target has an x coord < x coord of initial point the code won't work. The same for y coord. And even if they both are superior, there is a 'strange' (maybe it's what you want) behavior for
target=[0,500;800,1200;1650,1250;1800,1350].
Here is the code with the slight modifications. Hope it helps. Regards, Florin
clc
clear
%Parameter Constant
T=5;
v=10;
delta=0.5;
%Gain of PID controller
ki=0.094;
kp=0.406;
%Position of Target
target=[0,500;800,1200;1650,1250;1800,1500];
finalStep=2000;
for ii=2:size(target,1)
startx=target(ii1,1);
starty=target(ii1,2);
xg=target(ii,1);
yg=target(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);
for t=1:finalStep
if t==1
x(t)=startx;
y(t)=starty;
theta(t)=0;
turning(t)=0;
si=atan((xgx(t))/(ygy(t)));
err(t)=sitheta(t);
u(t)=ki*err(t)*delta/2;
r=kp*err(t)+u(t);
else if t>1
x(t)=x(t1)+delta*v*sin(theta(t1));
y(t)=y(t1)+delta*v*cos(theta(t1));
theta(t)=theta(t1)+delta*turning(t1);
turning(t)=turning(t1)+(delta*(rturning(t1)))/T;
si=atan((xgx(t))/(ygy(t)));
err(t)=sitheta(t);
u(t)=ki*(err(t)+err(t1))*delta/2;
r=kp*err(t)+u(t);
end
end
figure(1)
plot (startx,starty,'x',xg,yg,'x',x(t),y(t),'')
axis([0 2000 0 1500])
hold on
if x(t)>xg
disp('Simulation End')
break
end
end
end
