clc
clf
clear all
global LIB
LIB=zeros(3,3);
DUMMY=zeros(3,1);
dum=zeros(6,6);
LII=zeros(6,6);
LI=zeros(6,6);

% Leer en memoria o generar la seņal que se alimentara.
% consta de un vector de tx6, conformado por
% vector de posicion espacial de la plataforma (x,y,z) de tx3, referido a SSI
% y un vector de posicion angular de la plataforma (psi,theta,phi) de tx3, 
% que se alimenta a libs en cada iteracion.

muestras= 500;                  % Muestras. 
mps= 100 ;                        % Muestras por segundo.
segundos= muestras / mps;
Segundos_Reales=segundos
periodo_01=1;                       % Segundos
frec_01=1/periodo_01;               % Hertz
periodo_02=.5;                      % Segundos
frec_02=1/periodo_02;               % Hertz
amplitud=0.15                       % Rad/seg

rad2deg=pi/180;
psi=0;
theta=0;
phi=0;
% dtime=1/50;
Ancho_Linea_Actuador=4;
Ancho_Linea_Plat_SUP=2;
Ancho_Linea_Plat_INF=2;
Color_Linea_Actuador='g';
Color_Linea_Plat_SUP='r';
Color_Linea_Plat_INF='b';
Estilo_Linea_Actuador='-';
Estilo_Linea_Plat_SUP='-';
Estilo_Linea_Plat_INF='-';

whitebg('k');
for k=1:6
    jack(k)=line('color',Color_Linea_Actuador,'linestyle',Estilo_Linea_Actuador,'EraseMode','normal','xdata',[],'ydata',[],'zdata',[],'LineWidth',Ancho_Linea_Actuador);
    PlatSup(k)=line('color',Color_Linea_Plat_SUP,'linestyle',Estilo_Linea_Plat_SUP,'EraseMode','normal','xdata',[],'ydata',[],'zdata',[],'LineWidth',Ancho_Linea_Plat_SUP);
    PlatInf(k)=line('color',Color_Linea_Plat_INF,'linestyle',Estilo_Linea_Plat_INF,'EraseMode','normal','xdata',[],'ydata',[],'zdata',[],'LineWidth',Ancho_Linea_Plat_INF);
end
title( 'Plataforma Stewart de Movimiento con 6 GDL, Comportamiento en Tiempo Real')
% hold on;
axis([-2 2 -2 2 -0 4]);
axis equal;
view(3);
% H=gcf;


AAIS=[1.46647, 0.09525 , 0
      1.46647,-0.09525 , 0
     -0.65075,-1.3176  , 0
     -0.81572,-1.22237 , 0
     -0.81572, 1.22237 , 0
     -0.65075, 1.3176  , 0];

AAIST=AAIS';
  
BBII=[ 1.21717, 1.524 , 0
       1.21717,-1.524 , 0
       0.71124,-1.8161, 0
      -1.9284 ,-0.2921, 0
      -1.9284 , 0.2921, 0
       0.71124, 1.8161, 0];
   
BBIIT=BBII';   
   

Length_neutral=2.34;

tic

for i=1:muestras

    (frec_01*(360/mps)*rad2deg*i);
SSI(1,i)=(0.4*sin((frec_01*(360/mps)*rad2deg*i)+2));
SSI(2,i)=(0.4*sin((frec_01*(360/mps)*rad2deg*i)+10));
SSI(3,i)=+1.8363+(0.4*sin((frec_01*(360/mps)*rad2deg*i)+30));
    
omega(1,i)= (amplitud*sin((frec_02*(360/mps)*rad2deg*i)+5));
omega(2,i)=	(amplitud*sin((frec_02*(360/mps)*rad2deg*i)+10));                      % (.15*sin(i/5));
omega(3,i)=	(amplitud*sin((frec_02*(360/mps)*rad2deg*i)+15));                      % (.05*sin(i/5));

% end


% for i=1:muestras 

    LIB=real(lib(omega(:,i)));
    DUMMY=LIB*AAIST;
    dum=[(DUMMY(:,1)+SSI(:,i)),(DUMMY(:,2)+SSI(:,i)),(DUMMY(:,3)+SSI(:,i)),(DUMMY(:,4)+SSI(:,i)),(DUMMY(:,5)+SSI(:,i)),(DUMMY(:,6)+SSI(:,i))]';
    LII=dum'-BBIIT;
    LI(:,i)=[ sqrt(LII(1,1)*LII(1,1)+LII(2,1)*LII(2,1)+LII(3,1)*LII(3,1));
        sqrt(LII(1,2)*LII(1,2)+LII(2,2)*LII(2,2)+LII(3,2)*LII(3,2));
        sqrt(LII(1,3)*LII(1,3)+LII(2,3)*LII(2,3)+LII(3,3)*LII(3,3));
        sqrt(LII(1,4)*LII(1,4)+LII(2,4)*LII(2,4)+LII(3,4)*LII(3,4));
        sqrt(LII(1,5)*LII(1,5)+LII(2,5)*LII(2,5)+LII(3,5)*LII(3,5));
        sqrt(LII(1,6)*LII(1,6)+LII(2,6)*LII(2,6)+LII(3,6)*LII(3,6))];
    
    % set(jack,'xdata',[BBII(1,1) dum(1,1) BBII(2,1) dum(2,1) BBII(3,1) dum(3,1) BBII(4,1) dum(4,1) BBII(5,1) dum(5,1) BBII(6,1) dum(6,1)],'ydata',[BBII(1,2) dum(1,2) BBII(2,2) dum(2,2) BBII(3,2) dum(3,2) BBII(4,2) dum(4,2) BBII(5,2) dum(5,2) BBII(6,2) dum(6,2)] ,'zdata',[BBII(1,3) dum(1,3) BBII(2,3) dum(2,3) BBII(3,3) dum(3,3) BBII(4,3) dum(4,3) BBII(5,3) dum(5,3) BBII(6,3) dum(6,3)])
    
    set(jack(1),'xdata',[BBII(1,1) dum(1,1)],'ydata',[BBII(1,2) dum(1,2)] ,'zdata',[BBII(1,3) dum(1,3)])
    set(jack(2),'xdata',[BBII(2,1) dum(2,1)],'ydata',[BBII(2,2) dum(2,2)] ,'zdata',[BBII(2,3) dum(2,3)])
    set(jack(3),'xdata',[BBII(3,1) dum(3,1)],'ydata',[BBII(3,2) dum(3,2)] ,'zdata',[BBII(3,3) dum(3,3)])
    set(jack(4),'xdata',[BBII(4,1) dum(4,1)],'ydata',[BBII(4,2) dum(4,2)] ,'zdata',[BBII(4,3) dum(4,3)])
    set(jack(5),'xdata',[BBII(5,1) dum(5,1)],'ydata',[BBII(5,2) dum(5,2)] ,'zdata',[BBII(5,3) dum(5,3)])
    set(jack(6),'xdata',[BBII(6,1) dum(6,1)],'ydata',[BBII(6,2) dum(6,2)] ,'zdata',[BBII(6,3) dum(6,3)])

    set(PlatInf(1),'xdata',[BBII(1,1) BBII(2,1)],'ydata',[BBII(1,2) BBII(2,2)] ,'zdata',[BBII(1,3) BBII(2,3)])
    set(PlatInf(2),'xdata',[BBII(2,1) BBII(3,1)],'ydata',[BBII(2,2) BBII(3,2)] ,'zdata',[BBII(2,3) BBII(3,3)])
    set(PlatInf(3),'xdata',[BBII(3,1) BBII(4,1)],'ydata',[BBII(3,2) BBII(4,2)] ,'zdata',[BBII(3,3) BBII(4,3)])
    set(PlatInf(4),'xdata',[BBII(4,1) BBII(5,1)],'ydata',[BBII(4,2) BBII(5,2)] ,'zdata',[BBII(4,3) BBII(5,3)])
    set(PlatInf(5),'xdata',[BBII(5,1) BBII(6,1)],'ydata',[BBII(5,2) BBII(6,2)] ,'zdata',[BBII(5,3) BBII(6,3)])
    set(PlatInf(6),'xdata',[BBII(6,1) BBII(1,1)],'ydata',[BBII(6,2) BBII(1,2)] ,'zdata',[BBII(6,3) BBII(1,3)])

    set(PlatSup(1),'xdata',[dum(1,1) dum(2,1)],'ydata',[dum(1,2) dum(2,2)] ,'zdata',[dum(1,3) dum(2,3)])
    set(PlatSup(2),'xdata',[dum(2,1) dum(3,1)],'ydata',[dum(2,2) dum(3,2)] ,'zdata',[dum(2,3) dum(3,3)])
    set(PlatSup(3),'xdata',[dum(3,1) dum(4,1)],'ydata',[dum(3,2) dum(4,2)] ,'zdata',[dum(3,3) dum(4,3)])
    set(PlatSup(4),'xdata',[dum(4,1) dum(5,1)],'ydata',[dum(4,2) dum(5,2)] ,'zdata',[dum(4,3) dum(5,3)])
    set(PlatSup(5),'xdata',[dum(5,1) dum(6,1)],'ydata',[dum(5,2) dum(6,2)] ,'zdata',[dum(5,3) dum(6,3)])
    set(PlatSup(6),'xdata',[dum(6,1) dum(1,1)],'ydata',[dum(6,2) dum(1,2)] ,'zdata',[dum(6,3) dum(1,3)])
    drawnow
    F(i)=getframe;
    
end

Segundos_Simulados=toc