A = load("torque_for_Sid_60s.mat");
T_a = A.torque_60s(1:5:600001);
y0 = [0;0;0;0;0;104.719;0;0;0;0;0;104.719/3];
Torque = griddedInterpolant(time, T_a);
[t,y] = ode45(@(t,y) spur1(t,y,Torque),tspan,y0);
[~,TE(i),KS(i)] = spur1(t(i),y(i,:),Torque);
ylabel('Transmission Error TE (meter)')
title('Transmission Error vs time')
Y = fft(detrend(TE), NFFT)/L;
f = Fs/2*linspace(0,1,NFFT/2+1);
semilogy(f, 2*abs(Y(1:NFFT/2+1)))
title('FFT of TE vs frequency')
function [yp,TE,KS] = spur1(t,y,Torque)
Opp_Torque = 68.853/1000;
e= 20e-6 + 20e-8*sin(2*pi*Freq*t);
ks = 0.9e8 + 0.9e6*sin(2*pi*Freq*t);
Cs = 2*0.1*sqrt((ks*I_a*I_p)/(I_a*R_a + I_p*R_p)) + 2e-2*0.1*sqrt((ks*I_a*I_p)/(I_a*R_a + I_p*R_p))*sin(2*pi*Freq*t);
TE = -y(11)*R_p + y(5)*R_a ;
Fy = cos(beta)*(Cs*(cos(beta)*(y(2) - y(8) + R_a*y(6) - R_p*y(12)) + sin(beta)*(y(4) - y(10))) + ks*(cos(beta)*(y(1) - y(7) + R_a*y(5) - R_p*y(11)) + sin(beta)*(y(3) - y(9))));
Fz = sin(beta)*(Cs*(cos(beta)*(y(2) - y(8) + R_a*y(6) - R_p*y(12)) + sin(beta)*(y(4) - y(10))) + ks*(cos(beta)*(y(1) - y(7) + R_a*y(5) - R_p*y(11)) + sin(beta)*(y(3) - y(9))));
yp(2) = (-Fy - k_ay*y(1) - c_ay*y(2))/m_a;
yp(4) = (-Fz - k_az*y(3) - c_az*y(4))/m_a;
yp(6) = (Torque - Fy*R_a)/I_a;
yp(8) = (Fy - k_py*y(7) - c_py*y(8))/m_p;
yp(10) = (Fz - k_pz*y(9) - c_pz*y(10))/m_p;
yp(12) = (-Opp_Torque*i_r + Fy*R_p)/I_p;