clc
clear all
%Set motor
R=5
L=1.08e-2
Kt = 1.75
Kv = 0.2865 
Jm  =0.75e-2
Dm  =4.01e-4
%gearbox
Jg  = 0.21e-2
r   = 11.5
Dg  = 3e-4
%ballscrew
Jb  =0.2
Db  =0.1e-4
l   =0.008 
%connecting shaft
Cs  = 10
Ks  = 1684.5

%connect  model
Cc=10
Kk=1e7


%Finite element analysis of the rail
E=2e11;           
LL=9.21;            
I0=5.6e-6          
Ax0=0.009334 
rho=7850
mesh=20
a0=1
a1=0.6
fc=0.05 %Finite coeffeciency

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=1:mesh;
    elementNodes(i,1)=i;
    elementNodes(i,2)=i+1;
    I=I0;
    Ax=Ax0;
   xx(elementNodes(i,1))=i*(LL/mesh)
   xx(elementNodes(i,2))=(i+1)*(LL/mesh)
    if elementNodes(i,2)>12
       I=I0*[1-(1/4)^2*(xx(elementNodes(i,1))-5.0370)/(LL-5.0370)];
       Ax=Ax0*(1-(((1/4))*((xx(elementNodes(i,1)))-5.0370))/(LL-5.0370));
    end
   Section(i)=Ax;
   Moment(i)=I;
   Fe(i)=276*(Ax/Ax0)*9.8*fc
end


le=xx(elementNodes(i,2))-xx(elementNodes(i,1))

Me=rho*Ax*le/420*[156,22*le,54,-13*le;...
                22*le,4*le^2,13*le,-3*le^2;...
                54,13*le,156,-22*le;...
                -13*le,-3*le^2,-22*le,4*le^2];        

Ke=E*I/le^3*[12,6*le,-12,6*le;...
            6*le,4*le^2,-6*le,2*le^2;...
            -12,-6*le,12,-6*le;...
            6*le,2*le^2,-6*le,4*le^2];
            
            
Kgg=zeros(2*mesh+2,2*mesh+2);
Mgg=zeros(2*mesh+2,2*mesh+2);
Fgg=zeros(2*mesh+2,1);
for i=1:mesh
    Kgg(i*2-1:i*2+2,i*2-1:i*2+2)=Kgg(i*2-1:i*2+2,i*2-1:i*2+2)+Ke;
    Mgg(i*2-1:i*2+2,i*2-1:i*2+2)=Mgg(i*2-1:i*2+2,i*2-1:i*2+2)+Me;
    Fgg(i*2-1,1)=Fe(i);
  end

Kaa=Kgg;
Maa=Mgg;
Faa=Fgg;
spc=1:2;
Kaa(spc,:)=[];
Kaa(:,spc)=[];
Maa(spc,:)=[];
Maa(:,spc)=[];
Faa(spc,:)=[];

Caa=a0*Maa+a1*Kaa;        
Saa=inv(Maa);              

%Position set, based on the mesh number
excite_node1=[20];            
ex_dof1=2*excite_node1-1;            
ex_load1=zeros(mesh*2,1);
ex_load1(ex_dof1)=1;            

excite_node2=[18];           
ex_dof2=2*excite_node2-1;           
ex_load2=zeros(mesh*2,1);
ex_load2(ex_dof2)=1;            

excite_node3=[15];            
ex_dof3=2*excite_node3-1;            
ex_load3=zeros(mesh*2,1);
ex_load3(ex_dof3)=1;