clc close all; clear 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('左脚气压','右脚气压','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; 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; is_up = 0; is_left = 0; is_right = 0; count = 0; mag_last = [0;0;0]; transfer =[0 -1 0; -1 0 0; 0 0 1]; press = zeros(1,18); gyr_data = zeros(1, 3); mag_data = zeros(1, 3); rotate_matrix = eye(3); left_acc = [0,0,0]; left_gyr = [0,0,0]; left_front_mag = [0,0,0]; left_back_mag = [0,0,0]; right_acc = [0,0,0]; right_gyr = [0,0,0]; right_front_mag = [0,0,0]; right_back_mag = [0,0,0]; down = 0; jump = 0; s_rssi = 0; rssi_buff =[]; last_rssi = 0; press_buff = []; gyr_buff = []; h_acc_buff = []; h_acc_norm_buff = []; s_acc_buff = []; h_mag_buff = []; s_mag_buff = []; time_stamp = 0; relate_val = 0; buff_record = []; mag_norm = 0; h_mag_norm_buff = []; s_mag_norm_buff = []; % 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 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) && buf(4) == 4 && Len>52 left_acc(1) = double(bitshift(int16(buf(5)),8)+bitshift(int16(buf(6)),0)) ; left_acc(2) = double(bitshift(int16(buf(7)),8)+bitshift(int16(buf(8)),0)) ; left_acc(3) = double(bitshift(int16(buf(9)),8)+bitshift(int16(buf(10)),0)) ; left_acc_norm = norm(left_acc); left_gyr(1) = double(bitshift(int16(buf(11)),8)+bitshift(int16(buf(12)),0)) ; left_gyr(2) = double(bitshift(int16(buf(13)),8)+bitshift(int16(buf(14)),0)) ; left_gyr(3) = double(bitshift(int16(buf(15)),8)+bitshift(int16(buf(16)),0)) ; left_gyr_norm = norm(left_gyr); left_front_mag(1) = double(bitshift(int16(buf(17)),8)+bitshift(int16(buf(18)),0)); left_front_mag(2) = double(bitshift(int16(buf(19)),8)+bitshift(int16(buf(20)),0)); left_front_mag(3) = double(bitshift(int16(buf(21)),8)+bitshift(int16(buf(22)),0)); left_front_mag_norm = norm(left_front_mag); left_back_mag(1) = double(bitshift(int16(buf(23)),8)+bitshift(int16(buf(24)),0)) left_back_mag(2) = double(bitshift(int16(buf(25)),8)+bitshift(int16(buf(26)),0)); left_back_mag(3) = double(bitshift(int16(buf(27)),8)+bitshift(int16(buf(28)),0)); left_back_mag_norm = norm(left_back_mag); right_acc(1) = double(bitshift(int16(buf(29)),8)+bitshift(int16(buf(30)),0)) ; right_acc(2) = double(bitshift(int16(buf(31)),8)+bitshift(int16(buf(32)),0)) ; right_acc(3) = double(bitshift(int16(buf(33)),8)+bitshift(int16(buf(34)),0)) ; right_acc_norm = norm(right_acc); right_gyr(1) = double(bitshift(int16(buf(35)),8)+bitshift(int16(buf(36)),0)) ; right_gyr(2) = double(bitshift(int16(buf(37)),8)+bitshift(int16(buf(38)),0)) ; right_gyr(3) = double(bitshift(int16(buf(39)),8)+bitshift(int16(buf(40)),0)) ; right_gyr_norm = norm(right_gyr); right_front_mag(1) = double(bitshift(int16(buf(41)),8)+bitshift(int16(buf(42)),0)); right_front_mag(2) = double(bitshift(int16(buf(43)),8)+bitshift(int16(buf(44)),0)); right_front_mag(3) = double(bitshift(int16(buf(45)),8)+bitshift(int16(buf(46)),0)); right_front_mag_norm = norm(right_front_mag); right_back_mag(1) = double(bitshift(int16(buf(47)),8)+bitshift(int16(buf(48)),0)); right_back_mag(2) = double(bitshift(int16(buf(49)),8)+bitshift(int16(buf(50)),0)); right_back_mag(3) = double(bitshift(int16(buf(51)),8)+bitshift(int16(buf(52)),0)); right_back_mag_norm = norm(right_back_mag); buff_record = [buff_record;buf(1:Len)]; dt = dt + 1; t=[t dt]; if dt>250 x=x+1; end % if((size(h_acc_norm_buff,2) > 9) && (max(h_acc_norm_buff(end-9:end)) - 1 < 0.1) && (min(h_acc_norm_buff(end-9:end)) - 1 > -0.1)) % acc_zupt = 1; % else % acc_zupt = 0; % end % %测左右鞋地磁 % m=[m, [ left_front_mag_norm;left_back_mag_norm; right_front_mag_norm ; right_back_mag_norm; 0; 0; 0; 0; ]]; % 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,:)) % % %测左右鞋地磁 % m=[m, [ left_front_mag_norm;left_back_mag_norm; right_front_mag_norm ; right_back_mag_norm; 0; 0; 0; 0; ]]; % 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,:)) % %测左鞋前后地磁原始数据 % m=[m, [ left_front_mag(1);left_front_mag(2); left_front_mag(3) ; left_back_mag(1); left_back_mag(2); left_back_mag(3); 0; 0; ]]; % set(p(1),'XData',t,'YData',m(1,:),'Color','red') % set(p(2),'XData',t,'YData',m(2,:),'Color','green') % set(p(3),'XData',t,'YData',m(3,:),'Color','blue') % set(p(4),'XData',t,'YData',m(4,:),'Color','yellow') % set(p(5),'XData',t,'YData',m(5,:),'Color','black') % set(p(6),'XData',t,'YData',m(6,:),'Color','cyan') % %测右鞋前后地磁原始数据 % m=[m, [ right_front_mag(1);right_front_mag(2); right_front_mag(3) ; right_back_mag(1); right_back_mag(2); right_back_mag(3); 0; 0; ]]; % 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,:)) %测左鞋六轴 m=[m, [ left_acc_norm+4000;left_gyr_norm+3000; left_front_mag_norm+2000 ; left_back_mag_norm+1000;right_acc_norm-1000;right_gyr_norm-2000; right_front_mag_norm-3000 ; right_back_mag_norm-4000; ]]; set(p(1),'XData',t,'YData',m(1,:),'Color','red') set(p(2),'XData',t,'YData',m(2,:),'Color','green') set(p(3),'XData',t,'YData',m(3,:),'Color','blue') set(p(4),'XData',t,'YData',m(4,:),'Color','black') set(p(5),'XData',t,'YData',m(5,:),'Color','red') set(p(6),'XData',t,'YData',m(6,:),'Color','green') set(p(7),'XData',t,'YData',m(7,:),'Color','blue') set(p(8),'XData',t,'YData',m(8,:),'Color','black') % m=[m, [ right_back_mag(1);left_back_mag(1); 0 ; 0; 0;0; 0; 0; ]]; % set(p(1),'XData',t,'YData',m(1,:)) % set(p(2),'XData',t,'YData',m(2,:)) % %测右鞋六轴 % m=[m, [ right_acc(1);right_acc(2); right_acc(3) ; right_gyr(1); right_gyr(2); right_gyr(3); 0; 0; ]]; % 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(6),'XData',t,'YData', m(6,:)) % set(p(7),'XData',t,'YData',m(7,:)) % set(p(8),'XData',t,'YData',m(8,:)) % 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,:)) buf = buf(Len:end); else buf(1) = []; end state = 0; end otherwise state = 0; end end drawnow limitrate nocallbacks % axis([x x+300 -0.1 0.1]); axis([x x+300 -10000 10000]); end fclose(object); delete(object); clear object;