zhengxuelong/未整理/matlab/temp/savedata_uart_press.m

clc
close all;
clear all;

dt = 1;
t=[0];
% m=[0;0;0;0]
dis_m=[0;0;0;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
m = [0;0;]


%算法变量
F_val_now_back = 0;
B_val_now_back = 0;
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_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;

while true
    data=fread(object,15);%读取数据
    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));
                            
                            F_val_now = 4096-double(bitshift(uint16(buf(6)),8)+bitshift(uint16(buf(7)),0));
                            B_val_now = 4096-double(bitshift(uint16(buf(8)),8)+bitshift(uint16(buf(9)),0));
                            F_val_now_back = 4096-double(bitshift(uint16(buf(10)),8)+bitshift(uint16(buf(11)),0));
                            B_val_now_back = 4096-double(bitshift(uint16(buf(12)),8)+bitshift(uint16(buf(13)),0));
                            
                            F_val_sum = F_val_now + B_val_now;
                            
                            if F_val_sum>2080
                                F_step_zero = 3000
                            else
                                F_step_zero = 1500
                            end
    
%                             F_val_now = (F_val_now+B_val_now);
                            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);
%  if abs(F_filter_val-F_val_now)>200
%          F_filter_val = F_val_now;
%      end
%     F_filter_val = (F_filter_val*3-F_filter_val+F_val_now)/3;
%     disp(F_val_now);
%     disp(F_filter_val);
    
%     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>500
%                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>200
%                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;
    
%                             
%                             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>90 
                                x=x+1; 
                            end
                             dis_m =[dis_m [ F_val_now;  B_val_now;  F_val_sum;F_step_zero;F_val_now_back;B_val_now_back]];
%                              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,:))
                             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,:))
                   
                        buf = buf(Len:end);
                   else
                       buf(1) = [];
                   end
                   state = 0;
                end
            otherwise
                state = 0;
        end
    end
    drawnow limitrate nocallbacks
	axis([x x+100 800 4000]); 
end

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