clc
close all;
clear all;

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

%legend('左脚气压','右脚气压','r2','r3','b0','b1','b2','b3')
legend('前脚','后脚')
grid on;
set(gcf,'unit','normalized','position',[0.2,0.2,0.64,0.7]);

load('威严-P20210823.mat');
clc
buff_len = length(m(1,:));



%算法变量
F_val_now = 0;
B_val_now = 0;
F_rise_start = 0;
F_val_old = 0;
F_flag_step = 0;
F_filter_val = 0;
F_filter_val_now = 0;
B_filter_val_now = 0;
F_step_time_1 = 0;
F_step_time_2 = 0;
F_step_time_min = 0;
F_step_time_max = 0;
F_step_time_cnt = 0;

F_step_zero = 0;

for k = 1:buff_len
    F_val_now = m(1,k);
    B_val_now = m(2,k);
    
    if F_flag_step>0
       F_flag_step = 0; 
    end
    
    F_val_sum = F_val_now + B_val_now;
    %算法
%      F_filter_val = bitshift((bitshift(uint32(F_filter_val),3)-F_filter_val+F_val_sum),-3);
     
%     F_filter_val = (F_filter_val*3-F_filter_val+F_val_now)/3;
%     disp(F_val_now);
%     disp(F_filter_val);

%     if abs(F_filter_val-F_val_sum)>200
%         F_filter_val = F_val_sum;
%     end
    
    if F_val_sum>1900
        F_step_zero = 3000
    else
        F_step_zero = 1500
    end
    
%     F_step_time_cnt = F_step_time_cnt + 1;
    
%     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>800
%                F_flag_step = 1000;
%                F_step_time_1 = F_step_time_2;
%                F_step_time_2 = F_step_time_cnt;
%                F_step_time_cnt = 0;
%             elseif F_val_old-F_rise_start>500
%                tempV = [F_step_time_1,F_step_time_2];
%                F_step_time_min = min(tempV);
%                F_step_time_max = max(tempV);
%                if F_step_time_max-F_step_time_min<3 && abs((F_step_time_max+F_step_time_min)/2-F_step_time_cnt)<5
%                    F_flag_step = 500;
%                else
%                    F_flag_step = 100;
%                end
%                F_step_time_1 = F_step_time_2;
%                F_step_time_2 = F_step_time_cnt;
%                F_step_time_cnt = 0;
%             end          
%             F_rise_start = 0;
%         end
%     end
%     F_val_old = F_filter_val;
    
    dt = dt + 1;     
	t=[t dt];
	if dt>280 
        x=x+1; 
    end
	dis_m=[dis_m [ F_val_now;B_val_now; F_val_sum;F_step_zero; F_val_sum; F_filter_val]];
    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,:))
    set(p(4),'XData',t,'YData',dis_m(4,:))
%     set(p(5),'XData',t,'YData',dis_m(5,:))
%     set(p(6),'XData',t,'YData',dis_m(6,:))
    drawnow limitrate nocallbacks
	axis([x x+300 -100 5000]); 
%      pause(0.001);
end