Solving Linear Differential Equations for Path Following ; I am getting difficulties in implementing the coding with variable psi_e, let's say if I want to run it for psi_e going from 0 to pi/2, how can it be done?
2 visualizaciones (últimos 30 días)
Mostrar comentarios más antiguos
function [dydt] = MyFun(t,y,w1_x,w1_y,w2_x,w2_y)
dydt = zeros(3,1);
S = 15 ;
psi_e = pi/2 ;
k = 10 ;
Tau = 135 ;
alpha = 10 ;
S_x = y(1) ;
S_y = y(2) ;
psi = y(3) ;
p = [S_x , S_y]' ;
w1 = [w1_x , w1_y]' ;
w2 = [w2_x , w2_y]' ;
psi_f = atan2((w2_y - w1_y),(w2_x - w1_x)) ;
S_star = ((p - w1)'*(w2 - w1))/((norm(w2 - w1))^2) ;
Epsilon = norm( p - (S_star*(w2 - w1) + w1) ) ;
Rho = sign((w2 - w1).*(p - w1)) ;
if( abs(Epsilon) > Tau )
psi_d = psi_f + psi_e ;
psi_c = psi_d ;
else
psi_d = psi_f - psi_e*((Epsilon/Tau)^k) ;
psi_c = psi_d -(k*psi_e*S*(Epsilon^(k-1))*sin(psi))/(alpha*(Tau^k)) ;
end
u = k*(psi_d - psi) ;
psi_dot = u ;
dydt(1) = S*cos(psi) ;
dydt(2) = S*sin(psi) ;
dydt(3) = psi_dot ;
end
%%%%%%%%%%%%%%%%% FUNCTION'S USED IN THE BELOW CODE %%%%%%%%%%%%%%%%%%
clc;
clear all;
wps = [[0, 0];[400, 400];];
wp_1 = wps(1,:);
w1_x = wp_1(1); w1_y = wp_1(2);
wp_2 = wps(2,:);
w2_x = wp_2(1); w2_y = wp_2(2);
tspan = 0:0.1:50;
current_x = 125;
current_y = 0;
current_X = pi/2;
tau = 45;
fprintf("Trajectories\n");
arr_x = (0:1:300);
arr_y = (0:1:300);
plot(arr_x(1,:),arr_y(1,:),'--b');
y0 = [current_x current_y current_X] ;
[t,y] = ode45(@(t,y) MyFun(t,y,w1_x,w1_y,w2_x,w2_y), tspan, y0);
current_x = y(end,1);
current_y = y(end,2);
current_X = y(end,3);
hold on
grid on
for i = 1:length(y(:,1))-1
plot(y(i:i+1,1),y(i:i+1,2),'y','LineWidth',1.2);
pause(0.001)
end
xlabel('X(m)')
ylabel('Y(m)')
2 comentarios
J. Alex Lee
el 20 de Jul. de 2020
can you reformat using the "code view" functionality of the editor
Respuestas (1)
J. Alex Lee
el 21 de Jul. de 2020
To change value of psi_e (make it variable) seems straightforward:
make psi_e an input in your odefun, then loop over psi_e:
function [dydt] = MyFun(t,y,w1_x,w1_y,w2_x,w2_y , psi_e)
dydt = zeros(3,1);
S = 15 ;
%psi_e = pi/2 ;
...
end
Then in your script
N = 11
psi_e_list = linspace(0.1,2,N)
for i = 1:N
psi_e = psi_e_list(i)
...
end
It's not clear the rest of your objective...
0 comentarios
Ver también
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!