clc close all; clear all; dt = 1; t=[0]; % m=[0;0;0;0] m=[0;0;0;0;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('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 = []; % 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)==0 % acc(1)=double(bitshift(int32(buf(8)),24)+bitshift(int32(buf(9)),16)+bitshift(int32(buf(10)),8)+int32(buf(11))); % acc(2)=double(bitshift(int32(buf(12)),24)+bitshift(int32(buf(13)),16)+bitshift(int32(buf(14)),8)+int32(buf(15))); % acc(3)=double(bitshift(int32(buf(16)),24)+bitshift(int32(buf(17)),16)+bitshift(int32(buf(18)),8)+int32(buf(19))); % acc(1)=double(bitshift(int16(buf(8)),8)+int16(buf(9))); % acc(2)=double(bitshift(int16(buf(10)),8)+int16(buf(11))); % acc(3)=double(bitshift(int16(buf(12)),8)+int16(buf(13))); % disp(acc); gry(1)=double(bitshift(int16(buf(8)),8)+int16(buf(9))); gry(2)=double(bitshift(int16(buf(10)),8)+int16(buf(11))); gry(3)=double(bitshift(int16(buf(12)),8)+int16(buf(13))); % gyr_norm = [gyr_norm norm([gry(1),gry(2),gry(3)])./16.384*pi/180]; acc(1)=double(bitshift(int16(buf(14)),8)+int16(buf(15))); acc(2)=double(bitshift(int16(buf(16)),8)+int16(buf(17))); acc(3)=double(bitshift(int16(buf(18)),8)+int16(buf(19))); % disp(gry); press(1)=double(bitshift(int32(buf(20)),24)+bitshift(int32(buf(21)),16)+bitshift(int32(buf(22)),8)+int32(buf(23))); press_data = [press_data press(1)]; % disp(press(1)); pos_res(1) = double(bitshift(int16(buf(24)),8)+int16(buf(25))); pos_res(2) = double(bitshift(int16(buf(26)),8)+int16(buf(27))); pos_res(3) = double(bitshift(int16(buf(28)),8)+int16(buf(29))); pos_data = [pos_data [pos_res(1)/10000;pos_res(2)/10000]]; signal_out = int16(buf(30)); if(signal_out == 2) disp("MOTION_JUMP") elseif(signal_out == 3) disp("MOTION_DOWN") max(press_data(end-6:end)) - min(press_data(end-6:end)) elseif(signal_out == 4) disp("MOTION_LEFT") elseif(signal_out == 5) disp("MOTION_RIGHT") end if(norm(pos_data(1:2,end)) > 0.3) % pca_filter(pos_data(:,end-9:end)); end % T = [-0.7949 -0.6067;0.6067 -0.7949]; T = [1 0; 0 1]; pos_data(1:2,end) = T * [pos_res(1)/10000;pos_res(2)/10000]; % adc(1)=double(bitshift(int16(buf(20)),8)+int16(buf(21))); % adc(2)=double(bitshift(int16(buf(22)),8)+int16(buf(23))); % signal_out(1)=double(bitshift(int32(buf(24)),24)+bitshift(int32(buf(25)),16)+bitshift(int32(buf(26)),8)+int32(buf(27))); % signal_out(2)=double(bitshift(int32(buf(28)),24)+bitshift(int32(buf(29)),16)+bitshift(int32(buf(30)),8)+int32(buf(31))); % signal_out(3)=double(bitshift(int32(buf(32)),24)+bitshift(int32(buf(33)),16)+bitshift(int32(buf(34)),8)+int32(buf(35))); % norm([acc(1);acc(2);acc(3);]./4096/1.1992*9.88) % datestr(now,'mmmm dd,yyyy HH:MM:SS.FFF AM') % pos = footPDR(dt, [gry(1);gry(2);gry(3)]./16.384*pi/180, [acc(1);acc(2);acc(3);]./4096/1.1992*9.7833, press(1)); % disp(press(1)); dt = dt + 1; t=[t dt]; if dt>250 x=x+1; end imu_data = [imu_data [gry(1)/16.384*pi/180;gry(2)/16.384*pi/180;gry(3)/16.384*pi/180;press(1);]]; m=[m [gry(1)/16.384*pi/180;gry(2)/16.384*pi/180;gry(3)/16.384*pi/180;(press(1)-9000000)*0.0000005;pos_data(1,end)+2;pos_data(2,end)-2;pos_res(3)/10000]]; % 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(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 axis([x x+300 -10 10]); end fclose(object); delete(object); clear object;