clear
clc
close all
m_b = 0.3;
m_f = 0.025;
m_r = 0.1;
l_f = 0.05;
l_r = 0.13;
k_tf = 4;
k_tr = 10;
k_f = 5;
k_r = 20;
zeta_1 = 0.06;
zeta_2 = 0.02;
y_0 = 0.01;
y_dot_0 = 0.5;
theta_f_0 = pi/15;
theta_f_dot_0 = -0.01;
theta_r_0 = pi/5;
theta_r_dot_0 = 0.4;
t = 0:0.1:10;
T_f = 0.1*sin(2*pi*t-0.1);
T_r = 200*sin(10*pi*t);
M = [m_b+m_f+m_r m_f*l_f/2 m_r*l_r/2;m_f*l_f/2 m_f*l_f^2/3 0;m_r*l_r/2 0 m_r*l_r^2/3];
K = 2*[k_f+k_r k_f*l_f k_r*l_r;k_f*l_f k_f*l_f^2+k_tf 0;k_r*l_r 0 k_r*l_r^2+k_tr];
F = [T_f+T_r;T_f;T_r];
zeta = [zeta_1;zeta_2];
K_tilde = inv(sqrtm(M))*K*inv(sqrtm(M));
[P,Lambda]=eigs(K_tilde);
S = inv(sqrtm(M))*P;
for i=1:length(t)
R(:,i) = (P'*inv(sqrtm(M)))*F(:,i);
end
omega = sqrtm(Lambda);
AB_constants = [1/(2*omega(1,1)) omega(1,1)/2;1/(2*omega(2,2)) omega(2,2)/2];
AB = AB_constants\zeta;
alpha = AB(1);
beta = AB(2);
C = alpha*M + beta*K;
for i=1:length(zeta(:,1))
omega_d(i) = omega(i,i)*sqrt(1-(zeta(i))^2);
end
X_0 = [y_0;theta_f_0;theta_r_0];
X_dot_0 = [y_dot_0;theta_f_dot_0;theta_r_dot_0];
r_0 = (S^-1)*X_0;
r_dot_0 = (S^-1)*X_dot_0;
for j=1:length(r_0)
R_du(j,:) = DampedSystem_ArbitraryForce_DuhamelIntegral_Function(t,t,R(j,:),0,omega(j,j));
end
for j=1:length(r_0)
r(j,:) = (exp(-zeta(j)*omega(j,j)*t)*(cos(omega_d(j,j)*t)+(zeta(j)/sqrt(1-(zeta(j))^2))*sin(omega_d(j,j)*t))*r_0(j,:)+((1/(omega_d(j,j)))*exp(-zeta(j)*omega(j,j)*t)*sin(omega_d(j,j)*t))*r_dot_0(j,:)+R_du(j,:));
end
for i=1:length(t)
X(:,i) = S*r(:,i);
end
figure(1)
subplot(2,1,1)
plot(t,X(1,:))
hold on
subplot(2,1,2)
Error in Homework6Prob2 (line 77)
r(j,:) =
(exp(-zeta(j)*omega(j,j)*t)*(cos(omega_d(j,j)*t)+(zeta(j)/sqrt(1-(zeta(j))^2))*sin(omega_d(j,j)*t))*r_0(j,:)+((1/(omega_d(j,j)))*exp(-zeta(j)*omega(j,j)*t)*sin(omega_d(j,j)*t))*r_dot_0(j,:)+R_du(j,:));
>>