close all
clear all
clc

%Data de inicio 25-Mar-2011
%Nuno Silva

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

%% Impressão de gráficos

hist=200; %Historico sensores
XX=linspace(1,hist,hist)';
YY=zeros(hist,3);
YYg=zeros(hist,2);

figure(1);
h_ace=plot(XX,YY,'o',XX,YY,'',XX,YY,'--');
% Selecção de eixos
plot_acc.h_x1=h_ace(1);
plot_acc.h_y1=h_ace(2);
plot_acc.h_z1=h_ace(3);
plot_acc.h_x2=h_ace(4);
plot_acc.h_y2=h_ace(5);
plot_acc.h_z2=h_ace(6);
plot_acc.h_x3=h_ace(7);
plot_acc.h_y3=h_ace(8);
plot_acc.h_z3=h_ace(9);
grid on;
ylabel('Aceleração g');
legend('X','Y','Z');
axis([0 hist -1.5 1.5])

figure(2);
h_gyro=plot(XX,YYg,'o',XX,YYg,'',XX,YYg,'--');
% Selecção de eixos
plot_gyro.h_x1=h_gyro(1);
plot_gyro.h_y1=h_gyro(2);
plot_gyro.h_x2=h_gyro(3);
plot_gyro.h_y2=h_gyro(4);
plot_gyro.h_x3=h_gyro(5);
plot_gyro.h_y3=h_gyro(6);
grid on;
ylabel('Velocidade angular º/s');
legend('X','Y');
axis([0 hist -200 200])

%% 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
            
%% Actualizar gráficos
            
            if rem(counter,15)==0 && counter>20%Só com multiplos de 5 é que vai actualizar
                
                counterPlot=counterPlot+1;
                
                database.id.(['mod' num2str(nfo(1))]).acel.dataPlot(end+1,:)=database.id.(['mod' num2str(nfo(1))]).acel.data(end,:);
                database.id.(['mod' num2str(nfo(1))]).gyro.dataPlot(end+1,:)=database.id.(['mod' num2str(nfo(1))]).gyro.data(end,:);
                
                %Definição do gráfico 1 (aceleração com o efeito da gravidade)
                if min([size(database.id.mod2.acel.dataPlot,1) size(database.id.mod3.acel.dataPlot,1)])<=hist
%                     matriz1=[zeros((hist+1-counterPlot),3)
%                              database.id.mod1.acel.dataPlot(2:end,:)];
                    a=hist+1-size(database.id.mod2.acel.dataPlot,1);
                    matriz2=[zeros((a>0)*a,3)
                             database.id.mod2.acel.dataPlot(2:end,:)];
                    a=hist+1-size(database.id.mod3.acel.dataPlot,1);
                    matriz3=[zeros((a>0)*a,3)
                             database.id.mod3.acel.dataPlot(2:end,:)];
%                     set(plot_acc.h_x1,'ydata',matriz1(:,1));
%                     set(plot_acc.h_y1,'ydata',matriz1(:,2));
%                     set(plot_acc.h_z1,'ydata',matriz1(:,3));
                    set(plot_acc.h_x2,'ydata',matriz2(:,1));
                    set(plot_acc.h_y2,'ydata',matriz2(:,2));
                    set(plot_acc.h_z2,'ydata',matriz2(:,3));
                    set(plot_acc.h_x3,'ydata',matriz3(:,1));
                    set(plot_acc.h_y3,'ydata',matriz3(:,2));
                    set(plot_acc.h_z3,'ydata',matriz3(:,3));
                else
                    for i=2:3
                        set(plot_acc.(['h_x' num2str(i)]),'ydata',database.id.(['mod' num2str(i)]).acel.dataPlot(end-hist+1:end,1));
                        set(plot_acc.(['h_y' num2str(i)]),'ydata',database.id.(['mod' num2str(i)]).acel.dataPlot(end-hist+1:end,2));
                        set(plot_acc.(['h_z' num2str(i)]),'ydata',database.id.(['mod' num2str(i)]).acel.dataPlot(end-hist+1:end,3));
                    end
                end


                %Definição do gráfico 2 (velocidade angular)
                if min([size(database.id.mod2.gyro.dataPlot,1) size(database.id.mod3.gyro.dataPlot,1)])<=hist
%                     matriz1=[zeros(hist+1-counterPlot,2)
%                              database.id.mod1.gyro.dataPlot(2:end,:)];
                    a=hist+1-size(database.id.mod2.gyro.dataPlot,1);
                    matriz2=[zeros((a>0)*a,2)
                             database.id.mod2.gyro.dataPlot(2:end,:)];
                    a=hist+1-size(database.id.mod3.gyro.dataPlot,1);
                    matriz3=[zeros((a>0)*a,2)
                             database.id.mod3.gyro.dataPlot(2:end,:)];
%                     set(plot_gyro.h_x1,'ydata',matriz1(:,1));
%                     set(plot_gyro.h_y1,'ydata',matriz1(:,2));
                    set(plot_gyro.h_x2,'ydata',matriz2(:,1));
                    set(plot_gyro.h_y2,'ydata',matriz2(:,2));
                    set(plot_gyro.h_x3,'ydata',matriz3(:,1));
                    set(plot_gyro.h_y3,'ydata',matriz3(:,2));
                else
                    for i=2:3
                        set(plot_gyro.(['h_x' num2str(i)]),'ydata',database.id.(['mod' num2str(i)]).gyro.dataPlot(end-hist+1:end,1));
                        set(plot_gyro.(['h_y' num2str(i)]),'ydata',database.id.(['mod' num2str(i)]).gyro.dataPlot(end-hist+1:end,2));
                    end
                end
            
                pause(0.005)
            
            end
            
%% Zerar valores
            
            counter

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

    end

end

%% Fechar ligação

fclose(SerialHandle);