Tinymal可以通过上位机完成对实时数据的记录，数据存储地址为Corgi/Data的txt文本，记录方式为连接机器人后点击上位机中的记录按键：
开始记录后上位机不再会对数据进行刷新，但是操控指令可以正常下达，点击结束后数据将完成保存，此时采用WinSCP将txt数据文件拷贝到PC本地，并采用我们提供的MATLAB脚本进行数据绘制：
在记录中有时候会出现遥控指令无法下达的Bug，此时需要关闭记录重新开始记录即可
下面为Matlab脚本，其中ST和ADD可以选择绘制区域：
function plot12
clc;
clear all;
close all;
data=importdata('./file1.txt');
exp_p=data(:,1);
LL=length(exp_p);
ST=0.7; % \\\\\\\\\\\\\\\\\\\\LF\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\;
ADD=0.9;
ADDS=400/LL;
ED=ST+ADDS;
Draw_sel=1;
if ED>1
ED=1;
end
data=importdata('./file1.txt');
exp_p=data(:,1);
now_p=data(:,2);
exp_r=data(:,3);
now_r=data(:,4);
exp_y=data(:,5);
now_y=data(:,6);
now_pr=data(:,7);
now_rr=data(:,8);
now_yr=data(:,9);
now_cog_xr=limit(data(:,10),-0.5,0.5);
now_cog_x=limit(data(:,11),-0.5,0.5);
now_cog_dx=limit(data(:,12),-0.5,0.5);
now_cog_yr=limit(data(:,13),-0.5,0.5);
now_cog_y=limit(data(:,14),-0.5,0.5);
now_cog_dy=limit(data(:,15),-0.5,0.5);
now_cog_zr=limit(data(:,16),-0.5,0.5);
now_cog_z=limit(data(:,17),-0.5,0.5);
now_cog_dz=limit(data(:,18),-0.5,0.5);
g_pit=limit(data(:,19),-0.5,0.5);
g_rol=limit(data(:,20),-0.5,0.5);
tar_spdx=limit(data(:,21),-0.5,0.5);
tar_spdy=limit(data(:,22),-0.5,0.5);
tar_spdz=limit(data(:,23),-0.5,0.5);
%
data=importdata('./file2.txt'); %足底力
exp_fx0=data(:,1);
now_fx0=data(:,2);
exp_fy0=data(:,3);
now_fy0=data(:,4);
exp_fz0=data(:,5);
now_fz0=data(:,6);
exp_fx1=data(:,7);
now_fx1=data(:,8);
exp_fy1=data(:,9);
now_fy1=data(:,10);
exp_fz1=data(:,11);
now_fz1=data(:,12);
exp_fx2=data(:,13);
now_fx2=data(:,14);
exp_fy2=data(:,15);
now_fy2=data(:,16);
exp_fz2=data(:,17);
now_fz2=data(:,18);
exp_fx3=data(:,19);
now_fx3=data(:,20);
exp_fy3=data(:,21);
now_fy3=data(:,22);
exp_fz3=data(:,23);
now_fz3=data(:,24);
g_flag0=data(:,25);
touch_flag0=data(:,26);
g_flag1=data(:,27);
touch_flag1=data(:,28);
g_flag2=data(:,29);
touch_flag2=data(:,30);
g_flag3=data(:,31);
touch_flag3=data(:,32);
trig_state0=data(:,33);
trig_state1=data(:,34);
trig_state2=data(:,35);
trig_state3=data(:,36);
leg0_q0_exp= data(:,37);
leg0_q1_exp= data(:,38);
leg0_q2_exp= data(:,39);
leg0_q0_now= data(:,40);
leg0_q1_now= data(:,41);
leg0_q2_now= data(:,42);
leg0_posx_h_exp=data(:,43);
leg0_posy_h_exp=data(:,44);
leg0_posz_h_exp=data(:,45);
leg0_posx_h_now=data(:,46);
leg0_posy_h_now=data(:,47);
leg0_posz_h_now=data(:,48);
leg1_q0_exp= data(:,49);
leg1_q1_exp= data(:,50);
leg1_q2_exp= data(:,51);
leg1_q0_now= data(:,52);
leg1_q1_now= data(:,53);
leg1_q2_now= data(:,54);
leg1_posx_h_exp=data(:,55);
leg1_posy_h_exp=data(:,56);
leg1_posz_h_exp=data(:,57);
leg1_posx_h_now=data(:,58);
leg1_posy_h_now=data(:,59);
leg1_posz_h_now=data(:,60);
leg2_q0_exp= data(:,61);
leg2_q1_exp= data(:,62);
leg2_q2_exp= data(:,63);
leg2_q0_now= data(:,64);
leg2_q1_now= data(:,65);
leg2_q2_now= data(:,66);
leg2_posx_h_exp=data(:,67);
leg2_posy_h_exp=data(:,68);
leg2_posz_h_exp=data(:,69);
leg2_posx_h_now=data(:,70);
leg2_posy_h_now=data(:,71);
leg2_posz_h_now=data(:,72);
leg3_q0_exp= data(:,73);
leg3_q1_exp= data(:,74);
leg3_q2_exp= data(:,75);
leg3_q0_now= data(:,76);
leg3_q1_now= data(:,77);
leg3_q2_now= data(:,78);
leg3_posx_h_exp=data(:,79);
leg3_posy_h_exp=data(:,80);
leg3_posz_h_exp=data(:,81);
leg3_posx_h_now=data(:,82);
leg3_posy_h_now=data(:,83);
leg3_posz_h_now=data(:,84);
leg0_t0_exp= data(:,85);
leg0_t1_exp= data(:,86);
leg0_t2_exp= data(:,87);
leg0_t0_now= data(:,88);
leg0_t1_now= data(:,89);
leg0_t2_now= data(:,90);
leg1_t0_exp= data(:,91);
leg1_t1_exp= data(:,92);
leg1_t2_exp= data(:,93);
leg1_t0_now= data(:,94);
leg1_t1_now= data(:,95);
leg1_t2_now= data(:,96);
leg2_t0_exp= data(:,97);
leg2_t1_exp= data(:,98);
leg2_t2_exp= data(:,99);
leg2_t0_now= data(:,100);
leg2_t1_now= data(:,101);
leg2_t2_now= data(:,102);
leg3_t0_exp= data(:,103);
leg3_t1_exp= data(:,104);
leg3_t2_exp= data(:,105);
leg3_t0_now= data(:,106);
leg3_t1_now= data(:,107);
leg3_t2_now= data(:,108);
data=importdata('./file3.txt'); %unuse
temp0=data(:,1);
temp1=data(:,2);
temp2=data(:,3);
temp3=data(:,4);
temp4=data(:,5);
temp5=data(:,6);
temp6=data(:,7);
temp7=data(:,8);
temp8=data(:,9);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
L=length(now_cog_dz);
P_ST=int16(ST*L);%选择显示区域
P_END=int16(ED*L);
g_flag_4all=data(:,60);
g_flag_0all=data(:,60);
for i=1:L
if g_flag0(i)+g_flag1(i)+g_flag2(i)+g_flag3(i)>=3
g_flag_4all(i)=g_flag0(i)+g_flag1(i)+g_flag2(i)+g_flag3(i);
else
g_flag_4all(i)=0;
end
if g_flag0(i)==0 &&g_flag1(i)==0 &&g_flag2(i)==0 &&g_flag3(i)==0
g_flag_0all(i)=-2;
else
g_flag_0all(i)=0;
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if Draw_sel==0
%1 2 3 4
%5 6 7 8
%9 10 11 12
figure('NumberTitle', 'off', 'Name', '右前0腿足底力')
% subplot(3,5,1);
% plot(g_flag0(P_ST:P_END),'-k','LineWidth',2);
% hold on;
% plot(touch_flag0(P_ST:P_END),'-.r');
% grid on;
% ylabel('着地状态');
%
% subplot(3,5,6);
% plot(trig_state0(P_ST:P_END),'LineWidth',2);
% ylim([0,4]);
% ylabel('摆动相序');
% grid on;
subplot(3,4,1);
plot(now_fx0(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(exp_fx0(P_ST:P_END),'-.r','LineWidth',2);
grid on;
ylabel('X方向力');
%ylim([-50,50]);
subplot(3,4,5);
plot(now_fy0(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(exp_fy0(P_ST:P_END),'-.r','LineWidth',2);
grid on;
ylabel('Y方向力');
%ylim([-50,50]);
subplot(3,4,9);
plot(now_fz0(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(exp_fz0(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state0(P_ST:P_END)*20,'LineWidth',1);
hold on;
plot(touch_flag0(P_ST:P_END)*20,'LineWidth',1);
hold on;
grid on;
ylabel('Z方向力');
ylim([-50,90]);
subplot(3,4,2);
plot(leg0_posx_h_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg0_posx_h_exp(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state0(P_ST:P_END)/50+leg0_posx_h_exp(P_ST),'LineWidth',2);
ylabel('Hip X');
grid on;
subplot(3,4,6);
plot(leg0_posy_h_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg0_posy_h_exp(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state0(P_ST:P_END)/50+leg0_posy_h_exp(P_ST),'LineWidth',2);
hold on;
plot(touch_flag0(P_ST:P_END)/50+leg3_posy_h_exp(P_ST),'LineWidth',2);
ylabel('Hip Y');
grid on;
subplot(3,4,10);
plot(leg0_posz_h_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg0_posz_h_exp(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state0(P_ST:P_END)/100+leg0_posz_h_now(P_ST),'LineWidth',2);
ylabel('Hip Z');
%ylim([-0.2,-0.1]);
grid on;
subplot(3,4,3);
plot(leg0_q0_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg0_q0_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('Q0');
grid on;
subplot(3,4,7);
plot(leg0_q1_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg0_q1_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('Q1');
grid on;
subplot(3,4,11);
plot(leg0_q2_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg0_q2_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('Q2');
grid on;
subplot(3,4,4);
plot(leg0_t0_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg0_t0_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('T0');
grid on;
subplot(3,4,8);
plot(leg0_t1_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg0_t1_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('T1');
grid on;
subplot(3,4,12);
plot(leg0_t2_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg0_t2_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('T2');
grid on;
end
if Draw_sel==1
%-----------------------
figure('NumberTitle', 'off', 'Name', '右后1腿足底力')
%1 2 3 4
%5 6 7 8
%9 10 11 12
% subplot(3,5,1);
% plot(g_flag1(P_ST:P_END),'-k','LineWidth',2);
% hold on;
% plot(touch_flag1(P_ST:P_END),'-.r');
% grid on;
% ylabel('着地状态');
%
% subplot(3,5,6);
% plot(trig_state1(P_ST:P_END),'LineWidth',2);
% ylim([0,4]);
% ylabel('摆动相序');
% grid on;
subplot(3,4,1);
plot(now_fx1(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(exp_fx1(P_ST:P_END),'-.r','LineWidth',2);
grid on;
ylabel('X方向力');
%ylim([-50,50]);
subplot(3,4,5);
plot(now_fy1(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(exp_fy1(P_ST:P_END),'-.r','LineWidth',2);
grid on;
ylabel('Y方向力');
%ylim([-50,50]);
subplot(3,4,9);
plot(now_fz1(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(exp_fz1(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state1(P_ST:P_END)*20,'LineWidth',1);
grid on;
hold on;
plot(touch_flag1(P_ST:P_END)*20,'LineWidth',1);
hold on;
ylabel('Z方向力');
ylim([-50,90]);
subplot(3,4,2);
plot(leg1_posx_h_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg1_posx_h_exp(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state1(P_ST:P_END)/50+leg1_posx_h_exp(P_ST),'LineWidth',2);
ylabel('Hip X');
grid on;
subplot(3,4,6);
plot(leg1_posy_h_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg1_posy_h_exp(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state1(P_ST:P_END)/50+leg1_posy_h_exp(P_ST),'LineWidth',2);
hold on;
plot(touch_flag1(P_ST:P_END)/50+leg3_posy_h_exp(P_ST),'LineWidth',2);
ylabel('Hip Y');
grid on;
subplot(3,4,10);
plot(leg1_posz_h_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg1_posz_h_exp(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state1(P_ST:P_END)/100+leg1_posz_h_now(P_ST),'LineWidth',2);
ylabel('Hip Z');
%ylim([-0.2,-0.1]);
grid on;
subplot(3,4,3);
plot(leg1_q0_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg1_q0_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('Q0');
grid on;
subplot(3,4,7);
plot(leg1_q1_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg1_q1_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('Q1');
grid on;
subplot(3,4,11);
plot(leg1_q2_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg1_q2_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('Q2');
grid on;
subplot(3,4,4);
plot(leg1_t0_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg1_t0_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('T0');
grid on;
subplot(3,4,8);
plot(leg1_t1_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg1_t1_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('T1');
grid on;
subplot(3,4,12);
plot(leg1_t2_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg1_t2_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('T2');
grid on;
%---------------------------
figure('NumberTitle', 'off', 'Name', '左前2足底力')
%1 2 3 4
%5 6 7 8
%9 10 11 12
% subplot(3,5,1);
% plot(g_flag2(P_ST:P_END),'-k','LineWidth',2);
% hold on;
% plot(touch_flag2(P_ST:P_END),'-.r');
% grid on;
% ylabel('着地状态');
%
% subplot(3,5,6);
% plot(trig_state2(P_ST:P_END),'LineWidth',2);
% ylim([0,4]);
% ylabel('摆动相序');
% grid on;
subplot(3,4,1);
plot(now_fx2(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(exp_fx2(P_ST:P_END),'-.r','LineWidth',2);
grid on;
ylabel('X方向力');
%ylim([-50,50]);
subplot(3,4,5);
plot(now_fy2(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(exp_fy2(P_ST:P_END),'-.r','LineWidth',2);
grid on;
ylabel('Y方向力');
%ylim([-50,50]);
subplot(3,4,9);
plot(now_fz2(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(exp_fz2(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state2(P_ST:P_END)*20,'LineWidth',1);
grid on;
hold on;
plot(touch_flag2(P_ST:P_END)*20,'LineWidth',1);
hold on;
ylabel('Z方向力');
ylim([-50,90]);
subplot(3,4,2);
plot(leg2_posx_h_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg2_posx_h_exp(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state2(P_ST:P_END)/50+leg2_posx_h_exp(P_ST),'LineWidth',2);
ylabel('Hip X');
grid on;
subplot(3,4,6);
plot(leg2_posy_h_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg2_posy_h_exp(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state2(P_ST:P_END)/50+leg2_posy_h_exp(P_ST),'LineWidth',2);
hold on;
plot(touch_flag2(P_ST:P_END)/50+leg3_posy_h_exp(P_ST),'LineWidth',2);
ylabel('Hip Y');
grid on;
subplot(3,4,10);
plot(leg2_posz_h_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg2_posz_h_exp(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state2(P_ST:P_END)/100+leg2_posz_h_now(P_ST),'LineWidth',2);
ylabel('Hip Z');
%ylim([-0.2,-0.1]);
grid on;
subplot(3,4,3);
plot(leg2_q0_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg2_q0_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('Q0');
grid on;
subplot(3,4,7);
plot(leg2_q1_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg2_q1_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('Q1');
grid on;
subplot(3,4,11);
plot(leg2_q2_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg2_q2_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('Q2');
grid on;
subplot(3,4,4);
plot(leg2_t0_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg2_t0_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('T0');
grid on;
subplot(3,4,8);
plot(leg2_t1_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg2_t1_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('T1');
grid on;
subplot(3,4,12);
plot(leg2_t2_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg2_t2_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('T2');
grid on;
end
% -----------------
if Draw_sel==0
figure('NumberTitle', 'off', 'Name', '左后3足底力')
%1 2 3 4
%5 6 7 8
%9 10 11 12
% subplot(3,5,1);
% plot(g_flag3(P_ST:P_END),'-k','LineWidth',2);
% hold on;
% plot(touch_flag3(P_ST:P_END),'-.r');
% grid on;
% ylabel('着地状态');
%
% subplot(3,5,6);
% plot(trig_state3(P_ST:P_END),'LineWidth',2);
% ylim([0,4]);
% ylabel('摆动相序');
% grid on;
subplot(3,4,1);
plot(now_fx3(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(exp_fx3(P_ST:P_END),'-.r','LineWidth',2);
grid on;
ylabel('X方向力');
%ylim([-50,50]);
subplot(3,4,5);
plot(now_fy3(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(exp_fy3(P_ST:P_END),'-.r','LineWidth',2);
grid on;
ylabel('Y方向力');
%ylim([-50,50]);
subplot(3,4,9);
plot(now_fz3(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(exp_fz3(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state3(P_ST:P_END)*20,'LineWidth',1);
grid on;
hold on;
plot(touch_flag3(P_ST:P_END)*20,'LineWidth',1);
hold on;
ylabel('Z方向力');
ylim([-50,90]);
subplot(3,4,2);
plot(leg3_posx_h_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg3_posx_h_exp(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state3(P_ST:P_END)/50+leg3_posx_h_exp(P_ST),'LineWidth',2);
ylabel('Hip X');
grid on;
subplot(3,4,6);
plot(leg3_posy_h_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg3_posy_h_exp(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state3(P_ST:P_END)/50+leg3_posy_h_exp(P_ST),'LineWidth',2);
hold on;
plot(touch_flag3(P_ST:P_END)/50+leg3_posy_h_exp(P_ST),'LineWidth',2);
ylabel('Hip Y');
grid on;
subplot(3,4,10);
plot(leg3_posz_h_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg3_posz_h_exp(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(trig_state3(P_ST:P_END)/100+leg3_posz_h_now(P_ST),'LineWidth',2);
ylabel('Hip Z');
%ylim([-0.2,-0.1]);
grid on;
subplot(3,4,3);
plot(leg3_q0_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg3_q0_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('Q0');
grid on;
subplot(3,4,7);
plot(leg3_q1_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg3_q1_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('Q1');
grid on;
subplot(3,4,11);
plot(leg3_q2_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg3_q2_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('Q2');
grid on;
subplot(3,4,4);
plot(leg3_t0_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg3_t0_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('T0');
grid on;
subplot(3,4,8);
plot(leg3_t1_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg3_t1_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('T1');
grid on;
subplot(3,4,12);
plot(leg3_t2_now(P_ST:P_END),'-k','LineWidth',2);
hold on;
plot(leg3_t2_exp(P_ST:P_END),'-.r','LineWidth',2);
ylabel('T2');
grid on;
end
figure('NumberTitle', 'off', 'Name', '姿态')
subplot(3,3,1);
plot(exp_p(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(now_p(P_ST:P_END),'-k');
grid on;
ylabel('Pitch');
subplot(3,3,4);
plot(exp_r(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(now_r(P_ST:P_END),'-k');
grid on;
ylabel('Roll');
subplot(3,3,7);
plot(exp_y(P_ST:P_END),'-.r');
hold on;
plot(now_y(P_ST:P_END));
grid on;
ylabel('Yaw');
subplot(3,3,2);
plot(now_pr(P_ST:P_END));
hold on;
grid on;
ylabel('Dpitch');
subplot(3,3,5);
plot(now_rr(P_ST:P_END));
hold on;
grid on;
ylabel('Droll');
subplot(3,3,8);
plot(now_yr(P_ST:P_END));grid on;
hold on;
ylabel('Dyaw');
subplot(3,3,3);
%plot(g_pit(P_ST:P_END));grid on;
hold on;
ylabel('Gpitch');
subplot(3,3,6);
%plot(g_rol(P_ST:P_END));grid on;
hold on;
ylabel('Groll');
subplot(3,3,9);
plot(g_flag0(P_ST:P_END),':k','LineWidth',2);
hold on;
plot(g_flag1(P_ST:P_END),'-r','LineWidth',1);
hold on;
plot(g_flag2(P_ST:P_END),'-b','LineWidth',1);
hold on;
plot(g_flag3(P_ST:P_END),':m','LineWidth',2);
hold on;
plot(g_flag_4all(P_ST:P_END),'-r','LineWidth',2);
hold on;
plot(g_flag_0all(P_ST:P_END),':b','LineWidth',2);
hold on;
ylabel('G_Flag');
grid on;
figure('NumberTitle', 'off', 'Name', '质心')
subplot(3,2,1);
plot(now_cog_xr(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(now_cog_x(P_ST:P_END),'-k');
grid on;
ylabel('X方向Cog');
subplot(3,2,2);
plot(now_cog_yr(P_ST:P_END),'-.r','LineWidth',2);
hold on;
%plot(now_cog_y(P_ST:P_END),'-k');
grid on;
ylabel('Y方向Cog');
subplot(3,2,3);
plot(trig_state0(P_ST:P_END)/200+now_cog_z(P_ST),'LineWidth',1);
hold on;
plot(trig_state1(P_ST:P_END)/200+now_cog_z(P_ST),'LineWidth',1);
hold on;
plot(trig_state2(P_ST:P_END)/200+now_cog_z(P_ST),'-.r','LineWidth',1);
hold on;
plot(trig_state3(P_ST:P_END)/200+now_cog_z(P_ST),'-.b','LineWidth',1);
hold on;
plot(now_cog_zr(P_ST:P_END),'-.r','LineWidth',1.5);
hold on;
plot(now_cog_z(P_ST:P_END),'-k','LineWidth',1.5);
grid on;
ylabel('Z方向Cog');
subplot(3,2,4);
plot(tar_spdx(P_ST:P_END),'-.r','LineWidth',2);
hold on;
plot(now_cog_dx(P_ST:P_END),'-k');
ylim([-0.5,0.5]);
hold on;
grid on;
ylabel('X方向速度');
subplot(3,2,5);
plot(now_cog_dy(P_ST:P_END),'-k');
hold on;
plot(trig_state0(P_ST:P_END)/10+now_cog_dy(P_ST),'LineWidth',2);
hold on;
plot(trig_state1(P_ST:P_END)/10+now_cog_dy(P_ST),'LineWidth',2);
ylim([-0.5,0.5]);
hold on;
ylabel('Y方向速度');
grid on;
subplot(3,2,6);
plot(now_cog_dz(P_ST:P_END),'-k');
hold on;
plot(trig_state0(P_ST:P_END)/10+now_cog_dz(P_ST),'LineWidth',2);
hold on;
plot(trig_state1(P_ST:P_END)/10+now_cog_dz(P_ST),'LineWidth',2);
ylim([-0.5,0.5]);
hold on;
ylabel('Z方向速度');
grid on;
%
%
fs=500;
flt_rate=50;
[b,a]=butter(6,flt_rate/(fs/2));
%n是滤波器的阶数，根据需要选择合适的整数，Wn是归一化截止频率，又叫自然频率，
%Wn = 截止频率*2/采样频率，如果要留下小于截至频率的信号，用这种格式：
y=filtfilt(b,a,temp0);%good
yy=filter(b,a,temp0);
figure('NumberTitle', 'off', 'Name', 'TEMP')
subplot(2,2,1);
plot(temp0(P_ST:P_END),'-k','LineWidth',1.5);
hold on;
plot(temp1(P_ST:P_END)+trig_state2(P_ST:P_END)*0,'-.r','LineWidth',1.5);
hold on;
plot(temp2(P_ST:P_END)+trig_state2(P_ST:P_END)*1,':b','LineWidth',1.5);
% hold on;
% plot(y(P_ST:P_END),'-b','LineWidth',1.5);
% hold on;
% plot(yy(P_ST:P_END),'-m','LineWidth',2);
grid on;
subplot(2,2,2);
plot(temp3(P_ST:P_END),'-k','LineWidth',1.5);
hold on;
plot(temp4(P_ST:P_END)+trig_state2(P_ST:P_END)*0,'-.r','LineWidth',1.5);
hold on;
plot(temp5(P_ST:P_END),':b','LineWidth',1.5);
grid on;
subplot(2,2,3);
plot(temp6(P_ST:P_END)+now_cog_dz(P_ST:P_END)*0,'-k','LineWidth',1.5);
hold on;
plot(temp3(P_ST:P_END)/10,'-r','LineWidth',1.5);
hold on;
plot(temp2(P_ST:P_END)+trig_state0(P_ST:P_END)*5,':b','LineWidth',1.5);
hold on;
plot(temp7(P_ST:P_END),'-.r','LineWidth',1.5);
hold on;
plot(temp8(P_ST:P_END),':b','LineWidth',1.5);
grid on;
end
function out=limit(in,min,max)
out=in;
if in<min
out=min;
end
if in>max
out=max;
end
end
