close all
clear
clc

format long

filename = 'picontrol.csv';

%picontrol = 11000 dados
%picontrol2 = 23777 dados

namostras=11000;

sen2 = csvread(filename,0,8,[0,8,namostras,8]); %servo 2
sen4 = csvread(filename,0,7,[0,7,namostras,7]); %servo 1

vel1= csvread(filename,0,2,[0,2,namostras,2]); %servo 1
vel2= csvread(filename,0,6,[0,6,namostras,6]); %servo 2

max(vel1)
max(vel2)

x = 1:size(sen2);
LIM = 10.5*ones(size(sen2));
lim = 10*ones(size(sen2));

% ccwvel1 = zeros(size(vel1));
% cwvel1 = zeros(size(vel1));
% ccwvel2 = zeros(size(vel1));
% cwvel2 = zeros(size(vel1));
% 
% for i=1:11000
%     if vel1(i) < 1024
%         ccwvel1(i) = vel1(i);
%     end
%     if vel1(i) > 1023
%         cwvel1(i) = vel1(i) - 1024;
%     end
%     
%     if vel2(i) < 1024
%         ccwvel2(i) = vel2(i);
%     end
%     if vel2(i) > 1023
%         cwvel2(i) = vel2(i) - 1024;
%     end
% end

subplot(2,1,1)
plot(x,vel1*0.1,'.b',x,sen4,'.g',x,lim,'r')
ylim([0 15])
legend('Torque1 output * 0.1','Sensor4 reading','10 N limit')
subplot(2,1,2)
plot(x,vel2*0.1,'.b',x,sen2,'.g',x,lim,'r')
ylim([0 15])
legend('Torque2 output * 0.1','Sensor2 reading','10 N limit')

