%h=initcom(1,115200)
%applyjoint(h,2,3,[1 1 0])
% applyjoint(h,2,1,[50 50 50])
% applyjoint(h,2,0,[1 0 0])
% pause(1)
% applyjoint(h,2,0,[10 0 0])
% pause(1)
% applyjoint(h,2,0,[40 0 0])
% applyjoint(h,2,0,[0 0 0])
%applycontrol(h,2,3,[3 3 3])
%pause
%applycontrol(h,2,3,[0 0 0])

graus_init=-20;
close all;

applyjoint(h,2,1,[10 10 10])
applyjoint(h,2,0,[graus_init+95 0 0]);

i=1;
a=zeros(500,4);
while(i~=501)
    t=readspecial(h,2);
    if (~isempty(t))
        a(i,1:4)=t;
        i=i+1;        
    end
    pause(0.000001)
end
 
applyjoint(h,2,0,[graus_init 0 0]);
i=1;
b=zeros(500,4);
while(i~=501)
    t=readspecial(h,2);
    if (~isempty(t))
        b(i,1:4)=t;
        i=i+1;        
    end
    pause(0.000001)
end

applyjoint(h,2,0,[graus_init-100 0 0]);
i=1;
c=zeros(500,4);
while(i~=501)
    t=readspecial(h,2);
    if (~isempty(t))
        c(i,1:4)=t;
        i=i+1;        
    end
    pause(0.000001)
end

applyjoint(h,2,0,[graus_init 0 0]);
i=1;
d=zeros(500,4);
while(i~=501)
    t=readspecial(h,2);
    if (~isempty(t))
        d(i,1:4)=t;
        i=i+1;        
    end
    pause(0.000001)
end


plot(a)
figure
plot(b)
figure
plot(c)
figure
plot(d)

save acel_teste3_v10_4.mat a b c d
% e=zeros(100,4);
% for i=0:2:90
%     applyjoint(h,2,0,[i 0 0]);
%     %pause(0.02)
%     t=readspecial(h,2);
%     if (~isempty(t))
%         e(i+41,1:4)=t;
%     else
%       %  i=i-1;
%     end
%     i
%     %pause(0.02);
% end
% applyjoint(h,2,0,[0 0 0]);
    