NaN shows up in matrix when solving my ODEs, how can I fix this?
All of my matrices I am solving for have NaN after the third time line. How do I solved this issue?
Code:
clear
clc
%Menus/Variable Defintions/MEchanical Parameter Calculations
filename = 'Force Impact FBD.png';
I = imread('Force Impact FBD.png');
y = imread('Force Impact FBD.png', 'BackgroundColor', ["none"]);
imshow('Force Impact FBD.png')
%Impact on M1 FBD Menu
prompt={'Impact Force (N)','beta (degrees)','Time at which impulse will begin (seconds after zero)','Duration of Impact (seconds)'};
dlg_title='Impact on M1 FBD Menu';
num_lines=1;
defaultans={'3000','10','3','0.3'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);
%Variable Assignments from User Inputs
FimpM1=str2double(answer{1});
betaimp=str2double(answer{2});
tbeginimp=str2double(answer{3});
tdurimp=str2double(answer{4});
FimpM1x=-((FimpM1)*(cosd(betaimp)));
FimpM1z=((FimpM1)*(sind(betaimp)));
filename = 'FBD (with distances and dimensions and forces).png';
I = imread('FBD (with distances and dimensions and forces).png');
y = imread('FBD (with distances and dimensions and forces).png', 'BackgroundColor', ["none"]);
imshow('FBD (with distances and dimensions and forces).png')
%FBD Variable Defintion Menu 1
prompt={'x6i (m)','z6i (m)','x5i (m)','z5i (m)','x4i (m)','z4i (m)','x3i (m)','z3i (m)','x2i (m)','z2i (m)','x1i (m)','z1i (m)'};
dlg_title='FBD Variable Defintion Menu 1';
num_lines=1;
defaultans={'0.00','0.30','0.00','0.25','0.00','0.20','0.00','0.15','0.00','0.10','0.00','0.05'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);
%Variable Assignments from User Inputs
x6i=str2double(answer{1});
z6i=str2double(answer{2});
x5i=str2double(answer{3});
z5i=str2double(answer{4});
x4i=str2double(answer{5});
z4i=str2double(answer{6});
x3i=str2double(answer{7});
z3i=str2double(answer{8});
x2i=str2double(answer{9});
z2i=str2double(answer{10});
x1i=str2double(answer{11});
z1i=str2double(answer{12});
%FBD Variable Defintion Menu 2
prompt={'xw6 (m)','zw6 (m)','xw5 (m)','zw5 (m)','xw4 (m)','zw4 (m)','xw3 (m)','zw3 (m)'};
dlg_title='FBD Variable Defintion Menu 2';
num_lines=1;
defaultans={'0.10','0.30','0.10','0.25','0.10','0.20','0.10','0.15'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);
%Variable Assignments from User Inputs
xw6=str2double(answer{1});
zw6=str2double(answer{2});
xw5=str2double(answer{3});
zw5=str2double(answer{4});
xw4=str2double(answer{5});
zw4=str2double(answer{6});
xw3=str2double(answer{7});
zw3=str2double(answer{8});
zq6=zw6;
zq5=zw5;
zq4=zw4;
zq3=zw3;
%FBD Variable Defintion Menu 3
prompt={'xw2 (m)','zw2 (m)','xw1 (m)','zw1 (m)','xw0 (m)','zw0 (m)', 'xw7 (m)', 'zw7 (m)'};
dlg_title='FBD Variable Defintion Menu 3';
num_lines=1;
defaultans={'0.10','0.10','0.10','0.05','0.10','0.00','0.00','0.00'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);
%Variable Assignments from User Inputs
xw2=str2double(answer{1});
zw2=str2double(answer{2});
xw1=str2double(answer{3});
zw1=str2double(answer{4});
xw0=str2double(answer{5});
zw0=str2double(answer{6});
xw7=str2double(answer{7});
zw7=str2double(answer{8});
zq2=zw2;
zq1=zw1;
zq0=zw0;
xq1=xw0;
xq0=xw7;
%Car Seat Stiffnesses Menu
prompt={'Lower Seat Back (K12) (kN/m)','Lower Seat Back (K10) (kN/m)','Middle Seat Back (K8) (kN/m)','Middle Seat Back (K6) (kN/m)','Top Seat Back (K4) (kN/m)','Top Seat Back (K2) (kN/m)'};
dlg_title='Car Seat Stiffnesses Menu';
num_lines=1;
defaultans={'680.94','3565.77','1138.64','1138.64','666.55','2322.89'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);
%Variable Assignments from User Inputs
K12=str2double(answer{1});
K10=str2double(answer{2});
K8=str2double(answer{3});
K6=str2double(answer{4});
K4=str2double(answer{5});
K2=str2double(answer{6});
%Car Seat Damping Coefficients Menu
prompt={'Lower Seat Back (C12) (N-sec/m)','Lower Seat Back (C10) (N-sec/m)','Middle Seat Back (C8) (N-sec/m)','Middle Seat Back (C6) (N-sec/m)','Top Seat Back (C4) (N-sec/m)','Top Seat Back (C2) (N-sec/m)'};
dlg_title='Car Seat Damping Coefficients Menu';
num_lines=1;
defaultans={'4.10','0.75','24.45','24.45','772.68','3258.36'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);
%Variable Assignments from User Inputs
C12=str2double(answer{1});
C10=str2double(answer{2});
C8=str2double(answer{3});
C6=str2double(answer{4});
C4=str2double(answer{5});
C2=str2double(answer{6});
%Anthropomorphic Measurmeents Menu 1
prompt={'Standing Height (cm):','Shoulder Height (cm)','Head Length (cm)','Head Breadth (cm)','Neck Circumference (cm)','Chest Depth (cm)','Chest Breadth (cm)'};
dlg_title='Input Anthropomorphic Measurmeents (cm)';
num_lines=1;
defaultans={'168.67','146.53','19.86','15.57','37.95','23.32','32.89'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);
%Variable Assignments from User Inputs
L1=str2double(answer{1});
L2=str2double(answer{2});
L3=str2double(answer{3});
L4=str2double(answer{4});
L5=str2double(answer{5});
L6=str2double(answer{6});
L7=str2double(answer{7});
%Anthropomorphic Measurmeents Menu 2
prompt={'Waist Depth (cm)','Waist Breadth (cm)','Buttock Depth (cm)','Hip Breadth, Standing (cm)','Shoulder to Elbow Length (cm)','Elbow-Wrist Length (cm)','Wrist-Fingertip Length (cm)'};
dlg_title='Input Anthropomorphic Measurmeents (cm)';
num_lines=1;
defaultans={'21.51','28.22','23.19','35.43','37.54','25.28','19.72'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);
%Variable Assignments from User Inputs
L8=str2double(answer{1});
L9=str2double(answer{2});
L10=str2double(answer{3});
L11=str2double(answer{4});
L12=str2double(answer{5});
L13=str2double(answer{6});
L14=str2double(answer{7});
%Miscellaneous Inputs Menu 1
prompt={'Segments Combined Mass (kg)','Elastic Modulus of Bones and Tissue (MN/m^2)','Effective Coefficient of Viscosity of Blood and Synovial Fluid (N-sec/m^2)'};
dlg_title='Miscellaneous Inputs Menu 1';
num_lines=1;
defaultans={'35','1.30','0.003'};
answer=inputdlg(prompt,dlg_title,num_lines,defaultans);
%Variable Assignments from User Inputs
MassCom=str2double(answer{1});
E=str2double(answer{2});
mew=str2double(answer{3});
%Formulae for segment's ellipsoidal semi-axes
M6a = L4/2;
M6b = L4/2;
M6c = L3/2;
M5a = L5/(2*pi);
M5b = L5/(2*pi);
M5c = (L1-L2-L3)/2;
M4a = L6/2;
M4b = L7/2;
M4c = L12/2;
M3a = L6/2;
M3b = L7/2;
M3c = L12/2;
M2a = L8/2;
M2b = L9/2;
M2c = L13/2;
M1a = L8/2;
M1b = L9/2;
M1c = L14/2;
%Formulae for volume and mass of segments
VolumeM6 = pi*M6a*M6b*M6c;
VolumeM5 = pi*M5a*M5b*M5c;
VolumeM4 = pi*M4a*M4b*M4c;
VolumeM3 = pi*M3a*M3b*M3c;
VolumeM2 = pi*M2a*M2b*M2c;
VolumeM1 = pi*M1a*M1b*M1c;
VolumeCom = VolumeM6+VolumeM5+VolumeM4+VolumeM3+VolumeM2+VolumeM1;
MassM6 = (MassCom*VolumeM6)/VolumeCom;
MassM5 = (MassCom*VolumeM5)/VolumeCom;
MassM4 = (MassCom*VolumeM4)/VolumeCom;
MassM3 = (MassCom*VolumeM3)/VolumeCom;
MassM2 = (MassCom*VolumeM2)/VolumeCom;
MassM1 = (MassCom*VolumeM1)/VolumeCom;
%Formulae for segment stiffness
I = 0.366529188; %unitless
K11 = (pi*(E*1000)*(M6a/100)*(M6b/100))/((M6c/100)*I); %E*1000 converts from MN/m^2 to kN/m^2, each M../100 converts from cm to m
K9 = (pi*(E*1000)*(M5a/100)*(M5b/100))/((M5c/100)*I); %E*1000 converts from MN/m^2 to kN/m^2, each M../100 converts from cm to m
K7 = (pi*(E*1000)*(M4a/100)*(M4b/100))/((M4c/100)*I); %E*1000 converts from MN/m^2 to kN/m^2, each M../100 converts from cm to m
K5 = (pi*(E*1000)*(M3a/100)*(M3b/100))/((M3c/100)*I); %E*1000 converts from MN/m^2 to kN/m^2, each M../100 converts from cm to m
K3 = (pi*(E*1000)*(M2a/100)*(M2b/100))/((M2c/100)*I); %E*1000 converts from MN/m^2 to kN/m^2, each M../100 converts from cm to m
K1 = (pi*(E*1000)*(M1a/100)*(M1b/100))/((M1c/100)*I); %E*1000 converts from MN/m^2 to kN/m^2, each M../100 converts from cm to m
%Formulae for segment damping
AreaM6 = pi*(M6a/100)*(M6b/100);
LengthM6 = L3/100;
DiamM6 = L4/100;
ClearM6 = .07*DiamM6;
C11 = ((12*mew)/(pi))*((((AreaM6)^2)*(LengthM6))/((DiamM6)*((ClearM6)^3)));
AreaM5 = pi*(M5a/100)*(M5b/100);
LengthM5 = (L1-L2-L3)/100;
DiamM5 = L5/(pi*100);
ClearM5 = .06*DiamM5;
C9 = ((12*mew)/(pi))*((((AreaM5)^2)*(LengthM5))/((DiamM5)*((ClearM5)^3)));
AreaM4 = pi*(M4a/100)*(M4b/100);
LengthM4 = L12/100;
DiamM4 = L6/100;
ClearM4 = .06*DiamM4;
C7 = ((12*mew)/(pi))*((((AreaM4)^2)*(LengthM4))/((DiamM4)*((ClearM4)^3)));
AreaM3 = pi*(M3a/100)*(M3b/100);
LengthM3 = L12/100;
DiamM3 = L6/100;
ClearM3 = .06*DiamM3;
C5 = ((12*mew)/(pi))*((((AreaM3)^2)*(LengthM3))/((DiamM3)*((ClearM3)^3)));
AreaM2 = pi*(M2a/100)*(M2b/100);
LengthM2 = L13/100;
DiamM2 = L8/100;
ClearM2 = .01*DiamM2;
C3 = ((12*mew)/(pi))*((((AreaM2)^2)*(LengthM2))/((DiamM2)*((ClearM2)^3)));
AreaM1 = pi*(M1a/100)*(M1b/100);
LengthM1 = L14/100;
DiamM1 = L10/100;
ClearM1 = .01*DiamM1;
C1 = ((12*mew)/(pi))*((((AreaM1)^2)*(LengthM1))/((DiamM1)*((ClearM1)^3)));
tic
%Equations of Motion (X-direction and Z-direction)
% Other problem parameters
vx1i=2; % segement 1 initial x-velocity (m/s)
vz1i=2; % segement 1 initial z-velocity (m/s)
vx2i=2; % segement 2 initial x-velocity (m/s)
vz2i=2; % segement 2 initial z-velocity (m/s)
vx3i=2; % segement 3 initial x-velocity (m/s)
vz3i=2; % segement 3 initial z-velocity (m/s)
vx4i=2; % segement 4 initial x-velocity (m/s)
vz4i=2; % segement 4 initial z-velocity (m/s)
vx5i=2; % segement 5 initial x-velocity (m/s)
vz5i=2; % segement 5 initial z-velocity (m/s)
vx6i=2; % segement 6 initial x-velocity (m/s)
vz6i=2; % segement 6 initial z-velocity (m/s)
vxw1=3; % segement 1 initial x-velocity (m/s)
vzw1=2; % segement 1 initial z-velocity (m/s)
vxw2=2; % segement 2 initial x-velocity (m/s)
vzw2=2; % segement 2 initial z-velocity (m/s)
vxw3=2; % segement 3 initial x-velocity (m/s)
vzw3=2; % segement 3 initial z-velocity (m/s)
vxw4=2; % segement 4 initial x-velocity (m/s)
vzw4=2; % segement 4 initial z-velocity (m/s)
vxw5=2; % segement 5 initial x-velocity (m/s)
vzw5=2; % segement 5 initial z-velocity (m/s)
vxw6=2; % segement 6 initial x-velocity (m/s)
vzw6=2; % segement 6 initial z-velocity (m/s)
% Set time step stuff
simTime=10; % simulation time (s)
tStep=0.001; % simulation time step
iterations=simTime/tStep;
t=0:iterations;
% FimpM1=str2double(answer{1});
% betaimp=str2double(answer{2});
% tbeginimp=str2double(answer{3});
% tdurimp=str2double(answer{4});
% FimpM1x=-((FimpM1)*(cosd(betaimp)));
% FimpM1z=((FimpM1)*(sind(betaimp)));
if (tbeginimp <= t) & (t <= tbeginimp+tdurimp)
FimpM1xapplied = FimpM1x;
else
FimpM1xapplied = 0;
end
if (tbeginimp <= t) & (t <= tbeginimp+tdurimp)
FimpM1zapplied = FimpM1z;
else
FimpM1zapplied = 0;
end
% Pre-allocate variables for speed and add initial conditions
x1=zeros(iterations,1);
x1(1,:)=x1i;
x2=zeros(iterations,1);
x2(1,:)=x2i;
x3=zeros(iterations,1);
x3(1,:)=x3i;
x4=zeros(iterations,1);
x4(1,:)=x4i;
x5=zeros(iterations,1);
x5(1,:)=x5i;
x6=zeros(iterations,1);
x6(1,:)=x6i;
z1=zeros(iterations,1);
z1(1,:)=z1i;
z2=zeros(iterations,1);
z2(1,:)=z2i;
z3=zeros(iterations,1);
z3(1,:)=z3i;
z4=zeros(iterations,1);
z4(1,:)=z4i;
z5=zeros(iterations,1);
z5(1,:)=z5i;
z6=zeros(iterations,1);
z6(1,:)=z6i;
vx1=zeros(iterations,1);
vx1(1,:)=vx1i;
vx2=zeros(iterations,1);
vx2(1,:)=vx2i;
vx3=zeros(iterations,1);
vx3(1,:)=vx3i;
vx4=zeros(iterations,1);
vx4(1,:)=vx4i;
vx5=zeros(iterations,1);
vx5(1,:)=vx5i;
vx6=zeros(iterations,1);
vx6(1,:)=vx6i;
vz1=zeros(iterations,1);
vz1(1,:)=vz1i;
vz2=zeros(iterations,1);
vz2(1,:)=vz2i;
vz3=zeros(iterations,1);
vz3(1,:)=vz3i;
vz4=zeros(iterations,1);
vz4(1,:)=vz4i;
vz5=zeros(iterations,1);
vz5(1,:)=vz5i;
vz6=zeros(iterations,1);
vz6(1,:)=vz6i;
%Mass 1 Equations of Motion
d01i=((((z1i)^2)+((x1i-xq0)^2))^(1/2)); %2b
Fs10i=(-K1)*(d01i-zq1); %2a
d01dti=((z1i*vz1i)+((x1i-xq0)*vx1i))/d01i; %3b
Fd10i=(-C1)*(d01dti); %3a
d12i=((((z2i-z1i)^(2))+((x2i-x1i))^(2))^(1/2)); %4b
Fs12i=(K3)*(d12i-(zq2-zq1)); %4a
d12dti=(1/d12i)*(((z2i-z1i)*(vz2i))-((z2i-z1i)*(vz1i))+((x2i-x1i)*(vx2i))-((x2i-x1i)*(vx1i))); %5b
Fd12i=(C3)*(d12dti); %5a
d1w1i=((((zw1-z1i)^(2))+((xw1-x1i))^(2))^(1/2)); %6b
Fs1w1i=(K2)*(d1w1i-(xq1-xq0)); %6a
d1w1dti=(1/d1w1i)*(((zw1-z1i)*(vzw1))-((zw1-z1i)*(vz1i))+((xw1-x1i)*(vxw1))-((xw1-x1i)*(vx1i))); %7b
Fd1w1i=(C2)*(d1w1dti); %7a
axM1=zeros(iterations,1);
axM1(1,:)=((Fs10i)*((x1i-xq0)/(d01i)))+((Fd10i)*((x1i-xq0)/(d01i)))+(Fs12i*((x1i-x2i)/d12i))+(Fd12i*((x1i-x2i)/d12i))+(Fs1w1i*((x1i-xw1)/d1w1i))+(Fd1w1i*((x1i-xw1)/d1w1i))+(FimpM1xapplied)/MassM1; %1a
azM1=zeros(iterations,1);
azM1(1,:)=((Fs10i)*(z1i/d01i))+((Fd10i)*((z1i)/(d01i)))+(Fs12i*((z1i-z2i)/d12i))+(Fd12i*((z1i-z2i)/d12i))+(Fs1w1i*((z1i-zw1)/d1w1i))+(Fd1w1i*((z1i-zw1)/d1w1i))+(FimpM1zapplied)/MassM1; %1b
%Mass 2 Equations of Motion
d23i=((((z3i-z2i)^(2))+((x3i-x2i))^(2))^(1/2)); %11b
Fs23i=(K5)*(d23i-(zq3-zq2)); %11a
d23dti=(1/d23i)*(((z3i-z2i)*(vz3i))-((z3i-z2i)*(vz2i))+((x3i-x2i)*(vx3i))-((x3i-x2i)*(vx2i))); %12b
Fd23i=(C5)*(d23dti); %12a
d2w2i=((((zw2-z2i)^(2))+((xw2-x2i))^(2))^(1/2)); %13b
Fs2w2i=(K4)*(d2w2i-(xq1-xq0)); %13a
d2w2dti=(1/d2w2i)*(((zw2-z2i)*(vzw2))-((zw2-z2i)*(vz2i))+((xw2-x2i)*(vxw2))-((xw2-x2i)*(vx2i))); %14b
Fd2w2i=(C4)*(d2w2dti); %14a
axM2=zeros(iterations,1);
axM2(1,:)=(-Fs12i*((x1i-x2i)/d12i))+(-Fd12i*((x1i-x2i)/d12i))+(Fs23i*((x2i-x3i)/d23i))+(Fd23i*((x2i-x3i)/d23i))+(Fs2w2i*((x2i-xw2)/d2w2i))+(Fd2w2i*((x2i-xw2)/d2w2i))/MassM2; %8a
azM2=zeros(iterations,1);
azM2(1,:)=(-Fs12i*((z1i-z2i)/d12i))+(-Fd12i*((z1i-z2i)/d12i))+(Fs23i*((z2i-z3i)/d23i))+(Fd23i*((z2i-z3i)/d23i))+(Fs2w2i*((z2i-zw2)/d2w2i))+(Fd2w2i*((z2i-zw2)/d2w2i))/MassM2; %8b
%Mass 3 Equations of Motion
d34i=((((z4i-z3i)^(2))+((x4i-x3i))^(2))^(1/2)); %18b
Fs34i=(K7)*(d34i-(zq4-zq3)); %18a
d34dti=(1/d34i)*(((z4i-z3i)*(vz4i))-((z4i-z3i)*(vz3i))+((x4i-x3i)*(vx4i))-((x4i-x3i)*(vx3i))); %19b
Fd34i=(C7)*(d34dti); %19a
d3w3i=((((zw3-z3i)^(2))+((xw3-x3i))^(2))^(1/2)); %20b
Fs3w3i=(K6)*(d3w3i-(xq1-xq0)); %20a
d3w3dti=(1/d3w3i)*(((zw3-z3i)*(vzw3))-((zw3-z3i)*(vz3i))+((xw3-x3i)*(vxw3))-((xw3-x3i)*(vx3i))); %21b
Fd3w3i=(C6)*(d3w3dti); %21a
axM3=zeros(iterations,1);
axM3(1,:)=(-Fs23i*((x2i-x3i)/d23i))+(-Fd23i*((x2i-x3i)/d23i))+(Fs34i*((x3i-x4i)/d34i))+(Fd34i*((x3i-x4i)/d34i))+(Fs3w3i*((x3i-xw3)/d3w3i))+(Fd3w3i*((x3i-xw3)/d3w3i))/MassM3; %15a
azM3=zeros(iterations,1);
azM3(1,:)=(-Fs23i*((z2i-z3i)/d23i))+(-Fd23i*((z2i-z3i)/d23i))+(Fs34i*((z3i-z4i)/d34i))+(Fd34i*((z3i-z4i)/d34i))+(Fs3w3i*((z3i-zw3)/d3w3i))+(Fd3w3i*((z3i-zw3)/d3w3i))/MassM3; %15b
%Mass 4 Equations of Motion
d45i=((((z5i-z4i)^(2))+((x5i-x4i))^(2))^(1/2)); %25b
Fs45i=(K9)*(d45i-(zq5-zq4)); %25a
d45dti=(1/d45i)*(((z5i-z4i)*(vz5i))-((z5i-z4i)*(vz4i))+((x5i-x4i)*(vx5i))-((x5i-x4i)*(vx4i))); %26b
Fd45i=(C9)*(d45dti); %26a
d4w4i=((((zw4-z4i)^(2))+((xw4-x4i))^(2))^(1/2)); %27b
Fs4w4i=(K8)*(d4w4i-(xq1-xq0)); %27a
d4w4dti=(1/d4w4i)*(((zw4-z4i)*(vzw4))-((zw4-z4i)*(vz4i))+((xw4-x4i)*(vxw4))-((xw4-x4i)*(vx4i))); %28b
Fd4w4i=(C8)*(d4w4dti); %28a
axM4=zeros(iterations,1);
axM4(1,:)=(-Fs34i*((x3i-x4i)/d34i))+(-Fd34i*((x3i-x4i)/d34i))+(Fs45i*((x4i-x5i)/d34i))+(Fd45i*((x4i-x5i)/d34i))+(Fs4w4i*((x4i-xw4)/d4w4i))+(Fd4w4i*((x4i-xw4)/d4w4i))/MassM4; %22a
azM4=zeros(iterations,1);
azM4(1,:)=(-Fs34i*((z3i-z4i)/d34i))+(-Fd34i*((z3i-z4i)/d34i))+(Fs45i*((z4i-z5i)/d34i))+(Fd45i*((z4i-z5i)/d34i))+(Fs4w4i*((z4i-zw4)/d4w4i))+(Fd4w4i*((z4i-zw4)/d4w4i))/MassM4; %22b
%Mass 5 Equations of Motion
d56i=((((z6i-z5i)^(2))+((x6i-x5i))^(2))^(1/2)); %32b
Fs56i=(K11)*(d56i-(zq6-zq5)); %32a
d56dti=(1/d56i)*(((z6i-z5i)*(vz6i))-((z6i-z5i)*(vz5i))+((x6i-x5i)*(vx6i))-((x6i-x5i)*(vx5i))); %33b
Fd56i=(C11)*(d56dti); %33a
d5w5i=((((zw5-z5i)^(2))+((xw5-x5i))^(2))^(1/2)); %34b
Fs5w5i=(K10)*(d5w5i-(xq1-xq0)); %34a
d5w5dti=(1/d5w5i)*(((zw5-z5i)*(vzw5))-((zw5-z5i)*(vz5i))+((xw5-x5i)*(vxw5))-((xw5-x5i)*(vx5i))); %35b
Fd5w5i=(C10)*(d5w5dti); %35a
axM5=zeros(iterations,1);
axM5(1,:)=(-Fs45i*((x4i-x5i)/d34i))+(-Fd45i*((x4i-x5i)/d34i))+(Fs56i*((x5i-x6i)/d56i))+(Fd56i*((x5i-x6i)/d56i))+(Fs5w5i*((x5i-xw5)/d5w5i))+(Fd5w5i*((x5i-xw5)/d5w5i))/MassM5; %29a
azM5=zeros(iterations,1);
azM5(1,:)=(-Fs45i*((z4i-z5i)/d34i))+(-Fd45i*((z4i-z5i)/d34i))+(Fs56i*((z5i-z6i)/d56i))+(Fd56i*((z5i-z6i)/d56i))+(Fs5w5i*((z5i-zw5)/d5w5i))+(Fd5w5i*((z5i-zw5)/d5w5i))/MassM5; %29b
%Mass 6 Equations of Motion
d6w6i=((((zw6-z6i)^(2))+((xw6-x6i))^(2))^(1/2)); %41b
Fs6w6i=(K12)*(d6w6i-(xq1-xq0)); %42a
d6w6dti=(1/d6w6i)*(((zw6-z6i)*(vzw6))-((zw6-z6i)*(vz6i))+((xw6-x6i)*(vxw6))-((xw6-x6i)*(vx6i))); %43b
Fd6w6i=(C12)*(d6w6dti); %43a
axM6=zeros(iterations,1);
axM6(1,:)=(-Fs56i*((x5i-x6i)/d56i))+(-Fd56i*((x5i-x6i)/d56i))+(Fs6w6i*((x6i-xw6)/d6w6i))+(Fd6w6i*((x6i-xw6)/d6w6i))/MassM6; %36a
azM6=zeros(iterations,1);
azM6(1,:)=(-Fs56i*((z5i-z6i)/d56i))+(-Fd56i*((z5i-z6i)/d56i))+(Fs6w6i*((z6i-zw6)/d6w6i))+(Fd6w6i*((z6i-zw6)/d6w6i))/MassM6; %36b
% Solve the ODE's with Euler's Method
for n=2:(iterations+1)
x1(n,:)=x1(n-1,:)+vx1(n-1,:)*tStep; % segment 1 x-position
x2(n,:)=x2(n-1,:)+vx2(n-1,:)*tStep; % segment 2 x-position
x3(n,:)=x3(n-1,:)+vx3(n-1,:)*tStep; % segment 3 x-position
x4(n,:)=x4(n-1,:)+vx4(n-1,:)*tStep; % segment 4 x-position
x5(n,:)=x5(n-1,:)+vx5(n-1,:)*tStep; % segment 5 x-position
x6(n,:)=x6(n-1,:)+vx6(n-1,:)*tStep; % segment 6 x-position
z1(n,:)=z1(n-1,:)+vz1(n-1,:)*tStep; % segment 1 z-position
z2(n,:)=z2(n-1,:)+vz2(n-1,:)*tStep; % segment 2 z-position
z3(n,:)=z3(n-1,:)+vz3(n-1,:)*tStep; % segment 3 z-position
z4(n,:)=z4(n-1,:)+vz4(n-1,:)*tStep; % segment 4 z-position
z5(n,:)=z5(n-1,:)+vz5(n-1,:)*tStep; % segment 5 z-position
z6(n,:)=z6(n-1,:)+vz6(n-1,:)*tStep; % segment 6 z-position
vx1(n,:)=vx1(n-1,:)+axM1(n-1,:)*tStep; % segment 1 x-velocity
vx2(n,:)=vx2(n-1,:)+axM2(n-1,:)*tStep; % segment 2 x-velocity
vx3(n,:)=vx3(n-1,:)+axM3(n-1,:)*tStep; % segment 3 x-velocity
vx4(n,:)=vx4(n-1,:)+axM4(n-1,:)*tStep; % segment 4 x-velocity
vx5(n,:)=vx5(n-1,:)+axM5(n-1,:)*tStep; % segment 5 x-velocity
vx6(n,:)=vx6(n-1,:)+axM6(n-1,:)*tStep; % segment 6 x-velocity
vz1(n,:)=vz1(n-1,:)+azM1(n-1,:)*tStep; % segment 1 z-velocity
vz2(n,:)=vz2(n-1,:)+azM2(n-1,:)*tStep; % segment 2 z-velocity
vz3(n,:)=vz3(n-1,:)+azM3(n-1,:)*tStep; % segment 3 z-velocity
vz4(n,:)=vz4(n-1,:)+azM4(n-1,:)*tStep; % segment 4 z-velocity
vz5(n,:)=vz5(n-1,:)+azM5(n-1,:)*tStep; % segment 5 z-velocity
vz6(n,:)=vz6(n-1,:)+azM6(n-1,:)*tStep; % segment 6 z-velocity
%Mass 1 Equations of Motion
d01(n,:)=((((z1(n-1,:))^2)+((x1(n-1,:)-xq0)^2))^(1/2)); %2b
Fs10(n,:)=(-K1)*(d01(n-1,:)-zq1); %2a
d01dt(n,:)=((z1(n-1,:)*vz1(n-1,:))+((x1(n-1,:)-xq0)*vx1(n-1,:)))/d01(n-1,:); %3b
Fd10(n,:)=(-C1)*(d01dt(n-1,:)); %3a
d12(n,:)=((((z2(n-1,:)-z1(n-1,:))^(2))+((x2(n-1,:)-x1(n-1,:)))^(2))^(1/2)); %4b
Fs12(n,:)=(K3)*(d12(n-1,:)-(zq2-zq1)); %4a
d12dt(n,:)=(1/d12(n-1,:))*(((z2(n-1,:)-z1(n-1,:))*(vz2(n-1,:)))-((z2(n-1,:)-z1(n-1,:))*(vz1(n-1,:)))+((x2(n-1,:)-x1(n-1,:))*(vx2(n-1,:)))-((x2(n-1,:)-x1(n-1,:))*(vx1(n-1,:)))); %5b
Fd12(n,:)=(C3)*(d12dt(n-1,:)); %5a
d1w1(n,:)=((((zw1-z1(n-1,:))^(2))+((xw1-x1(n-1,:)))^(2))^(1/2)); %6b
Fs1w1(n,:)=(K2)*(d1w1(n-1,:)-(xq1-xq0)); %6a
d1w1dt(n,:)=(1/d1w1(n-1,:))*(((zw1-z1(n-1,:))*(vzw1))-((zw1-z1(n-1,:))*(vz1(n-1,:)))+((xw1-x1(n-1,:))*(vxw1))-((xw1-x1(n-1,:))*(vx1(n-1,:)))); %7b
Fd1w1(n,:)=(C2)*(d1w1dt(n-1,:)); %7a
%Mass 2 Equat(n,:)ons of Mot(n,:)on
d23(n,:)=((((z3(n-1,:)-z2(n-1,:))^(2))+((x3(n-1,:)-x2(n-1,:)))^(2))^(1/2)); %11b
Fs23(n,:)=(K5)*(d23(n-1,:)-(zq3-zq2)); %11a
d23dt(n,:)=(1/d23(n-1,:))*(((z3(n-1,:)-z2(n-1,:))*(vz3(n-1,:)))-((z3(n-1,:)-z2(n-1,:))*(vz2(n-1,:)))+((x3(n-1,:)-x2(n-1,:))*(vx3(n-1,:)))-((x3(n-1,:)-x2(n-1,:))*(vx2(n-1,:)))); %12b
Fd23(n,:)=(C5)*(d23dt(n-1,:)); %12a
d2w2(n,:)=((((zw2-z2(n-1,:))^(2))+((xw2-x2(n-1,:)))^(2))^(1/2)); %13b
Fs2w2(n,:)=(K4)*(d2w2(n-1,:)-(xq1-xq0)); %13a
d2w2dt(n,:)=(1/d2w2(n-1,:))*(((zw2-z2(n-1,:))*(vzw2))-((zw2-z2(n-1,:))*(vz2(n-1,:)))+((xw2-x2(n-1,:))*(vxw2))-((xw2-x2(n-1,:))*(vx2(n-1,:)))); %14b
Fd2w2(n,:)=(C4)*(d2w2dt(n-1,:)); %14a
%Mass 3 Equat(n,:)ons of Mot(n,:)on
d34(n,:)=((((z4(n-1,:)-z3(n-1,:))^(2))+((x4(n-1,:)-x3(n-1,:)))^(2))^(1/2)); %18b
Fs34(n,:)=(K7)*(d34(n-1,:)-(zq4-zq3)); %18a
d34dt(n,:)=(1/d34(n-1,:))*(((z4(n-1,:)-z3(n-1,:))*(vz4(n-1,:)))-((z4(n-1,:)-z3(n-1,:))*(vz3(n-1,:)))+((x4(n-1,:)-x3(n-1,:))*(vx4(n-1,:)))-((x4(n-1,:)-x3(n-1,:))*(vx3(n-1,:)))); %19b
Fd34(n,:)=(C7)*(d34dt(n-1,:)); %19a
d3w3(n,:)=((((zw3-z3(n-1,:))^(2))+((xw3-x3(n-1,:)))^(2))^(1/2)); %20b
Fs3w3(n,:)=(K6)*(d3w3(n-1,:)-(xq1-xq0)); %20a
d3w3dt(n,:)=(1/d3w3(n-1,:))*(((zw3-z3(n-1,:))*(vzw3))-((zw3-z3(n-1,:))*(vz3(n-1,:)))+((xw3-x3(n-1,:))*(vxw3))-((xw3-x3(n-1,:))*(vx3(n-1,:)))); %21b
Fd3w3(n,:)=(C6)*(d3w3dt(n-1,:)); %21a
%Mass 4 Equat(n,:)ons of Mot(n,:)on
d45(n,:)=((((z5(n-1,:)-z4(n-1,:))^(2))+((x5(n-1,:)-x4(n-1,:)))^(2))^(1/2)); %25b
Fs45(n,:)=(K9)*(d45(n-1,:)-(zq5-zq4)); %25a
d45dt(n,:)=(1/d45(n-1,:))*(((z5(n-1,:)-z4(n-1,:))*(vz5(n-1,:)))-((z5(n-1,:)-z4(n-1,:))*(vz4(n-1,:)))+((x5(n-1,:)-x4(n-1,:))*(vx5(n-1,:)))-((x5(n-1,:)-x4(n-1,:))*(vx4(n-1,:)))); %26b
Fd45(n,:)=(C9)*(d45dt(n-1,:)); %26a
d4w4(n,:)=((((zw4-z4(n-1,:))^(2))+((xw4-x4(n-1,:)))^(2))^(1/2)); %27b
Fs4w4(n,:)=(K8)*(d4w4(n-1,:)-(xq1-xq0)); %27a
d4w4dt(n,:)=(1/d4w4(n-1,:))*(((zw4-z4(n-1,:))*(vzw4))-((zw4-z4(n-1,:))*(vz4(n-1,:)))+((xw4-x4(n-1,:))*(vxw4))-((xw4-x4(n-1,:))*(vx4(n-1,:)))); %28b
Fd4w4(n,:)=(C8)*(d4w4dt(n-1,:)); %28a
%Mass 5 Equat(n,:)ons of Mot(n,:)on
d56(n,:)=((((z6(n-1,:)-z5(n-1,:))^(2))+((x6(n-1,:)-x5(n-1,:)))^(2))^(1/2)); %32b
Fs56(n,:)=(K11)*(d56(n-1,:)-(zq6-zq5)); %32a
d56dt(n,:)=(1/d56(n-1,:))*(((z6(n-1,:)-z5(n-1,:))*(vz6(n-1,:)))-((z6(n-1,:)-z5(n-1,:))*(vz5(n-1,:)))+((x6(n-1,:)-x5(n-1,:))*(vx6(n-1,:)))-((x6(n-1,:)-x5(n-1,:))*(vx5(n-1,:)))); %33b
Fd56(n,:)=(C11)*(d56dt(n-1,:)); %33a
d5w5(n,:)=((((zw5-z5(n-1,:))^(2))+((xw5-x5(n-1,:)))^(2))^(1/2)); %34b
Fs5w5(n,:)=(K10)*(d5w5(n-1,:)-(xq1-xq0)); %34a
d5w5dt(n,:)=(1/d5w5(n-1,:))*(((zw5-z5(n-1,:))*(vzw5))-((zw5-z5(n-1,:))*(vz5(n-1,:)))+((xw5-x5(n-1,:))*(vxw5))-((xw5-x5(n-1,:))*(vx5(n-1,:)))); %35b
Fd5w5(n,:)=(C10)*(d5w5dt(n-1,:)); %35a
%Mass 6 Equat(n,:)ons of Mot(n,:)on
d6w6(n,:)=((((zw6-z6(n-1,:))^(2))+((xw6-x6(n-1,:)))^(2))^(1/2)); %41b
Fs6w6(n,:)=(K12)*(d6w6(n-1,:)-(xq1-xq0)); %42a
d6w6dt(n,:)=(1/d6w6(n-1,:))*(((zw6-z6(n-1,:))*(vzw6))-((zw6-z6(n-1,:))*(vz6(n-1,:)))+((xw6-x6(n-1,:))*(vxw6))-((xw6-x6(n-1,:))*(vx6(n-1,:)))); %43b
Fd6w6(n,:)=(C12)*(d6w6dt(n-1,:)); %43a
% Find segment accelerations
axM1(n,:)=((Fs10(n,:))*((x1(n,:)-xq0)/(d01(n,:))))+((Fd10(n,:))*((x1(n,:)-xq0)/(d01(n,:))))+(Fs12(n,:)*((x1(n,:)-x2(n,:))/d12(n,:)))+(Fd12(n,:)*((x1(n,:)-x2(n,:))/d12(n,:)))+(Fs1w1(n,:)*((x1(n,:)-xw1)/d1w1(n,:)))+(Fd1w1(n,:)*((x1(n,:)-xw1)/d1w1(n,:)))+(FimpM1xapplied)/MassM1; % segment 1 x-acceleration
azM1(n,:)=((Fs10(n,:))*(z1(n,:)/d01(n,:)))+((Fd10(n,:))*((z1(n,:))/(d01(n,:))))+(Fs12(n,:)*((z1(n,:)-z2(n,:))/d12(n,:)))+(Fd12(n,:)*((z1(n,:)-z2(n,:))/d12(n,:)))+(Fs1w1(n,:)*((z1(n,:)-zw1)/d1w1(n,:)))+(Fd1w1(n,:)*((z1(n,:)-zw1)/d1w1(n,:)))+(FimpM1zapplied)/MassM1; % segment 1 z-acceleration
axM2(n,:)=(-Fs12(n,:)*((x1(n,:)-x2(n,:))/d12(n,:)))+(-Fd12(n,:)*((x1(n,:)-x2(n,:))/d12(n,:)))+(Fs23(n,:)*((x2(n,:)-x3(n,:))/d23(n,:)))+(Fd23(n,:)*((x2(n,:)-x3(n,:))/d23(n,:)))+(Fs2w2(n,:)*((x2(n,:)-xw2)/d2w2(n,:)))+(Fd2w2(n,:)*((x2(n,:)-xw2)/d2w2(n,:)))/MassM2; % segment 2 x-acceleration
azM2(n,:)=(-Fs12(n,:)*((z1(n,:)-z2(n,:))/d12(n,:)))+(-Fd12(n,:)*((z1(n,:)-z2(n,:))/d12(n,:)))+(Fs23(n,:)*((z2(n,:)-z3(n,:))/d23(n,:)))+(Fd23(n,:)*((z2(n,:)-z3(n,:))/d23(n,:)))+(Fs2w2(n,:)*((z2(n,:)-zw2)/d2w2(n,:)))+(Fd2w2(n,:)*((z2(n,:)-zw2)/d2w2(n,:)))/MassM2; % segment 2 z-acceleration
axM3(n,:)=(-Fs23(n,:)*((x2(n,:)-x3(n,:))/d23(n,:)))+(-Fd23(n,:)*((x2(n,:)-x3(n,:))/d23(n,:)))+(Fs34(n,:)*((x3(n,:)-x4(n,:))/d34(n,:)))+(Fd34(n,:)*((x3(n,:)-x4(n,:))/d34(n,:)))+(Fs3w3(n,:)*((x3(n,:)-xw3)/d3w3(n,:)))+(Fd3w3(n,:)*((x3(n,:)-xw3)/d3w3(n,:)))/MassM3; % segment 3 x-acceleration
azM3(n,:)=(-Fs23(n,:)*((z2(n,:)-z3(n,:))/d23(n,:)))+(-Fd23(n,:)*((z2(n,:)-z3(n,:))/d23(n,:)))+(Fs34(n,:)*((z3(n,:)-z4(n,:))/d34(n,:)))+(Fd34(n,:)*((z3(n,:)-z4(n,:))/d34(n,:)))+(Fs3w3(n,:)*((z3(n,:)-zw3)/d3w3(n,:)))+(Fd3w3(n,:)*((z3(n,:)-zw3)/d3w3(n,:)))/MassM3; % segment 3 z-acceleration
axM4(n,:)=(-Fs34(n,:)*((x3(n,:)-x4(n,:))/d34(n,:)))+(-Fd34(n,:)*((x3(n,:)-x4(n,:))/d34(n,:)))+(Fs45(n,:)*((x4(n,:)-x5(n,:))/d34(n,:)))+(Fd45(n,:)*((x4(n,:)-x5(n,:))/d34(n,:)))+(Fs4w4(n,:)*((x4(n,:)-xw4)/d4w4(n,:)))+(Fd4w4(n,:)*((x4(n,:)-xw4)/d4w4(n,:)))/MassM4; % segment 4 x-acceleration
azM4(n,:)=(-Fs34(n,:)*((z3(n,:)-z4(n,:))/d34(n,:)))+(-Fd34(n,:)*((z3(n,:)-z4(n,:))/d34(n,:)))+(Fs45(n,:)*((z4(n,:)-z5(n,:))/d34(n,:)))+(Fd45(n,:)*((z4(n,:)-z5(n,:))/d34(n,:)))+(Fs4w4(n,:)*((z4(n,:)-zw4)/d4w4(n,:)))+(Fd4w4(n,:)*((z4(n,:)-zw4)/d4w4(n,:)))/MassM4; % segment 4 z-acceleration
axM5(n,:)=(-Fs45(n,:)*((x4(n,:)-x5(n,:))/d34(n,:)))+(-Fd45(n,:)*((x4(n,:)-x5(n,:))/d34(n,:)))+(Fs56(n,:)*((x5(n,:)-x6(n,:))/d56(n,:)))+(Fd56(n,:)*((x5(n,:)-x6(n,:))/d56(n,:)))+(Fs5w5(n,:)*((x5(n,:)-xw5)/d5w5(n,:)))+(Fd5w5(n,:)*((x5(n,:)-xw5)/d5w5(n,:)))/MassM5; % segment 5 x-acceleration
azM5(n,:)=(-Fs45(n,:)*((z4(n,:)-z5(n,:))/d34(n,:)))+(-Fd45(n,:)*((z4(n,:)-z5(n,:))/d34(n,:)))+(Fs56(n,:)*((z5(n,:)-z6(n,:))/d56(n,:)))+(Fd56(n,:)*((z5(n,:)-z6(n,:))/d56(n,:)))+(Fs5w5(n,:)*((z5(n,:)-zw5)/d5w5(n,:)))+(Fd5w5(n,:)*((z5(n,:)-zw5)/d5w5(n,:)))/MassM5; % segment 5 z-acceleration
axM6(n,:)=(-Fs56(n,:)*((x5(n,:)-x6(n,:))/d56(n,:)))+(-Fd56(n,:)*((x5(n,:)-x6(n,:))/d56(n,:)))+(Fs6w6(n,:)*((x6(n,:)-xw6)/d6w6(n,:)))+(Fd6w6(n,:)*((x6(n,:)-xw6)/d6w6(n,:)))/MassM6; % segment 6 x-acceleration
azM6(n,:)=(-Fs56(n,:)*((z5(n,:)-z6(n,:))/d56(n,:)))+(-Fd56(n,:)*((z5(n,:)-z6(n,:))/d56(n,:)))+(Fs6w6(n,:)*((z6(n,:)-zw6)/d6w6(n,:)))+(Fd6w6(n,:)*((z6(n,:)-zw6)/d6w6(n,:)))/MassM6; % segment 6 z-acceleration
end
% % Plot results
% subplot(3,1,1)
% hold on;
% plot(t',x1,'r')
% plot(t',x2,'m')
% ylabel('Position (m)')
% title('Position, Velocity, & Acceleration as a Function of Time')
% legend('Cart 1','Cart 2')
% subplot(3,1,2)
% hold on;
% plot(t',v1,'b')
% plot(t',v2,'c')
% ylabel('Velocity (m/s)')
% legend('Cart 1','Cart 2')
% subplot(3,1,3)
% hold on;
% plot(t',a1,'g')
% plot(t',a2,'y')
% ylabel('Acceleration (m/s^2)')
% xlabel('time (iterations)')
% legend('Cart 1','Cart 2')
toc
0 Comments
Accepted Answer
0 Comments
More Answers (0)
See Also
Categories
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!