clc
close all;
clear all;

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

data = [];
buf = [];

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);

state = 0;

transfer =[0 -1 0; -1 0 0; 0 0 1];
press_1 = zeros(1,18);
press_2 = zeros(1,18);

time_stamp = 0;
time_stamp_last = 0;
% for i=1:shuliang



%算法变量
F_val_now = 0;
B_val_now = 0;
F_rise_start = 0;
F_val_old = 0;
F_flag_step = 0;
F_filter_val = 0;
m = [0;0;]

while true
    data=fread(object,30);%读取数据
    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) == 165
%                             press_2 = double(bitshift(uint16(buf(6)),8)+bitshift(uint16(buf(7)),0));
%                             press_1 = double(bitshift(uint16(buf(8)),8)+bitshift(uint16(buf(9)),0));
                            
                            B_val_now = double(bitshift(uint16(buf(6)),8)+bitshift(uint16(buf(7)),0));
                            F_val_now = double(bitshift(uint16(buf(8)),8)+bitshift(uint16(buf(9)),0));
                            m = [m [F_val_now;B_val_now]];

                            if F_flag_step>0
                               F_flag_step = 0; 
                            end

                            %算法
                             F_filter_val = bitshift((bitshift(uint32(F_filter_val),1)-F_filter_val+F_val_now),-1);
                        %     F_filter_val = (F_filter_val*3-F_filter_val+F_val_now)/3;
                            disp(F_val_now);
                            disp(F_filter_val);


                            if F_filter_val>=F_val_old
                                if F_rise_start==0
                                    F_rise_start = F_val_old;
                                end
                            else
                                if F_rise_start>0
                                    if F_val_old-F_rise_start>500
                                       F_flag_step = F_rise_start;
                                    end
                                    F_rise_start = 0;
                                end
                            end
                            F_val_old = F_filter_val;
    
                            
                            time_stamp = bitshift(uint16(buf(10)),0);
                            if time_stamp ~= (time_stamp_last+1)
                                disp(['data loss:',num2str(time_stamp)]);
                            end
                            time_stamp_last = time_stamp;
%                             disPress(1) = press_1;
%                             disPress(2) = press_2;
%                             disPress(1) = time_stamp;
%                             disp(disPress);
                            dt = dt + 1;
                            
                            t=[t dt];
                            if dt>250 
                                x=x+1; 
                            end
                             dis_m =[dis_m [ F_val_now;  F_filter_val;  F_flag_step]];
%                              m=[m [ mag_x;  mag_y; mag_z;press ;0 ;0; 0; 0; 0;]];
%                       else
%                             tempbu = double(bitshift(uint16(buf(6)),24)+bitshift(uint16(buf(7)),16)+bitshift(uint16(buf(8)),8)+bitshift(uint16(buf(9)),0));
%                             press_max = double(bitshift(uint16(buf(10)),8)+bitshift(uint16(buf(11)),0));
%                             press_in = double(bitshift(uint16(buf(12)),8)+bitshift(uint16(buf(13)),0));
%                             X = sprintf('press_max:%d,press_min:%d,step:%d',press_max,press_min,tempbu);
%                             
                      end
                             set(p(1),'XData',t,'YData',dis_m(1,:))
                             set(p(2),'XData',t,'YData',dis_m(2,:))
                             set(p(3),'XData',t,'YData',dis_m(3,:))
                   
                        buf = buf(Len:end);
                   else
                       buf(1) = [];
                   end
                   state = 0;
                end
            otherwise
                state = 0;
        end
    end
    drawnow limitrate nocallbacks
	axis([x x+300 -500 6000]); 
end

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