clc
close all;

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

data = [];
buf = [];

x=0;
legend('前后','左右','上下','压力')
grid on;
% set(gcf,'unit','normalized','position',[0.2,0.2,0.64,0.7]);
set(gcf,'unit','normalized','position',[0.2,0.2,0.64,0.7],'Interruptible','off');

delete(instrfindall('Type','serial'));%清理串口
object = serial('com5','BaudRate',115200);%配置串口
fopen(object);

state = 0;
Len = 0;
Lenf = 0;

Zarr = [];
Zarr(1) = 0;
Zarr_en  = 0;
run = 0;
left = 0;
right = 0;
acc3_b = 0;
acc3_bb = 0;
% for i=1:shuliang
while true
    data=fread(object);%读取数据
    while ~isempty(data)
        buf = [buf data(1)];
        data(1) = [];
        switch state
            case 0
               if length(buf)>=5
                   if  buf(1)==170 && buf(2)==187 && buf(3)==204
                       Len = buf(4);
                       Lenf = 255 - buf(5);
                       if Len==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(6)==0 && buf(7)==1
                            clc
                            disp(buf);
                            if buf(8)==0
                                disp('MOTION_STOP');
                            elseif buf(8)==1
                                disp('MOTION_RUN');
                            elseif buf(8)==2
                                disp('MOTION_JUMP');
                            elseif buf(8)==3
                                disp('MOTION_DOWN');
                            elseif buf(8)==4
                                disp('MOTION_LEFT');
                            elseif buf(8)==5
                                disp('MOTION_RIGHT');
                            elseif buf(8)==6
                                disp('MOTION_FRONT');
                            elseif buf(8)==7
                                disp('MOTION_BACK');
                            end
                            disp(double(bitshift(int16(buf(9)),8)+int16(buf(10))));
                       end
                       if buf(6)==0 && buf(7)==0
%                             acc(1)=double(bitshift(int32(buf(8)),24)+bitshift(int32(buf(9)),16)+bitshift(int32(buf(10)),8)+int32(buf(11)));
%                             acc(2)=double(bitshift(int32(buf(12)),24)+bitshift(int32(buf(13)),16)+bitshift(int32(buf(14)),8)+int32(buf(15)));
%                             acc(3)=double(bitshift(int32(buf(16)),24)+bitshift(int32(buf(17)),16)+bitshift(int32(buf(18)),8)+int32(buf(19)));
                            
                            
                             press(1)=double(bitshift(uint16(buf(8)),8)+uint16(buf(9)));
                             press(2)=double(bitshift(uint16(buf(10)),8)+uint16(buf(11)));
                             disp(press);
                             press(1) = (press(1) - 39900)/1000.0;
                             press(2) = (press(2) - 35000)/1000.0;
                              
                              
                            acc(1)=double(bitshift(int16(buf(12)),8)+int16(buf(13)));
                            acc(2)=double(bitshift(int16(buf(14)),8)+int16(buf(15)));
                            acc(3)=double(bitshift(int16(buf(16)),8)+int16(buf(17)));
                            
                            acc(4)=double(bitshift(int16(buf(18)),8)+int16(buf(19)));
                            acc(5)=double(bitshift(int16(buf(20)),8)+int16(buf(21)));
                            acc(6)=double(bitshift(int16(buf(22)),8)+int16(buf(23)));
                            acc = acc/8192.0;
                            disp(acc);


                            dt = dt + 1;
                            t=[t dt];
                            if dt>450 
                                x=x+1; 
                            end
                            
                            m=[m [acc(1)+6;acc(2)+6;acc(3)+6;acc(4)+2;acc(5)+2;acc(6)+2;press(1)-8;press(2)-8]];
                                   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,:))
                       end
                        buf = buf(Len:end);
                   else
                       buf(1) = [];
                   end
                   state = 0;
                end
            otherwise
                state = 0;
        end
    end

	axis([x x+500 -10 10]); 
     drawnow limitrate nocallbacks
end

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