|
ImageAnalyst <imageanalyst@mailinator.com> wrote in message <9b92cd08-c0d9-42e6-b602-2cfe72f0ba6e@e40g2000yqn.googlegroups.com>...
> Tell us which variables are scalars and which are arrays.
I paste here the code
clear all;
global mb rho l r b d Jx Jy Jz Jt m1 m2 m3 Ix Iy Iz T1 T2 theta
mb = 21.43; % mass of the vehicle
rho = 1023.0; % fluid density
l = 0.1; % distance
r = 0.1; % radius
b = 0.068; % thrust factor
d = 3.617e-4; % drag factor
Jx = 0.0857; %roll inertia of the vehicle
Jy = 1.1143; % pitch inertia of the vehicle
Jz = 1.1143; % yaw inertia of the vehicle
Jt = 1.1941e-4; % thruster inertia
%
m1 = mb + 0.394*rho*pi*r^3; %total mass in the x-direction
m2 = mb + 5.96*rho*pi*r^3; %total mass in the y-direction
m3 = mb + 5.96*rho*pi*r^3; %total mass in the z-direction
%
Ix = Jx + 0; %total inertia in the x-direction
Iy = Jy + 24.2648*rho*pi*r^5; %total inertia in the y-direction
Iz = Jz + 24.2648*rho*pi*r^5; %total inertia in the z-direction
T1=2;
T2=2;
theta=pi/4;
%define tstart, tend and the number of time steps you want to take, n:
% tstart = 0;
% tend = 15;
% n = 0.01;
% tspan = linspace(tstart,tend,n);
tspan=[0 15];
%define the initial conditions making sure to use the right ordering
xinit = [2;0;pi;0;2;0;2;0];
%Get x,psi.
[t,x] = ode45(@integratingfunction3, tspan, xinit)
%define the output variables:
xx = x(:,1); % value for x
xxdot = x(:,2); %value for xdot
psi = x(:,3); % psi
psidot = x(:,4); %psidot
z = x(:,5); % value for x
zdot = x(:,6); %value for xdot
y = x(:,7); % psi
ydot = x(:,8); %psidot
% input
u1=cos(theta)*cos(psi)*T1/m1;
% u4=(l*T2-Iy*thetadot*phidot+(Jt*Omega+Ix*phidot)*thetadot)/Iz; % asal u4
u4=(l*T2)/Iz; % bila thetadot=0
v1=u1;
v2=(2*m1/m2.*tan(psi).*psidot)/(cos(psi).*cos(psi))+((m1/m2*u4)/(cos(psi).*cos(psi)));
%
% z11=x;
% z12=xdot;
z21=-m1/m3*tan(theta)*sec(psi);
z22=m1/m3*tan(theta).*sec(psi).*tan(psi).*psidot;
z31=m1/m2*tan(psi);
z32=m1/m2*psidot/(cos(psi).*cos(psi));
% z41=y;
% z42=ydot;
%
xddot=v1;
% psiddot=v2;
zddot=z21.*v1;
yddot=z31.*v1;
xinit1 = [2;0;2;0];
figure(1)
plot(x(:,1),'-','LineWidth',1.5) % x
title('position {\it x}','FontSize',18,'FontName','Times')
figure(2)
plot(x(:,3),'-','LineWidth',1.5) % psi
title('psi {\it {\psi}}','FontSize',18,'FontName','Times')
figure(3)
plot(x(:,5),'-','LineWidth',1.5) % z
title('position {\it z}','FontSize',18,'FontName','Times')
_________________
function dxdt = integratingfunction3(t,x)
global mb rho l r b d Jx Jy Jz Jt m1 m2 m3 Ix Iy Iz T1 T2 theta
%Recall that x=[x, xdot, psi, psidot, z, zdot, y, ydot]
%Define new variables: y=x(1), ydot=x(2), etc
p=x(1);%define parameter x as p --> position x
pdot=x(2); % xdot
psi=x(3);
psidot=x(4);
z=x(5);
zdot=x(6);
y=x(7);
ydot=x(8);
%notice that here x is just a 1 by 4 row vector and not a large array like
%in [t,x] = ode45(@fname, tspan, xinit, options)
u4=(l*T2)/Iz; %apabila thetadot=0
%the array dxdt is the same length as x
dxdt = zeros(size(x));
dxdt(1) = pdot;
dxdt(2) = cos(theta)*cos(psi)*T1/m1; % this is xddot =v1
dxdt(3) = psidot;
dxdt(4) = 2*m1/m2*tan(psi)*psidot/(cos(psi)*cos(psi))+ m1/m2*u4/(cos(psi)*cos(psi)); %this is psiddot
dxdt(5) = zdot;
dxdt(6) = -m1/m3*tan(theta)*sec(psi)*dxdt(2); %zddot
dxdt(7) = ydot;
dxdt(8) = m1/m2*tan(psi)*dxdt(2); %yddot
|