%%   MASTER THESIS   %%
%==================== %
%        JOÃO PEIXOTO %
%               60140 %
%  joao.peixoto@ua.pt %
%                2016 %
%==================== %


%% CHAPTER 3 - Sensors Experiments
%     This matlab script has for an objective to analyze the data coming
% from the sensors, either from the node “sensor_receiver” as well as
% “complementary_filter”. This was not the first matlab trial done, but
% rather the combination of all the experiments performed in order to
% understand the accelerometers as well as its data. There were a various
% number of experiments done, but not all of them of interest, so in this
% script we will analyze the best set of data created to understand the
% accelerometers.
%
%     Before we begin, I just want you to notice that we have strange for
% cycles. Their objective is to hide blocks of code. (For example:
% for chapter = 0:0, hides the global variable definition)


%% 00 Clean Workspace
% The first step in order to create this script is to remove all the
% previous matlab data in the workspace.
for chapter = 0:0
    clc
    clear all
    close all
end
%% 01 Load Results
% After the workspace is cleared, we have to load the data from the
% sensors. In this case, “exp_cart” is the data obtained from fanuc,
% “exp_raw” is obtained from the sensors directly and “exp_filt” is
% obtained from the “complementary_filter” node. This data is corresponding
% to an experiment made in the Fanuc robot in which the sensors were
% describing a circular motion.
for chapter = 1:1
    exp_cart = csvread('results/fanuc_exp7_cart.csv',1,0);
     exp_raw = csvread('results/fanuc_exp7_raw.csv',1,0);
    exp_filt = csvread('results/fanuc_exp7_filt.csv',1,0);
end
%% 02 Data Treatment
% Since we got our data, now it’s time to correct it. This correction is
% only so the data from the sensors and data from the fanuc can be
% compared. We also transform the time (nanoseconds) into seconds, and
% subtract every value for the first one. This will allow to start the
% experiment at 0s.
for chapter = 2:2
    % Nanoseconds to Seconds
    exp_cart(:,1) = (exp_cart(:,1)-exp_cart(1,1))/1e9;
     exp_raw(:,1) = ( exp_raw(:,1)- exp_raw(1,1))/1e9;
    exp_filt(:,1) = (exp_filt(:,1)-exp_filt(1,1))/1e9;
    % Transforming from 255 (1G) into 9.81 m/s^2.
     exp_raw(:,2:4) =  exp_raw(:,2:4) * (9.81/255);
    exp_filt(:,2:4) = exp_filt(:,2:4) * (9.81/255);
    % Subtracting the Gravity to the Sensors (This can obly be mada as
    % simple as it is, since the sensor, during the experiment was always
    % with the same orientation.
     exp_raw(:,  4) =  exp_raw(:,  4) - 9.81;
    exp_filt(:,  4) = exp_filt(:,  4) - 9.81;
    % Fanuc mm to m
    exp_cart(:,2:4) = exp_cart(:,2:4)/1000; 
    % RAD TO DEGREE
    exp_filt(:,8:10) = exp_filt(:,8:10) * 180/pi;
end
%% 03 Creation of Motion Variable 
% In this chapter we create the motion variable, which is the variable that
% contains all the information in the three axis relative to the linear
% motion of the sensors, i.e., linear position, velocity and acceleration.
% The motion variable works as described:
%       motion(:,   1) time
%       motion(:,2:04) linear position (x,y,z)
%       motion(:,5:07) linear velocity (x,y,z)
%       motion(:,8:10) linear acceleration (x,y,z)
for chapter = 3:3
    % creation of motion
    motion = zeros(size(exp_filt,1),10);
    % time and acceleration
    motion(:,   1) = exp_filt(:,  1);
    motion(:,8:10) = exp_filt(:,2:4);
    % initial position (obtained from fanuc)
    motion(1, 2:4) = exp_cart(1,2:4); 
    motion(1, 5:7) = (exp_cart(2,5:7)-exp_cart(1, 5:7))/(exp_cart(2,1)-exp_cart(1,1));
end
%% 04 Integration
% After we have motion created, it is time to calculate its velocity and
% position. For that we (taking velocity for example) that the current
% velocity is equal to the previous velocity plus the time iteration times
% the acceleration. In other words:
%       v(t+1) = v(t) + dt*a(t)
%       p(t+1) = p(t) + dt*v(t)
for chapter = 4:4
    for i = 1:size(motion,1)-1
        for j = 1:3
            dt = motion(i+1,1) - motion(i,1);
            motion(i+1,j+4) = motion(i,j+4) + dt*(motion(i,j+7));
            motion(i+1,j+1) = motion(i,j+1) + dt*(motion(i,j+4));
        end
    end
end
%% 05 Plot
% Now that we have our treated data, it is time to compare it with the
% Fanuc data. For that we will first create a graphic that shows us how the
% sensors moved (according to the fanuc). Then we will see for every axis,
% the position of the fanuc and the position, velocity and acceleration
% that the sensors measured.
for chapter = 5:5
    % Described Path - The path described by the fanuc end-effector
    figure(1); grid on; hold on; view(130,15); 
    title('Described Path'); xlabel('time(s)'); ylabel('position y axis (m)'); zlabel('position z axis (m)')
    plot3(exp_cart(:,1), exp_cart(:,3), exp_cart(:,4), '-k');

    % Direct Analisys of the outputs of the nodes used.
             % Raw Data     (sensor_receiver)
    Graphics(     'Accelerometers', exp_cart, exp_raw,  2, 2)
    Graphics(         'Gyroscopes', exp_cart, exp_raw,  5, 3)
             % Filt Data    (complementary_filter)
    Graphics('Linear Acceleration', exp_cart, exp_filt, 2, 4)
    Graphics(   'Angular Velocity', exp_cart, exp_filt, 5, 5)
    Graphics(   'Angular Position', exp_cart, exp_filt, 8, 6)
    
    %Fanuc Position and Sensors Acceleration, Velocity and Position
    for i=1:3
        figure(i+6); hold on; grid on;
        % Fanuc Position
        subplot(2,2,1); hold on; grid on;
        axis([min(motion(:,1)), max(motion(:,1)), -0.5, 0.5])
        if     (i == 1); title('Fanuc Cartesian Position - x axix')
        elseif (i == 2); title('Fanuc Cartesian Position - y axis')
        elseif (i == 3); title('Fanuc Cartesian Position - z axis')
        end
        xlabel('time (s)'); ylabel('position (m)');
        plot(exp_cart(:,1), exp_cart(:,i+1), 'k');   %FANUC
        %Sensor Acceleration
        subplot(2,2,2); hold on; grid on;
        title('Measured Acceleration (m/s^2)'); xlabel('time (s)'); ylabel('acceleration (m/s^2)');
        plot(  motion(:,1),   motion(:,i+7), 'b');   %MOTION ACCELERATION
        %Sensor Velocity
        subplot(2,2,3); hold on; grid on;
        title('Integrated Velocity (m/s)'); xlabel('time (s)'); ylabel('velocity (m/s)');
        plot(  motion(:,1),   motion(:,i+4), 'g');   %MOTION VELOCITY
        %Sensor Position
        subplot(2,2,4); hold on; grid on;
        title('Integrated Position (m)'); xlabel('time (s)'); ylabel('position (m)');
        plot(  motion(:,1),   motion(:,i+1), 'r');   %MOTION POSITION
    end
    
    
    i = 2;
    A(:,1) = exp_raw(:,1);  A(:,2) = exp_raw(:, i);  save('../../../LaTeX/data/03/AccelDataX.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_raw(:,1);  A(:,2) = exp_raw(:, i);  save('../../../LaTeX/data/03/AccelDataY.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_raw(:,1);  A(:,2) = exp_raw(:, i);  save('../../../LaTeX/data/03/AccelDataZ.txt', 'A', '-ascii','-double'); i = i+1;

    A(:,1) = exp_raw(:,1);  A(:,2) = exp_raw(:, i);  save('../../../LaTeX/data/03/GyroDataX.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_raw(:,1);  A(:,2) = exp_raw(:, i);  save('../../../LaTeX/data/03/GyroDataY.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_raw(:,1);  A(:,2) = exp_raw(:, i);  save('../../../LaTeX/data/03/GyroDataZ.txt', 'A', '-ascii','-double'); i = i+1;
    A = [];
        
    
    i = 2;
    A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:, i); save('../../../LaTeX/data/03/LinAccelDataX.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:, i); save('../../../LaTeX/data/03/LinAccelDataY.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:, i); save('../../../LaTeX/data/03/LinAccelDataZ.txt', 'A', '-ascii','-double'); i = i+1;
    
    A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:, i); save('../../../LaTeX/data/03/AngVelDataX.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:, i); save('../../../LaTeX/data/03/AngVelDataY.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:, i); save('../../../LaTeX/data/03/AngVelDataZ.txt', 'A', '-ascii','-double'); i = i+1;
    
    A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:, i); save('../../../LaTeX/data/03/AngPosDataX.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:, i); save('../../../LaTeX/data/03/AngPosDataY.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:, i); save('../../../LaTeX/data/03/AngPosDataZ.txt', 'A', '-ascii','-double'); i = i+1;
    A = [];
    
    
    i = 2;
    A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:, i); save('../../../LaTeX/data/03/Cart02.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:, i); save('../../../LaTeX/data/03/Cart03.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:, i); save('../../../LaTeX/data/03/Cart04.txt', 'A', '-ascii','-double'); i = i+1;
    
    A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:, i); save('../../../LaTeX/data/03/Cart05.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:, i); save('../../../LaTeX/data/03/Cart06.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:, i); save('../../../LaTeX/data/03/Cart07.txt', 'A', '-ascii','-double'); i = i+1;
    
    A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:, i); save('../../../LaTeX/data/03/Cart08.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:, i); save('../../../LaTeX/data/03/Cart09.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:, i); save('../../../LaTeX/data/03/Cart10.txt', 'A', '-ascii','-double'); i = i+1;
    A = [];
    
    
    i = 2;
    A(:,1) =   motion(:,1); A(:,2) =   motion(:, i); save('../../../LaTeX/data/03/Motion02.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) =   motion(:,1); A(:,2) =   motion(:, i); save('../../../LaTeX/data/03/Motion03.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) =   motion(:,1); A(:,2) =   motion(:, i); save('../../../LaTeX/data/03/Motion04.txt', 'A', '-ascii','-double'); i = i+1;
    
    A(:,1) =   motion(:,1); A(:,2) =   motion(:, i); save('../../../LaTeX/data/03/Motion05.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) =   motion(:,1); A(:,2) =   motion(:, i); save('../../../LaTeX/data/03/Motion06.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) =   motion(:,1); A(:,2) =   motion(:, i); save('../../../LaTeX/data/03/Motion07.txt', 'A', '-ascii','-double'); i = i+1;
    
    A(:,1) =   motion(:,1); A(:,2) =   motion(:, i); save('../../../LaTeX/data/03/Motion08.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) =   motion(:,1); A(:,2) =   motion(:, i); save('../../../LaTeX/data/03/Motion09.txt', 'A', '-ascii','-double'); i = i+1;
    A(:,1) =   motion(:,1); A(:,2) =   motion(:, i); save('../../../LaTeX/data/03/Motion10.txt', 'A', '-ascii','-double'); i = i+1;
    A = [];
end