%% 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];
log_pos_s1=[0 0 0];

%Orientações Euler (O_euler=[1,2,3];)
O_euler2=[3,2,1];
O_euler3=[3,2,1];

%% 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

                %Outro metodo 3D
                if nfo(1)==2
                    ws0=[(database.id.mod2.gyro.data(end,1)-bias2_x) (database.id.mod2.gyro.data(end,2)-bias2_y) 0];
                    ws0=[ws0(O_euler2(1)) ws0(O_euler2(2)) ws0(O_euler2(3))];

                    junta1=ws0*((database.id.mod2.timestamp(end)-database.id.mod2.timestamp(end-1))*tempo_estoiro_tmr1);
                    
%                     rot_y=roty(teta2);
%                     junta1=(rot_y(1:3,1:3)*junta1')';
                    
                    %Soma da posição da junta
                    log_pos_s0=[log_pos_s0
                                log_pos_s0(end,:)+junta1];
                            
                    acel_ab2=sqrt(sum(database.id.mod2.acel.data(end,:).^2))*9.81;

                    if acel_ab2>9.5 && acel_ab2<10
                        teta2=-atan2(database.id.mod2.acel.data(end,O_euler2(1)),database.id.mod2.acel.data(end,O_euler2(3)))*180/pi; %Eixo Y
                        log_pos_s0(end,2)=teta2;
                        fi2=-atan2(database.id.mod2.acel.data(end,O_euler2(2)),sqrt(database.id.mod2.acel.data(end,O_euler2(3))^2+database.id.mod2.acel.data(end,O_euler2(1))^2))*180/pi; %Eixo X X
                        log_pos_s0(end,1)=fi2;
                    end
                    
                elseif nfo(1)==3
                    ws1=[(database.id.mod3.gyro.data(end,1)-bias3_x) -(database.id.mod3.gyro.data(end,2)-bias3_y) 0];
                    ws1=[ws1(O_euler3(1)) ws1(O_euler3(2)) ws1(O_euler3(3))];
                    
                    junta2=ws1*((database.id.mod3.timestamp(end)-database.id.mod3.timestamp(end-1))*tempo_estoiro_tmr1);
                    
%                     rot_y=roty(teta3);
%                     junta2=(rot_y(1:3,1:3)*junta2')';
                    
                    %Soma da posição da junta
                    log_pos_s1=[log_pos_s1
                                log_pos_s1(end,:)+junta2];
                    
                    acel_ab3=sqrt(sum(database.id.mod3.acel.data(end,:).^2))*9.81;

                    if acel_ab3>9 && acel_ab3<10.5
                       teta3=atan2(database.id.mod3.acel.data(end,O_euler3(1)),database.id.mod3.acel.data(end,O_euler3(3)))*180/pi; %Eixo Y
                       log_pos_s1(end,2)=teta3;
                       fi3=-atan2(database.id.mod3.acel.data(end,O_euler3(2)),sqrt(database.id.mod3.acel.data(end,O_euler3(3))^2+database.id.mod3.acel.data(end,O_euler2(1))^2))*180/pi; %Eixo X X
                       log_pos_s1(end,1)=fi3;
                    end
                end

                if rem(counter,200)==0
                    teta2_dif=log_pos_s0(end,2)+log_pos_s1(end,2);
                    log_pos_s0_dif=log_pos_s0(end,1)-log_pos_s1(end,1);
                    move_Joints(log_pos_s1(end,3),0,log_pos_s1(end,1),log_pos_s1(end,2),log_pos_s0_dif,teta2_dif,robCOMM);
%                     move_Joints(log_pos_s1(end,3),0,0,log_pos_s1(end,2),0,0,robCOMM);
                end
                
            elseif counter<140
                
                bias2_x=mean(database.id.mod2.gyro.data(:,1));
                bias2_y=mean(database.id.mod2.gyro.data(:,2));
                bias3_x=mean(database.id.mod3.gyro.data(:,1));
                bias3_y=mean(database.id.mod3.gyro.data(:,2));
                
            elseif counter==140
                fclose(SerialHandle); %Close serial port
                desligar(robCOMM);
                pause
                robCOMM=ligar()
                fopen(SerialHandle); %Open serial port
            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);