m=5000;%mass of milling machine in kg
c=1000;%damping coefficient of milling machine in N-s/m
k=10^6;%spring constant of milling machine in N/m

t2=0:0.01:10; % time span and fixed time step
x0=[0;0]; % initial conditions
A=[0 1;-k/m -c/m]; % state matrix A
B=[0 0;1 0]; % input matrix B
C=[1 0;0 1]; % output matrix C
D=[0 0;0 0]; % direct transmission matrix D
% input u must have the same number of rows as t and the same number of columns as B 
[l,m]=size(t2);
t2(m)=[];
u=zeros(2,m-1);
i=1;
y=2/1000*sin(200*pi()*t2);
y_prime=2/1000*200*pi()*cos(200*pi()*t2);
while(i<m)
u(1,i)=(2000*t2(i)*heaviside(t2(i))-2000*t2(i)*heaviside(t2(i)-2)+4000*heaviside(t2(i)-2)-4000*heaviside(t2(i)-4)+(-4000*t2(i)+20000)*heaviside(t2(i)-4)-(-4000*t2(i)+20000)*heaviside(t2(i)-5)+c*y_prime(i)+k*y(i))/m;
i=i+1;
    
end 

x=lsim(ss(A,B,C,D),u,t2,x0); 
x1=x(:,1); % extract x1
x2=x(:,2); % extract x2

%figure(1)

%plot(t2,x1)
%title('Position of Milling Machine vs Time-Statespace Solution')
%xlabel('Time (s)')
%ylabel('Position of Milling Machine (m)')

%figure(2)
%plot(t2,x2)
%title('Velocity of Milling Machine vs Time-Statespace Solution')
%xlabel('Time (s)')
%ylabel('Velocity of Milling Machine (m)')


clear x s t

m=5000;%mass of milling machine in kg
c=1000;%damping coefficient of milling machine in N-s/m
k=10^6;%spring constant of milling machine in N/m
a=0; %initial position in x
b=0; %initial velocity in x
d=0; %initial position in y

syms s t

%enter y(t)

y=2/1000*sin(200*pi()*t);

%Resonant frequency y input
%y=2/1000*sin(sqrt(k/m)*t);

%F in Direction of x

F=2000*t*heaviside(t)-2000*t*heaviside(t-2)+4000*heaviside(t-2)-4000*heaviside(t-4)+(-4000*t+20000)*heaviside(t-4)-(-4000*t+20000)*heaviside(t-5);

%F opposed to x direction

%F=-(2000*t*heaviside(t)-2000*t*heaviside(t-2)+4000*heaviside(t-2)-4000*heaviside(t-4)+(-4000*t+20000)*heaviside(t-4)-(-4000*t+20000)*heaviside(t-5));



%Find laplace transform of F

laplace_F=laplace(F,t,s);

%Find Laplace transform of Y with Initial conditions

laplace_y=laplace(y,t,s);

laplace_y_with_ics=c*(s*laplace_y-d)+k*laplace_y;

%Represent RHS of equation of motion expression
rhs_with_ics=laplace_y_with_ics+laplace_F+m*s*a+m*b+c*a;

%Solve equation of motion expression for X(s)
x(s)=rhs_with_ics/(m*s^2+c*s+k);

%Take the inverse Laplace transform of X(s)
x(t)=ilaplace(x(s));

%Set the number of increments to be considered for the model,
%representative of the resolution of the model, with a higher value
%indicating a higher resoluion
number_of_increments=1000;

%Set the max time of interest for the equation of motion
max_time=10;

%create linearly spaced row vector to be utilized to determine x(t) for
%each element
t_sub=linspace(0,max_time,number_of_increments);

%Probably a better way of converting x(t) to a numeric array but this works 
syms x1(t)
x1(t)=subs(x,{t},{t_sub});

out_sym=formula(x1);

numeric_x_t=double(out_sym);

%%now have x(t) as a numeric array

%built in matlab function to find dx
dx=diff(numeric_x_t);

%ditto but for dt
dt=diff(t_sub);

%Find velocity 
v_t=dx./dt;


%Positon vs time plot
figure(1)

plot(t_sub,numeric_x_t)
title('Position of Milling Machine vs Time')
xlabel('Time (s)')
ylabel('Position of Milling Machine (m)')
axis([0 max_time -.005 .005])
hold on
plot(t2,x1, '--')
legend('Laplace Transform Solutuion','State Space Solution')
hold off