clc
close all;
clear all;

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

data = [];
buf = [];

x=0;
legend('x','y','z','压力')
grid on;
set(gcf,'unit','normalized','position',[0.2,0.2,0.64,0.7]);

delete(instrfindall('Type','serial'));%清理串口
object = serial('com4','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;
press_data = [];
press_var = [];
gyr_var = [];
pos_data = [];

imu_data = [];

is_down = 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
                           
                       end
                       if buf(6)==1 && 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');
                                is_down = 1;
                            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)==1 && buf(7)==0
                            
                             press(1)=double(bitshift(int32(buf(8)),24)+bitshift(int32(buf(9)),16)+bitshift(int32(buf(10)),8)+int32(buf(11)));
                             press(2)=double(bitshift(int32(buf(12)),24)+bitshift(int32(buf(13)),16)+bitshift(int32(buf(14)),8)+int32(buf(15)));
                             time = double(bitshift(int16(buf(16)),8)+int16(buf(17)));
                            disp([press time])
                            
                            dt = dt + 1;
                            t=[t dt];
                            if dt>250 
                                x=x+1; 
                            end
                            imu_data = [imu_data [press(1);press(2);time]];
                            m=[m [(press(1)- 9550000)/10e4;(press(2)-9550000)/10e4;time]];
                                 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,:))
%                                             is_down = 0;
%                                        set(p(1),'XData',t,'YData',m(5,:))
%                                         set(p(2),'XData',t,'YData',m(6,:))
%                                    set(p(3),'XData',t,'YData',m(7,:))
%                                  set(p(8),'XData',t,'YData',m(8,:))
%                                 set(p(9),'XData',t,'YData',m(9,:))
%                                set(p(10),'XData',t,'YData',m(10,:))
                       end
                        buf = buf(Len:end);
                   else
                       buf(1) = [];
                   end
                   state = 0;
                end
            otherwise
                state = 0;
        end
    end
    drawnow limitrate nocallbacks
	axis([x x+300 0 10]); 
end

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