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


%% CHAPTER 3 - Sensors Experiments
%     In this script, the main objective is to study the gyroscopes and
% compare its data with the data obtained from the Fanuc. The main question
% is if the sensors may be used to understand the orientation of the
% sensors and if it is possible to determine their location using polar
% coordinates for example.


%% 00 Clean Workspace and Global Variables
% As always, the first thing that needs to be done is clean the worskspace
% as well defining the global variables for the scrip to work. In this
% case, the global variables are only meant to decide if something is shown
% or not.
for chapter = 0:0
    clc
    clear all
    close all
% ================================================= %
% =============== Program Variables =============== %
    show_pause     = true;                         % makes pause
    show_img_chap02 = true;                         % Described Path
    show_img_chap03 = true;                         % Orientation (w,p,r)
    show_img_chap04 = true;                         % Polar Coordinates
    show_img_chap05 = true;                         % Simulation#1
    show_img_chap06 = true;                         % Gyroscopes Comparison (exp_raw)
    show_img_chap07 = true;                         % Gyroscopes Comparison (exp_filt)
    show_img_chap08 = true;                         % Angular Position (exp_filt)
    show_img_chap09 = true;                         % Angular Position VS Fanuc w,p,r
    show_img_chap10 = true;                         % Simulation#2
    speed_up_simulation = 100;                      % Number of Times that Simulation runs faster
% ================================================= %
% ================================================= %
% This variable exists for the chapter 4 when simulating using the w,p,r
% obtained from the fanuc. The angles obtained from the fanuc present some
% peaks due to the definition of the angles themselves, i.e., instead of an
% angle of value 90 goes to 91, it goes to -89 for example. In this
% particular case we can see the red and blue peaks (w,r). If this
% variables is true, then those peaks will be ignored.
dismiss_errors_in_drawing_sensor_orientation = true;
end
%% 01 Load Results
% Once the global variables are defined, we need to load and treat the data
% in order for us to study it
%       exp_cart - data from the fanuc (cartesian coordinates)
%       exp_raw  - data from "sensor_receiver"
%       exp_filt - data from "complementary_filter"
for chapter = 1:1
    exp_cart = csvread('results/fanuc_cart.csv',1,0);
     exp_raw = csvread('results/fanuc_raw.csv',1,0);
    exp_filt = csvread('results/fanuc_filt.csv',1,0);
    % convert 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;
    %correct gyroscope data
    exp_raw(:,5:7) = exp_raw(:,5:7)*0.07;
    exp_filt(:,5:7) = exp_filt(:,5:7)*0.07;
end
%% 02 Cartesian Position
% In this section we plotted the path that the end-effector has done. 
for chapter = 2:2
    if(show_img_chap02)
        % 2.1 - 3D SPACE
        figure(21)
        plot3(exp_cart(:,1), exp_cart(:,3), exp_cart(:,4), '-k'); grid on; hold on;
        set(gca,'PlotBoxAspectRatio',[1 1 1]);
        title('Cartesian Position [Y,Z] Through Time'); xlabel('time (s)'); ylabel('y (mm)'); zlabel('z (mm)');
        if(show_pause);pause;end

        % 2.2 - 2D SPACE
        figure(22)
        subplot(2,1,1); plot(exp_cart(:,1), exp_cart(:,3), '-g'); grid on; hold on;
        title('Cartesian Position Y'); xlabel('time (s)'); ylabel('y (mm)');
        subplot(2,1,2); plot(exp_cart(:,1), exp_cart(:,4), '-b'); grid on; hold on;
        title('Cartesian Position Z'); xlabel('time (s)'); ylabel('z (mm)');
        if(show_pause);pause;end
    end
end
%% 03 Orientation
% In here we plot the w,p,r (obtained from fanuc)
for chapter = 3:3
    if(show_img_chap03)
    figure(3)
    for i = 1:3
        subplot(3,1,i); grid on; hold on;
        axis([min(exp_cart(:,1)) max(exp_cart(:,1)) -180 180])
            if(i==1); title('Angular Orientation (w)'); xlabel('time(s)'); ylabel('w (º)'); color = '-r';
        elseif(i==2); title('Angular Orientation (p)'); xlabel('time(s)'); ylabel('p (º)'); color = '-g';
        elseif(i==3); title('Angular Orientation (r)'); xlabel('time(s)'); ylabel('r (º)'); color = '-b'; end
        plot(exp_cart(:,1), exp_cart(:,i+4), color);
    end
    if(show_pause);pause;end
    end
end
%% 04 Polar Coordinates
% In here we represent the polar coordinates of the path described
for chapter = 4:4
    % 4.1 - CONVERSION FROM CARTESIAN TO POLAR
    [angle,distance] = cart2pol(exp_cart(:,3),exp_cart(:,4));
    exp_pol = zeros(size(exp_cart,1),3);
    for i=1:size(exp_cart,1)
        exp_pol(i,1) = exp_cart(i,1);                   % time
        exp_pol(i,2) = distance(i);                     % distance
        exp_pol(i,3) = angle(i)*180/pi;                 % angle (degrees)
    end
    
    
    % 4.2 - 3D SPACE
    if(show_img_chap04)
        figure(42)
        polar(exp_pol(:,3)*pi/180,exp_pol(:,2))
        if(show_pause);pause;end
    
    % 4.3 - 2D SPACE
        for i = 1:size(exp_pol,1);
            if(exp_pol(i,3)<0); 
                exp_pol(i,3) = exp_pol(i,3) +360;       % angle correction
            end;
        end;
        
        
        figure(43)
        % Distance Graphic
        subplot(2,1,1); grid on; hold on;
        plot(exp_pol(:,1), exp_pol(:,2), '-g');
        axis([min(exp_pol(:,1)) max(exp_pol(:,1)) 0 300])
        title('Distance (\rho)'); xlabel('time (s)'); ylabel('distance (mm)');
        % Angle Graphic
        subplot(2,1,2); grid on; hold on;
        plot(exp_pol(:,1), exp_pol(:,3), '-b');
        title('Angle (\theta)'); xlabel('time (s)'); ylabel('angle (º)');
        if(show_pause);pause;end
    end
end
%% 05 Draw Sensor From Fanuc Data (Orientation)
% In this chapter the first simulation is created, using the data obtained
% from the Fanuc
for chapter = 5:5
    if(show_img_chap05)
    figure(5)
            %Orientation with angle line
            subplot(3,2,2); hold on; grid on;
            axis([min(exp_cart(:,1)) max(exp_cart(:,1)) -180 180])
            title('Angular Position (w)'); xlabel('time(s)'); ylabel('position (º)');
            plot(exp_cart(:,1),exp_cart(:,5),'-r')
            
            subplot(3,2,4); hold on; grid on;
            axis([min(exp_cart(:,1)) max(exp_cart(:,1)) -180 180])
            title('Angular Position (p)'); xlabel('time(s)'); ylabel('position (º)');
            plot(exp_cart(:,1),exp_cart(:,6),'-g')

            subplot(3,2,6); hold on; grid on;
            axis([min(exp_cart(:,1)) max(exp_cart(:,1)) -180 180])
            title('Angular Position (r)'); xlabel('time(s)'); ylabel('position (º)');
            plot(exp_cart(:,1),exp_cart(:,7),'-b')
            
    % Creation of the "sensor"
    Points = [-1  1  1 -1 -1  1  1 -1
              -2 -2  2  2 -2 -2  2  2
              -3 -3 -3 -3  3  3  3  3]'*10;
    hpoints=[ Points'; ones(1, size(Points,1))];
    Faces = [1 2 3 4
             5 6 7 8
             1 2 6 5
             2 3 7 6
             3 4 8 7
             4 1 5 8];

    % Sensor Designed
    subplot(3,2,[1 3])
    title('3D Space Simulation'); xlabel('x (mm)'); ylabel('y (mm)'); zlabel('z (mm)'); 
    h1 = patch('Vertices', Points, 'Faces', Faces, 'FaceColor', 'c');
    axis equal; grid on
    axis([-300 100 -100 300 -100 300])
    view(120,30)
    
    subplot(3,2,5)
    title('Rotation Simulation'); xlabel('x (mm)'); ylabel('y (mm)'); zlabel('z (mm)'); 
    h2 = patch('Vertices', Points, 'Faces', Faces, 'FaceColor', 'c');
    axis equal; grid on
    axis([-50 50 -50 50 -50 50])
    view(120,30)

    % Mathematic Calculations using the fanuc w,p,r to animate the "sensor"
    newpoints = hpoints;
    newpoints = roty(pi/2) * newpoints;
    newpoints = rotx(0)    * newpoints;
    newpoints = rotz(pi/2) * newpoints;
    newpoints1 = newpoints;
    newpoints2 = newpoints1;
    for i = 1:size(exp_cart, 1)-1
        subplot(3,2,[1 3])
        if(~dismiss_errors_in_drawing_sensor_orientation); newpoints2 = rotx((exp_cart(i+1,5) - exp_cart(i,5))*pi/180) * newpoints2; end;
        newpoints2 = roty((exp_cart(i+1,6) - exp_cart(i,6))*pi/180) * newpoints2;
        if(~dismiss_errors_in_drawing_sensor_orientation); newpoints2 = rotz((exp_cart(i+1,7) - exp_cart(i,7))*pi/180) * newpoints2; end;
        newpoints2 = trans((exp_cart(i+1,2)-exp_cart(i,2)), 0*(exp_cart(i+1,3)-exp_cart(i,3)), (exp_cart(i+1,4)-exp_cart(i,4))) * newpoints2;
        set(h1,'Vertices',newpoints2(1:3,:)');

        subplot(3,2,5)
        if(~dismiss_errors_in_drawing_sensor_orientation); newpoints1 = rotx((exp_cart(i+1,5) - exp_cart(i,5))*pi/180) * newpoints1; end;
        newpoints1 = roty((exp_cart(i+1,6) - exp_cart(i,6))*pi/180) * newpoints1;
        if(~dismiss_errors_in_drawing_sensor_orientation); newpoints1 = rotz((exp_cart(i+1,7) - exp_cart(i,7))*pi/180) * newpoints1; end;
        set(h2,'Vertices',newpoints1(1:3,:)');

        pause((exp_cart(i+1,1)-exp_cart(i,1))/speed_up_simulation)
    end
    if(show_pause);pause;end
    end
end
%% 06 Gyroscopes Data Treatment (Raw)
% In here we try to integrate the data obtained from the "sensor_receiver"
% relative to the gyroscopes
for chapter = 6:6
    % 6.1 - PRELIMINAR DATA
    if(show_img_chap06)
    figure(61)
    subplot(3,2,1); hold on; grid on;
        plot(exp_raw(:,1), exp_raw(:,5), '-r');
    title('Gyroscopes (w) (exp\_raw)'); xlabel('time(s)'); ylabel('X Axis (º/s)');
    subplot(3,2,3); hold on; grid on;
        plot(exp_raw(:,1), exp_raw(:,6), '-g');
    title('Gyroscopes (p) (exp\_raw)'); xlabel('time(s)'); ylabel('Y Axis (º/s)');
    subplot(3,2,5); hold on; grid on;
        plot(exp_raw(:,1), exp_raw(:,7), '-b');
    title('Gyroscopes (r) (exp\_raw)'); xlabel('time(s)'); ylabel('Z Axis (º/s)');
    end

    % 6.2 - INTEGRATION 
    %       angular_position(t+1) = angular_position(t) + dt*angular_velocity(t)
    exp_raw_int = zeros(size(exp_raw,1)-1, size(exp_raw,2));
    C = 0;
    for i=1:size(exp_raw_int,1)
        exp_raw_int(i,1) = exp_raw(i,1);
        for j = 2:size(exp_raw_int,2);
            exp_raw_int(i,j) = (exp_raw(i+1,1) - exp_raw(i,1))*((exp_raw(i+1,j) + exp_raw(i,j))/2);
            if(i == 1); exp_raw_int(i,j) = exp_raw_int(i,j) + C; end
            if(i ~= 1); exp_raw_int(i,j) = exp_raw_int(i,j) + exp_raw_int(i-1,j); end
        end
    end

    % 6.3 - SHOW RESULTS
    if(show_img_chap06)
    figure(61)
    subplot(3,2,2); hold on; grid on;
        plot(exp_raw_int(:,1), exp_raw_int(:,5), '-r');
    title('Angular Position (w)'); xlabel('time(s)'); ylabel('w Orientation (º)');
    subplot(3,2,4); hold on; grid on;
        plot(exp_raw_int(:,1), exp_raw_int(:,6), '-g');
    title('Angular Position (p)'); xlabel('time(s)'); ylabel('p Orientation (º)');
    subplot(3,2,6); hold on; grid on;
        plot(exp_raw_int(:,1), exp_raw_int(:,7), '-b');
    title('Angular Position (r)'); xlabel('time(s)'); ylabel('r Orientation (º)');
    if(show_pause);pause;end
    end
end
%% 07 Gyroscopes Data Treatment (Filt)
% In here we try to integrate the data obtained from the
% "complementary_filter" relative to the gyroscopes
for chapter = 7:7
    % 7.1 - PRELIMINAR DATA
    if(show_img_chap07)
    figure(71)
    subplot(3,2,1); hold on; grid on;
        plot(exp_filt(:,1), exp_filt(:,5), '-r');
    title('Gyroscopes (X) (exp\_filt)'); xlabel('time(s)'); ylabel('X Axis (º/s)');
    subplot(3,2,3); hold on; grid on;
        plot(exp_filt(:,1), exp_filt(:,6), '-g');
    title('Gyroscopes (Y) (exp\_filt)'); xlabel('time(s)'); ylabel('Y Axis (º/s)');
    subplot(3,2,5); hold on; grid on;
        plot(exp_filt(:,1), exp_filt(:,7), '-b');
    title('Gyroscopes (Z) (exp\_filt)'); xlabel('time(s)'); ylabel('Z Axis (º/s)');
    end
    
    % 7.2 - INTEGRATION
    exp_filt_int = zeros(size(exp_filt,1)-1, size(exp_filt,2));
    C = 0;
    for i=1:size(exp_filt_int,1)
        exp_filt_int(i,1) = exp_filt(i,1);
        for j = 2:size(exp_filt_int,2);
            exp_filt_int(i,j) = (exp_filt(i+1,1) - exp_filt(i,1))*((exp_filt(i+1,j) + exp_filt(i,j))/2);
            if(i == 1); exp_filt_int(i,j) = exp_filt_int(i,j) + C; end
            if(i ~= 1); exp_filt_int(i,j) = exp_filt_int(i,j) + exp_filt_int(i-1,j); end
        end
    end


    % 7.3 - SHOW RESULTS
    if(show_img_chap07)
    figure(71)
    subplot(3,2,2); hold on; grid on;
        plot(exp_filt_int(:,1), exp_filt_int(:,5), '-r');
    title('Angular Position (x)'); xlabel('time(s)'); ylabel('X Axis (º)');
    subplot(3,2,4); hold on; grid on;
        plot(exp_filt_int(:,1), exp_filt_int(:,6), '-g');
    title('Angular Position (y)'); xlabel('time(s)'); ylabel('Y Axis (º)');
    subplot(3,2,6); hold on; grid on;
        plot(exp_filt_int(:,1), exp_filt_int(:,7), '-b');
    title('Angular Position (z)'); xlabel('time(s)'); ylabel('Z Axis (º)');
    if(show_pause);pause;end
    end
end 
%% 08 Angular Position (Filt)
% In here we will try to compare the angular position obtained from the
% "complementary_filter" and the w,p,r obtained from the Fanuc
for chapter = 8:8
    % 8.1 - PRELIMINAR DATA
    if(show_img_chap08)
    figure(81)
    subplot(3,2,1); hold on; grid on;
        plot(exp_filt(:,1), exp_filt(:,8)*180/pi, '-r');
        axis([min(exp_filt(:,1)) max(exp_filt(:,1)) -180 180])
    title('Angular Position (X)'); xlabel('time(s)'); ylabel('X Orientation (º)');
    subplot(3,2,3); hold on; grid on;
        plot(exp_filt(:,1), exp_filt(:,9)*180/pi, '-g');
        axis([min(exp_filt(:,1)) max(exp_filt(:,1)) -180 180])
    title('Angular Position (Y)'); xlabel('time(s)'); ylabel('Y Orientation (º)');
    subplot(3,2,5); hold on; grid on;
        plot(exp_filt(:,1), exp_filt(:,10)*180/pi, '-b');
        axis([min(exp_filt(:,1)) max(exp_filt(:,1)) -180 180])
    title('Angular Position (Z)'); xlabel('time(s)'); ylabel('Z Orientation (º)');
    end
    
    
    % 8.2 - SHOW RESULTS
    if(show_img_chap08)
    figure(81)
    subplot(3,2,2); hold on; grid on;
        plot(exp_cart(:,1), exp_cart(:,5), '-r');
        axis([min(exp_filt(:,1)) max(exp_filt(:,1)) -180 180])
    title('Angular Position (w)'); xlabel('time(s)'); ylabel('X Orientation (º)');
    subplot(3,2,4); hold on; grid on;
        plot(exp_cart(:,1), exp_cart(:,6), '-g');
        axis([min(exp_filt(:,1)) max(exp_filt(:,1)) -180 180])
    title('Angular Position (p)'); xlabel('time(s)'); ylabel('Y Orientation (º)');
    subplot(3,2,6); hold on; grid on;
        plot(exp_cart(:,1), exp_cart(:,7), '-b');
        axis([min(exp_filt(:,1)) max(exp_filt(:,1)) -180 180])
    title('Angular Position (r)'); xlabel('time(s)'); ylabel('Z Orientation (º)');
    if(show_pause);pause;end
    end
    
    
    % 8.3 - COMPARISON
    test2=ones(size(exp_filt,1),1);
    if(show_img_chap08)
    figure(83)
    subplot(3,1,1); hold on; grid on;
        plot(exp_filt(:,1), exp_filt(:,8)*180/pi, '-r');
        plot(exp_cart(:,1), exp_cart(:,5), '-g');
        axis([min(exp_filt(:,1)) max(exp_filt(:,1)) -180 180])
    title('Red(X) Green(w)'); xlabel('time(s)'); ylabel('X Orientation (º)');
    subplot(3,1,2); hold on; grid on;
        plot(exp_filt(:,1), exp_filt(:,9)*180/pi, '-r');
        plot(exp_cart(:,1), exp_cart(:,6), '-g');
        axis([min(exp_filt(:,1)) max(exp_filt(:,1)) -180 180])
    title('Red(Y) Green(p)'); xlabel('time(s)'); ylabel('Y Orientation (º)');
    subplot(3,1,3); hold on; grid on;
        plot(exp_filt(:,1), exp_filt(:,10)*180/pi, '-r');
        plot(exp_cart(:,1), exp_cart(:,7), '-g');
        axis([min(exp_filt(:,1)) max(exp_filt(:,1)) -180 180])
    title('Red(Z) Green(r)'); xlabel('time(s)'); ylabel('Z Orientation (º)');
    if(show_pause);pause;end
    end


% ANG POS (beforeTreatment)
A = []; i = 1;
A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:,i+7)*180/pi; save('../../../LaTeX/data/04/angPosDegBefX.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:,i+7)*180/pi; save('../../../LaTeX/data/04/angPosDegBefY.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:,i+7)*180/pi; save('../../../LaTeX/data/04/angPosDegBefZ.txt', 'A', '-ascii','-double'); i = i+1;
end    
%% 09 Correction
% After understanding that the data was simillar we corrected it in order
% to match the results obtained from fanuc.
for chapter = 9:9
    % ====================================================== %
         exp_filt(:,1) =  exp_filt(:,1) - 0.75;        % === %
         exp_filt(:,8) =  exp_filt(:,8) * 180/pi - 90; % === %
         exp_filt(:,9) =  exp_filt(:,9) *-180/pi - 90; % === %
        exp_filt(:,10) = exp_filt(:,10) * 180/pi - 90; % === %
    % ====================================================== %
    if(show_img_chap09)
    figure(9)
    subplot(3,1,1); hold on; grid on;
        plot(exp_cart(:,1), exp_cart(:,5), '-g');
        axis([min(exp_filt(:,1)) max(exp_filt(:,1)) -180 180])
    title('Red(X) Green(w)'); xlabel('time(s)'); ylabel('X orientation (º)');
    subplot(3,1,2); hold on; grid on;
        plot(exp_cart(:,1), exp_cart(:,6), '-g');
        axis([min(exp_filt(:,1)) max(exp_filt(:,1)) -180 180])
    title('Red(Y) Green(p)'); xlabel('time(s)'); ylabel('Y orientation (º)');
    subplot(3,1,3); hold on; grid on;
        plot(exp_cart(:,1), exp_cart(:,7), '-g');
        axis([min(exp_filt(:,1)) max(exp_filt(:,1)) -180 180])
    title('Red(Z) Green(r)'); xlabel('time(s)'); ylabel('Z orientation (º)');
    if(show_pause);pause;end
    
    figure(9)
    subplot(3,1,1); hold on; grid on;
        plot(exp_filt(:,1), exp_filt(:,8), '-r');
    subplot(3,1,2); hold on; grid on;
        plot(exp_filt(:,1), exp_filt(:,9), '-r');
    subplot(3,1,3); hold on; grid on;
        plot(exp_filt(:,1), exp_filt(:,10), '-r');
    end
    if(show_pause);pause;end
end
%% 10 Draw Sensor From Sensor Data (Orientation)
% In this chapter the second simulation is created, using the data obtained
% from the sensor ("complementary_filter")
for chapter = 10:10
    if(show_img_chap10)
    test=ones(size(exp_filt,1),1);
    line = linspace(0,50,size(test,1));
    figure(10)
            % Orientation 
            % w
            subplot(3,2,2); hold on; grid on;
            axis([min(exp_cart(:,1)) max(exp_cart(:,1)) -180 180])
            plot(exp_filt(:,1),exp_filt(:,8),'-r')
            title('Angular Position (w)'); xlabel('time(s)'); ylabel('position (º)');
            % p
            subplot(3,2,4); hold on; grid on;
            axis([min(exp_cart(:,1)) max(exp_cart(:,1)) -180 180])
            plot(exp_filt(:,1),exp_filt(:,9),'-g')
            title('Angular Position (p)'); xlabel('time(s)'); ylabel('position (º)');
            % r
            subplot(3,2,6); hold on; grid on;
            axis([min(exp_cart(:,1)) max(exp_cart(:,1)) -180 180])
            plot(exp_filt(:,1),exp_filt(:,10),'-b')
            title('Angular Position (r)'); xlabel('time(s)'); ylabel('position (º)');
            
            subplot(3,2,[1 3 5]); hold on; grid on;
            plot3(line, test(:,1)*0, test(:,1)*0, '*r-');
            plot3(test(:,1)*0, line, test(:,1)*0, '*g-');
            plot3(test(:,1)*0, test(:,1)*0, line, '*b-');

            
    % Sensor Creation
    Points = [-1  1  1 -1 -1  1  1 -1; -2 -2  2  2 -2 -2  2  2; -3 -3 -3 -3  3  3  3  3]'*10;
    hpoints=[ Points'; ones(1, size(Points,1))];
    Faces = [1 2 3 4; 5 6 7 8; 1 2 6 5; 2 3 7 6; 3 4 8 7; 4 1 5 8];

    subplot(3,2,[1 3 5])
    title('Rotation Simulation'); xlabel('x (mm)'); ylabel('y (mm)'); zlabel('z (mm)'); 
    h2 = patch('Vertices', Points, 'Faces', Faces, 'FaceColor', 'c');
    axis equal; grid on
    axis([-50 50 -50 50 -50 50])
    view(120,30)

    newpoints = hpoints;
    newpoints = roty(pi/2) * newpoints;
    newpoints = rotx(0)    * newpoints;
    newpoints = rotz(pi/2) * newpoints;
    newpoints1 = newpoints;
    newpoints2 = newpoints1;
    for i = 1:size(exp_filt, 1)-1
        subplot(3,2,[1 3 5])
        newpoints1 = rotx((exp_filt(i+1,8) - exp_filt(i,8))*pi/180) * newpoints1;
        newpoints1 = roty((exp_filt(i+1,9) - exp_filt(i,9))*pi/180) * newpoints1;
        newpoints1 = rotz((exp_filt(i+1,10) - exp_filt(i,10))*pi/180) * newpoints1;
        set(h2,'Vertices',newpoints1(1:3,:)');

        pause((exp_filt(i+1,1)-exp_filt(i,1))/speed_up_simulation)
    end
    if(show_pause);pause;end
    end
end


%trajetoria
A = [];
A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:,3); A(:,3) = exp_cart(:,4); save('../../../LaTeX/data/04/traj.txt', 'A', '-ascii','-double');


% W,P,R
A = []; i = 1;
A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:,i+4); save('../../../LaTeX/data/04/W.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:,i+4); save('../../../LaTeX/data/04/P.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_cart(:,1); A(:,2) = exp_cart(:,i+4); save('../../../LaTeX/data/04/R.txt', 'A', '-ascii','-double'); i = i+1;


% Comparison1 (Gyro(Raw) VS Integrated Values)
A = []; i = 1;
A(:,1) = exp_raw(:,1); A(:,2) = exp_raw(:,i+4); save('../../../LaTeX/data/04/GyroRawX.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_raw(:,1); A(:,2) = exp_raw(:,i+4); save('../../../LaTeX/data/04/GyroRawY.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_raw(:,1); A(:,2) = exp_raw(:,i+4); save('../../../LaTeX/data/04/GyroRawZ.txt', 'A', '-ascii','-double'); i = i+1;

A = []; i = 1;
A(:,1) = exp_raw_int(:,1); A(:,2) = exp_raw_int(:,i+4); save('../../../LaTeX/data/04/GyroRawIntX.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_raw_int(:,1); A(:,2) = exp_raw_int(:,i+4); save('../../../LaTeX/data/04/GyroRawIntY.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_raw_int(:,1); A(:,2) = exp_raw_int(:,i+4); save('../../../LaTeX/data/04/GyroRawIntZ.txt', 'A', '-ascii','-double'); i = i+1;


% Comparison1 (Gyro(Filt) VS Integrated Values)
A = []; i = 1;
A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:,i+4); save('../../../LaTeX/data/04/GyroFiltX.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:,i+4); save('../../../LaTeX/data/04/GyroFiltY.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:,i+4); save('../../../LaTeX/data/04/GyroFiltZ.txt', 'A', '-ascii','-double'); i = i+1;

A = []; i = 1;
A(:,1) = exp_filt_int(:,1); A(:,2) = exp_filt_int(:,i+4); save('../../../LaTeX/data/04/GyroFiltIntX.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_filt_int(:,1); A(:,2) = exp_filt_int(:,i+4); save('../../../LaTeX/data/04/GyroFiltIntY.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_filt_int(:,1); A(:,2) = exp_filt_int(:,i+4); save('../../../LaTeX/data/04/GyroFiltIntZ.txt', 'A', '-ascii','-double'); i = i+1;


% ANG POS (afterTreatment)
A = []; i = 1;
A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:,i+7); save('../../../LaTeX/data/04/angPosDegAftX.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:,i+7); save('../../../LaTeX/data/04/angPosDegAftY.txt', 'A', '-ascii','-double'); i = i+1;
A(:,1) = exp_filt(:,1); A(:,2) = exp_filt(:,i+7); save('../../../LaTeX/data/04/angPosDegAftZ.txt', 'A', '-ascii','-double'); i = i+1;
