close all
clear
clc

filename = 'MX_posControl_test12.csv';

[status, result] = system( ['wc -l ', filename] );
Index = strfind(result, ' ');
Namostras = sscanf(result(1:Index), '%i', 1);
namostras = Namostras-1;
format long

% << Vel1 << "," << sensor4 << "," << loadError4*GAIN << "," << diffAccumulator4*GAIN1 << ","
% << loadErrorAccumulator4*GAIN2 << "," << Vel2 << "," << sensor2 << "," << loadError2*GAIN
% << "," << diffAccumulator2*GAIN1 << "," << loadErrorAccumulator2*GAIN2  << "," << loadMean2
% << ","<< GAIN << ","<< GAIN1 << ","<< GAIN2 << endl;

vel1 = csvread(filename,0,0,[0,0,namostras,0]); %vel1
sen4 = csvread(filename,0,1,[0,1,namostras,1]); %sen4
P1 = csvread(filename,0,2,[0,2,namostras,2]); %proportional gain
P1_1 = csvread(filename,0,3,[0,3,namostras,3]); %diff between sensors
I1_2 = csvread(filename,0,4,[0,4,namostras,4]); %error accumulator

vel2 = csvread(filename,0,5,[0,5,namostras,5]); %vel1
sen2 = csvread(filename,0,6,[0,6,namostras,6]); %sen4
P2 = csvread(filename,0,7,[0,7,namostras,7]); %proportional gain
P2_1 = csvread(filename,0,8,[0,8,namostras,8]); %diff between sensors
I2_2 = csvread(filename,0,9,[0,9,namostras,9]); %error accumulator


x=1:size(vel1);

% subplot(2,1,1)
% plot(x,vel1,'.',x,vel2,'.')
% set(gca,'fontsize',20)
% legend('vel1','vel2')
% 
% subplot(2,1,2)
plot(x,sen4,x,sen2,x,5*ones(size(sen4)),'r')
 ylim([0 12])
xlim([0 20000])
set(gca,'fontsize',20)
legend('Sensor 4','Sensor 2','Desired tension')
xlabel('sample')
ylabel('Tension [N]')

%'MX_NO_filterEffect_test12.csv';
%csvwrite('BEST_result.csv',[x(1:10:20000)' sen4(1:10:20000) sen2(1:10:20000) 5*ones(size(sen4(1:10:20000)))]);

csvwrite('BAD_behavior.csv',[x(1:10:20000)' sen4(1:10:20000) sen2(1:10:20000) 5*ones(size(sen4(1:10:20000)))]);

%%
% 
% figure(2)
% subplot(2,1,1)
% plot(x,P1,x,P1_1,x,I1_2,x, sen4)
% set(gca,'fontsize',20)
% legend('P1','Diff accumulator','Error accumulator','sen4')
% % xlim([0 1000])
% % ylim([-250 220])
% 
% subplot(2,1,2)
% plot(x,vel2,'.',x,P2,x,P2_1,x,I2_2,x, sen2)
% set(gca,'fontsize',20)
% legend('vel2','P2','Diff accumulator','Error accumulator','sen2')
% % xlim([0 1000])
% % ylim([-220 400])
% 
% % figure(3)
% % plot(x,sen2,x,P2_1,'.',x,P2,x,I2_2,x,sen4)
% % legend('sen2','sen diff','P2','I2','sen4')
% 
% % isto é para o teste 10
% % figure(4)
% % xx=1:size(P1(106720:107020));
% % plot(xx,P1(106720:107020),xx,P1_1(106720:107020),xx,I1_2(106720:107020), xx, sen4(106720:107020).*0.01)
% % legend('P1.1','P2.1','I1','sen4')
% 
% 
% 
% 
