clc
close all;
clear all;

dt = 1;
t=[0];
% m=[0;0;0;0]
m=[0;0;0;0;0;0;0;0;0;]
p = plot(t,m)

data = [];
buf = [];
state = 0;
x=0;
%legend('左脚气压','右脚气压','r2','r3','b0','b1','b2','b3')
legend('前脚','后脚')
grid on;
set(gcf,'unit','normalized','position',[0.2,0.2,0.64,0.7]);
delete(instrfindall('Type','serial'));%清理串口
object = serial('com3','BaudRate',115200);%配置串口
fopen(object);

while true
    data=fread(object,230);%读取数据
    while ~isempty(data)
        buf = [buf data(1)];
        data(1) = [];
        switch state
            case 0
               if length(buf)>=5
                   if  buf(1)==170
                       Len = buf(2);
                       Lenf = 255 - buf(2);
                       if buf(3) == Lenf
                          state = 1;
                       else
                          buf(1) = [];
                       end
                   else
                       buf(1) = [];
                   end
               end
            case 1
                if length(buf)>=Len
                  ver = 0;
                   for i=1:(Len-1)
                       ver = ver + buf(i);
                   end
                   ver = mod(ver,256);
    %                disp(ver);
    %                disp(buf(Len));
                   if ver==buf(Len) 
                      if buf(4) == 0
                            disp(buf);
                             ax_n = double(bitshift(int16(buf(5)),8)+bitshift(int16(buf(6)),0))/100;
                             ay_n = double(bitshift(int16(buf(7)),8)+bitshift(int16(buf(8)),0))/100;
                             az_n = double(bitshift(int16(buf(9)),8)+bitshift(int16(buf(10)),0))/100;
                             
                             gx_n = double(bitshift(int16(buf(11)),8)+bitshift(int16(buf(12)),0))/100;
                             gy_n = double(bitshift(int16(buf(13)),8)+bitshift(int16(buf(14)),0))/100;
                             gz_n = double(bitshift(int16(buf(15)),8)+bitshift(int16(buf(16)),0))/100;
                             
                             mx_n = double(bitshift(int16(buf(17)),8)+bitshift(int16(buf(18)),0))/100;
                             my_n = double(bitshift(int16(buf(19)),8)+bitshift(int16(buf(20)),0))/100;
                             mz_n = double(bitshift(int16(buf(21)),8)+bitshift(int16(buf(22)),0))/100;
                            
                            m = [m [ax_n;ay_n;az_n;gx_n;gy_n;gz_n;mx_n;my_n;mz_n;]];
                            
                            dt = dt + 1;
                            t=[t dt];
                            if dt>250 
                                x=x+1; 
                            end
                      end
%                              set(p(1),'XData',t,'YData',m(1,:))
                             set(p(2),'XData',t,'YData',m(2,:))
%                              set(p(3),'XData',t,'YData',m(3,:))
%                              set(p(4),'XData',t,'YData',m(4,:))
%                              set(p(5),'XData',t,'YData',m(5,:))
%                              set(p(6),'XData',t,'YData',m(6,:))
%                              set(p(7),'XData',t,'YData',m(7,:))
%                              set(p(8),'XData',t,'YData',m(8,:))
%                              set(p(9),'XData',t,'YData',m(9,:))
                   
                        buf = buf(Len:end);
                   else
                       buf(1) = [];
                   end
                   state = 0;
                end
            otherwise
                state = 0;
        end
    end
    drawnow limitrate nocallbacks
	axis([x x+300 -5 5]); 
end

fclose(object);
delete(object);
clear object;