MatLab not plotting as expected and can't determine a fix.
26 views (last 30 days)
Show older comments
This code is supposed to demonstrate a vehicle/cart moving towards a goal coordinate that is moving over time. How the cart moves is based upon it's location and heading in relation to the goal. The problem occurs when the y or x coordinate of the cart gets extremely close to the goal and the angle between the cart and goal suddenly becomes very large, causing it to rotate while the goal moves away. This creates a jarring collection lines that are along the path of the goal. I've been trying various different modifications to the algorithm (which is the only thing I'm changing to alter the path finding of the cart) but none have fixed this problem.
clear; clc; close;
%% ODE45
tspan=0:0.1:60;
u0=[10,7,5];
[tout,uout]=ode45(@xytheta,tspan,u0);
xg=1.25*cos(2*tspan');
yg=1.25*sin(2*tspan');
%% Plot x and y vs Time
figure(1);
hold on
plot(tout,uout(:,2),'--r');
plot(tout,uout(:,3),'-.b');
plot(tspan,yg,'g')
plot(tspan,xg,'m')
ylabel('Distance (m)');
xlabel('Time (s)');
title('X and Y vs Time');
legend('x distance','y distance');
hold off
%% Plot Theta vs Time
figure(2);
Hding=uout(:,1)*(180/pi);
plot(tout,Hding,'g');
ylabel('Heading (deg)');
xlabel('Time (s)');
title('Heading vs Time');
%% Plot Path of Vehicle
figure(3);
hold on
plot(uout(:,2),uout(:,3),'o','MarkerSize',3);
plot(xg,yg,'r');
xlabel('x Distance (m)');
ylabel('y Distance (m)');
title('Path of Vehicle');
axis equal
%% Function for ODE45
function dudt=xytheta(t,u)
D=0.2;
w=0.3;
R0=1.25;
wg=2;
xg=R0*cos(wg*t);
yg=R0*sin(wg*t);
L=[xg-u(2),yg-u(3)];
gamma=atan2(L(2),L(1));
Omega=gamma-u(1); %% This is the begging of the Algorithm
if Omega<-pi
Omega=Omega+(2*pi);
elseif Omega>pi
Omega=Omega-(2*pi);
end
if abs(L(1))>5||abs(L(2))>5 %The values may seem random as I have been trying a bunch of different combinations
if Omega>(8*pi)/9
PhiL=20;
PhiR=-20;
elseif Omega<(-pi*8)/9
PhiL=-20;
PhiR=20;
elseif Omega<=(8*pi)/9&&Omega>(4*pi)/9
PhiL=10;
PhiR=-10;
elseif Omega>=(-8*pi)/9&&Omega<(-4*pi)/9
PhiL=-10;
PhiR=10;
elseif Omega<=(4*pi)/9&&Omega>(2*pi)/9
PhiL=5;
PhiR=-5;
elseif Omega>=(-4*pi)/9&&Omega<(-2*pi)/9
PhiL=-5;
PhiR=5;
elseif Omega<=(2*pi)/9&&Omega>(pi)/18
PhiL=2;
PhiR=-2;
elseif Omega>=(-2*pi)/9&&Omega<(-pi)/18
PhiL=-2;
PhiR=2;
else
PhiL=100;
PhiR=100;
end
elseif (abs(L(1))<=5&&abs(L(2))<=5)&&(abs(L(1))>1||abs(L(2))>1)
if abs(L(1))>2.5||abs(L(2))>2.5
if Omega>(8*pi)/9
PhiL=20;
PhiR=-20;
elseif Omega<(-pi*8)/9
PhiL=-20;
PhiR=20;
elseif Omega<=(8*pi)/9&&Omega>(4*pi)/9
PhiL=10;
PhiR=-10;
elseif Omega>=(-8*pi)/9&&Omega<(-4*pi)/9
PhiL=-10;
PhiR=10;
elseif Omega<=(4*pi)/9&&Omega>(2*pi)/9
PhiL=5;
PhiR=-5;
elseif Omega>=(-4*pi)/9&&Omega<(-2*pi)/9
PhiL=-85;
PhiR=5;
elseif Omega<=(2*pi)/9&&Omega>(pi)/18
PhiL=2;
PhiR=-2;
elseif Omega>=(-2*pi)/9&&Omega<(-pi)/18
PhiL=-2;
PhiR=2;
else
PhiL=120;
PhiR=120;
end
elseif abs(L(1))<=2.5&&abs(L(2))<=2.5
if Omega>(8*pi)/9
PhiL=20;
PhiR=-20;
elseif Omega<(-pi*8)/9
PhiL=-20;
PhiR=20;
elseif Omega<=(8*pi)/9&&Omega>(4*pi)/9
PhiL=10;
PhiR=-10;
elseif Omega>=(-8*pi)/9&&Omega<(-4*pi)/9
PhiL=-10;
PhiR=10;
elseif Omega<=(4*pi)/9&&Omega>(2*pi)/9
PhiL=5;
PhiR=-5;
elseif Omega>=(-4*pi)/9&&Omega<(-2*pi)/9
PhiL=-5;
PhiR=5;
elseif Omega<=(2*pi)/9&&Omega>(pi)/18
PhiL=3;
PhiR=-3;
elseif Omega>=(-2*pi)/9&&Omega<(-pi)/18
PhiL=-3;
PhiR=3;
else
PhiL=120;
PhiR=120;
end
end
elseif abs(L(1))<=1&&abs(L(2))<=1
if abs(xg)>0.1&&abs(yg)>0.1
if Omega>(8*pi)/9
PhiL=20;
PhiR=-20;
elseif Omega<(-pi*8)/9
PhiL=-20;
PhiR=20;
elseif Omega<=(8*pi)/9&&Omega>(4*pi)/9
PhiL=10;
PhiR=-10;
elseif Omega>=(-8*pi)/9&&Omega<(-4*pi)/9
PhiL=-10;
PhiR=10;
elseif Omega<=(4*pi)/9&&Omega>(2*pi)/9
PhiL=2.5;
PhiR=-2.5;
elseif Omega>=(-4*pi)/9&&Omega<(-2*pi)/9
PhiL=-2.5;
PhiR=2.5;
elseif Omega<=(2*pi)/9&&Omega>(pi)/18
PhiL=3;
PhiR=-3;
elseif Omega>=(-2*pi)/9&&Omega<(-pi)/18
PhiL=-3;
PhiR=3;
else
PhiL=90;
PhiR=90;
end
elseif abs(xg)<=0.1||abs(yg)<=0.1
if Omega>(8*pi)/9
PhiL=20;
PhiR=-20;
elseif Omega<(-pi*8)/9
PhiL=-20;
PhiR=20;
elseif Omega<=(8*pi)/9&&Omega>(4*pi)/9
PhiL=10;
PhiR=-10;
elseif Omega>=(-8*pi)/9&&Omega<(-4*pi)/9
PhiL=-10;
PhiR=10;
elseif Omega<=(4*pi)/9&&Omega>(2*pi)/9
PhiL=5;
PhiR=-5;
elseif Omega>=(-4*pi)/9&&Omega<(-2*pi)/9
PhiL=-5;
PhiR=5;
elseif Omega<=(2*pi)/9&&Omega>(pi)/18
PhiL=3;
PhiR=-3;
elseif Omega>=(-2*pi)/9&&Omega<(-pi)/18
PhiL=-3;
PhiR=3;
else
PhiL=15;
PhiR=15;
end
end
end % This is the end of the Algorithm
dudt=[(D/w)*(PhiL-PhiR);(D/2)*((PhiL+PhiR)/2)*cos(u(1));(D/2)*((PhiL+PhiR)/2)*sin(u(1))];
end
2 Comments
Steven Lord
on 2 May 2024
What is the mathematical form of the differential equations you're trying to solve? Your if / elseif / else blocks smell fishy to me.
in order to avoid the "circling the drain" behavior that I think you're concerned about, consider using an event function to stop the solver when you're "close enough" to the goal.
Francisco J. Triveno Vargas
on 18 Jul 2024
My friend i change two things in your graphs
The first one
tspan=0:0.001:30;
and second one about the heading angle that in fact is between 0 and 360, for this reason it is possible to use the values between -180 and +180 degrees.
Hding=wrapTo180(uout(:,1)*(180/pi));
The results
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1736891/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1736896/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1736901/image.png)
Answers (0)
See Also
Categories
Find more on Ordinary Differential Equations in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!