%% Controlar Fanuc com sensores

close all
clear all
clc

%Data de inicio 13-Junho-2011
%Nuno Silva

robCOMM=ligar(); %Conexão com o manipulador
move_Joints(0,0,0,0,0,0,robCOMM);
%% Base de dados

%Timestamp
% database.timestamp=0;

%Modulo 1
% database.id.mod1.acel.data=zeros(1,3);
% database.id.mod1.acel.dataPlot=zeros(1,3);
% database.id.mod1.acel.scale=0;
% database.id.mod1.gyro.data=zeros(1,2);
% database.id.mod1.gyro.dataPlot=zeros(1,2);
% database.id.mod1.gyro.scale=0;
% database.id.mod1.timestamp=0;
% database.id.mod1.N=0;

%Modulo 2
database.id.mod2.acel.data=zeros(1,3);
database.id.mod2.acel.dataPlot=zeros(1,3);
database.id.mod2.acel.scale=0;
database.id.mod2.gyro.data=zeros(1,2);
database.id.mod2.gyro.dataPlot=zeros(1,2);
database.id.mod2.gyro.scale=0;
database.id.mod2.timestamp=0;
database.id.mod2.N=0;
database.id.mod2.acel.P=zeros(3,3,3);
database.id.mod2.acel.Xest=zeros(3,1,3);
database.id.mod2.acel.K=[];

%Modulo 3
database.id.mod3.acel.data=zeros(1,3);
database.id.mod3.acel.dataPlot=zeros(1,3);
database.id.mod3.acel.scale=0;
database.id.mod3.gyro.data=zeros(1,2);
database.id.mod3.gyro.dataPlot=zeros(1,2);
database.id.mod3.gyro.scale=0;
database.id.mod3.timestamp=0;
database.id.mod3.N=0;
database.id.mod3.acel.P=zeros(3,3,3);
database.id.mod3.acel.Xest=zeros(3,1,3);
database.id.mod3.acel.K=[];

counter=0; %Contador da base de dados
counterPlot=1; %Contador do plot

% tempo_estoiro_tmr1=0.065535; %conversão para seg
% tempo_estoiro_tmr1=0.049664; %conversão para segmatlab.mat
tempo_estoiro_tmr1=0.02; %conversão para seg

%% Diferencial

%Dados
l1=1;
l2=1;

%Matriz inicial
p0_T_p1=[1 0 0 0
         0 1 0 l1
         0 0 1 0
         0 0 0 1];

%Matriz inicial para os acelerometros
p0_T_p1_a=p0_T_p1;

log_pos_s0=[0 0 0];

%% Definição da porta
%Windows 'com1'...
%Linux /dev/ttyUSB0
SerialHandle=serial('/dev/ttyUSB0','Baudrate',57600,'Databits',8,'StopBits',1);

fopen(SerialHandle); %Open serial port

%% Aquisição de dados
while (counter<42000)

    a=0;
    while (a==0)
        
        [start,count]=fread(SerialHandle,2,'uint8');
        if (count==2 && start(1)==170 && start(2)==85) %Quando esta condição for verdade é porque está no inicio da mensagem
            a=1;

            [timetag,count]=fread(SerialHandle,3,'uint8'); %Get Time stamp
            [info1,count]=fread(SerialHandle,2,'uint8'); % Get ID and scale for next sensor
            data1=fread(SerialHandle,3,'int16'); % Get Accel data
            
            [info2,count]=fread(SerialHandle,2,'uint8'); % Get ID and scale for next sensor
            [data2,count]=fread(SerialHandle,3,'int16'); %Get Gyro data
       
            nfo=getnfo(info1); % Get ID and scale in decimal

            timestmp=cnv32(timetag); %Convert 3 bytes to decimal (Time stamp)
            
            counter=counter+1; %Contagem de recepção de dados
            
            if database.id.(['mod' num2str(nfo(1))]).N(end)==0
                database.id.(['mod' num2str(nfo(1))]).acel.data(end,:)=(data1'.*2)./32767; %Guarda os valores do acelerometro
                database.id.(['mod' num2str(nfo(1))]).gyro.data(end,:)=(data2(1:2,1)'.*3.3)./(1023.*0.0083); %Guarda os valores do gyro
                database.id.(['mod' num2str(nfo(1))]).timestamp(end)=timestmp;
                database.id.(['mod' num2str(nfo(1))]).N(end)=counter;
            else
                database.id.(['mod' num2str(nfo(1))]).acel.data(end+1,:)=(data1'.*2)./32767; %Guarda os valores do acelerometro
                database.id.(['mod' num2str(nfo(1))]).gyro.data(end+1,:)=(data2(1:2,1)'.*3.3)./(1024.*0.0083); %Guarda os valores do gyro
                database.id.(['mod' num2str(nfo(1))]).timestamp(end+1)=timestmp;
                database.id.(['mod' num2str(nfo(1))]).N(end+1)=counter;
            end
            
            %Diferencial
            if counter>140
                
%                 acel_ab2=sqrt(sum(database.id.mod2.acel.data(end,:).^2)); 
%                 
%                 %Parado
% %                 if acel_ab2>9.5 && acel_ab2<10
%                 if acel_ab2>0.96 && acel_ab2<1.04
%                     
%                     teta2=atan2(database.id.mod2.acel.data(end,1),database.id.mod2.acel.data(end,3))*180/pi; %Eixo Y
%                     fi2=atan2(database.id.mod2.acel.data(end,2),sqrt(database.id.mod2.acel.data(end,3)^2+database.id.mod2.acel.data(end,1)^2))*180/pi; %Eixo X
%                     
%                     acc_vec=[-fi2 -teta2 0];
%                     
%                     %Soma da posição da junta
%                     log_pos_s0=[log_pos_s0
%                                 acc_vec];
                    
%                 else
                    
%                     if nfo(1)==2
%                         ws0=[-(database.id.mod2.gyro.data(end,1)+1.22) -(database.id.mod2.gyro.data(end,2)+2.12) 0];
%                     elseif nfo(1)==3
%                         ws0=[0 0 -(database.id.mod3.gyro.data(end,1)+1.204)]*pi/180;
%                     end

%                       junta1=ws0'*((database.id.(['mod' num2str(nfo(1))]).timestamp(end)-database.id.(['mod' num2str(nfo(1))]).timestamp(end-1))*tempo_estoiro_tmr1);
% 
%                       %Soma da posição da junta
%                       log_pos_s0=[log_pos_s0
%                                   log_pos_s0(end,:)+junta1'];
                    
                    %Outro metodo 3D
                    if nfo(1)==2
                        ws0=[(database.id.mod2.gyro.data(end,2)-bias_y) 0 (database.id.mod2.gyro.data(end,1)-bias_x)];
                    elseif nfo(1)==3
%                         ws0=[0 0 -(database.id.mod3.gyro.data(end,1)+1.204)]*pi/180;
                          ws0=[0 0 0];
                    end

                    teta2=-atan2(database.id.mod2.acel.data(end,2),database.id.mod2.acel.data(end,1))*180/pi; %Eixo Y
                    
                    rot_y=roty(teta2);
                    ws0=rot_y(1:3,1:3)*ws0';
                    
                    junta1=ws0'*((database.id.(['mod' num2str(nfo(1))]).timestamp(end)-database.id.(['mod' num2str(nfo(1))]).timestamp(end-1))*tempo_estoiro_tmr1);
 
                    acel_ab2=sqrt(sum(database.id.mod2.acel.data(end,:).^2))*9.81;
                    
                    
                     %Soma da posição da junta
                     log_pos_s0=[log_pos_s0
                                 log_pos_s0(end,:)+junta1];
                     
                     if acel_ab2>9.5 && acel_ab2<10
                        fi2=-atan2(database.id.mod2.acel.data(end,3),sqrt(database.id.mod2.acel.data(end,1)^2+database.id.mod2.acel.data(end,2)^2))*180/pi; %Eixo X X
                        log_pos_s0(end,1)=fi2;
                     end
%%%%%%%%%%%Não interessa
%                     %Para efeitos de animação
%                     %Calculo do novo ponto
%                     pos_s0=(p0_T_p1(1:3,1:3)*ws0')*((database.id.(['mod' num2str(nfo(1))]).timestamp(end)-database.id.(['mod' num2str(nfo(1))]).timestamp(end-1))*tempo_estoiro_tmr1);
%                     p0_T_p1=RPY(pos_s0(3,1),pos_s0(2,1),pos_s0(1,1))*p0_T_p1;
%                     
%                     t1=atan2(p0_T_p1(1,4),p0_T_p1(2,4))*180/pi;
%                     t2=atan2(p0_T_p1(3,4),p0_T_p1(2,4))*180/pi;
%                     t2=asin((p0_T_p1(3,4)-l1)/l2)*180/pi;
%%%%%%%%%%%%%%%%%%%%%%%%
%                 end
                
                if rem(counter,250)==0
%                         move_Joints(t1,0,t2,teta2,0,0,robCOMM);
                    move_Joints(log_pos_s0(end,3),0,log_pos_s0(end,1),teta2,0,0,robCOMM);
                end
                
            elseif counter<140
                
                bias_x=mean(database.id.mod2.gyro.data(:,1));
                bias_y=mean(database.id.mod2.gyro.data(:,2));
                
            elseif counter==140
                pause
            end
            
%% Zerar valores
            
            counter

            timetag(:)=[0;0;0;];
            info1(:)=[0;0];
            data1(:)=[0;0;0];
            info2(:)=[0;0];
            data2(:)=[0;0;0];
        end

    end

end

%% Fechar ligação
desligar(robCOMM);
fclose(SerialHandle);