clc
close all;
clear all;

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

data = [];
buf = [];

x=0;
%legend('左脚气压','右脚气压','r2','r3','b0','b1','b2','b3')
legend('phlex_0','phlex_1')
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);

right_pos_data = [0,0,0];
right_att_data = [0,0,0];
left_pos_data  = [0,0,0];
left_att_data  = [0,0,0];

down = 0;
jump = 0;

s_rssi = 0;

rssi_buff =[];
last_rssi = 0;

press_buff = [];
gyr_buff = [];
acc_buff = [];

time_stamp = 0;

relate_val = 0;
tempbu =0;
press_max=0;
press_min =0;
pressFlag =0;
press_base_noone =0;
press_lvbo =0;

mag_x =0;
mag_y =0;
mag_z =0;
phlex_0 = 0;
phlex_1 = 0;
% for i=1:shuliang
while true
    data=fread(object,30);%读取数据
    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) == 136
%                           tempbu = double(bitshift(uint32(buf(6)),24)+bitshift(uint32(buf(7)),16)+bitshift(uint32(buf(8)),8)+bitshift(uint32(buf(9)),0));
%                            press_max = double(bitshift(uint16(buf(10)),8)+bitshift(uint16(buf(11)),0));
%                             press_min = double(bitshift(uint16(buf(12)),8)+bitshift(uint16(buf(13)),0));
%                             press_base = double(bitshift(uint16(buf(14)),8)+bitshift(uint16(buf(15)),0));
%                             press = double(bitshift(uint16(buf(16)),8)+bitshift(uint16(buf(17)),0));
%                             press_base_noone = double(bitshift(uint16(buf(18)),8)+bitshift(uint16(buf(19)),0));
%                             pressFlag = double(bitshift(uint16(buf(20)),8)+bitshift(uint16(buf(21)),0));
%                             press_lvbo = double(bitshift(uint16(buf(22)),8)+bitshift(uint16(buf(23)),0));
%                             
%                             mag_x = double(bitshift(int16(buf(24)),8)+bitshift(int16(buf(25)),0));
%                             mag_y = double(bitshift(int16(buf(26)),8)+bitshift(int16(buf(27)),0));
%                             mag_z = double(bitshift(int16(buf(28)),8)+bitshift(int16(buf(29)),0));
%                             
%                             if mag_x <0
%                                mag_x = -(32768+mag_x);
%                             end
%                             if mag_y <0
%                                mag_y = -(32768+mag_y);
%                             end
%                             if mag_z <0
%                                mag_z = -(32768+mag_z);
%                             end
%                             
%                             if pressFlag ==0
%                                pressFlag=0;
%                             else
%                                pressFlag =1000;
%                             end
%                             disPress(1) = mag_x;
%                             disPress(2) = mag_y;
%                             disPress(3) = mag_z;
%                             disp(disPress);
                           phlex_0 = double(bitshift(uint32(buf(5)),8)+bitshift(uint32(buf(6)),0));
                           phlex_1 = double(bitshift(uint32(buf(7)),8)+bitshift(uint32(buf(8)),0));
                           disPress(1) = phlex_0;
                           disPress(2) = phlex_1;
                           disp(disPress);
                            dt = dt + 1;
                            
                            t=[t dt];
                            if dt>250 
                                x=x+1; 
                            end
%                            if press_base == 65535
%                                press_base = 100;
%                            end
%                             press_lvbo =press_lvbo -3000;
%                              m=[m [ press_max;  press_min; press_base; press; press_base_noone;pressFlag; mag_x; mag_y; mag_z;]];
%                              m=[m [ mag_x;  mag_y; mag_z;0 ;0 ;0; 0; 0; 0;]];
                             m=[m [ phlex_0;  phlex_1; 0;0 ;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',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(7),'XData',t,'YData',m(7,:))
%                              set(p(8),'XData',t,'YData',m(8,:))
%                              set(p(9),'XData',t,'YData',m(9,:))
                   
                        buf = buf(Len:end);
                   else
                       buf(1) = [];
                   end
                   state = 0;
                end
            otherwise
                state = 0;
        end
    end
    drawnow limitrate nocallbacks
	axis([x x+300 500 2000]); 
end

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