close all
clear
clc

filename = 'MX_torque_test7.csv';

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

%vel1 = csvread(filename,0,0,[0,0,namostras,0]); %vel1
%sen4 = csvread(filename,0,1,[0,1,namostras,1]); %sen4
%vel2 = csvread(filename,0,2,[0,2,namostras,2]); %vel2
%sen2 = csvread(filename,0,3,[0,3,namostras,3]); %sen2

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
I1_1 = csvread(filename,0,3,[0,3,namostras,3]); %diff accumulator
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
I2_1 = csvread(filename,0,8,[0,8,namostras,8]); %diff accumulator
I2_2 = csvread(filename,0,9,[0,9,namostras,9]); %error accumulator

loadmean = csvread(filename,0,10,[0,10,namostras,10]);

x=1:size(vel1);
y=ones(size(vel1));


subplot(2,1,1)
plot(x,vel1,'.',x,vel2,'.',x,y*1024,'r')
legend('vel1','vel2','Load dir change')

subplot(2,1,2)
plot(x,sen4,x,sen2)
legend('sen4','sen2')

nvel1 = zeros(size(vel1));

%for n=1:size(vel1)
%    if (vel1(n) > 1023)
%        nvel1(n) = vel1(n)-1023;
%        vel1(n) = 0;
%    end
%end

%figure(2)
%plot(x,vel1./10,'.',x,sen4,'r',x,y*29.5,x,y*30.5, x, nvel1,'.')
%legend('CCW load * 10^-^1','sen4','limit min', 'limit max','CW load')

figure(2)
subplot(2,1,1)
plot(x,vel1,'.',x,P1,x,I1_1,x,I1_2,x, sen4,x,sen2)
legend('vel1','P1','Diff accumulator','Error accumulator','sen4','sen2')

subplot(2,1,2)
plot(x,vel2,'.',x,P2,x,I2_1,x,I2_2,x, sen2)
legend('vel2','P2','Diff accumulator','Error accumulator','sen2')

figure(3)
plot(x,sen2,x,I2_1,'.',x,P2,x,I2_2,x,sen4)
legend('sen2','sen diff','P2','I2','sen4')

