dual_foot_read_double_mag.m 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464
  1. clc
  2. close all;
  3. clear all;
  4. dt = 1;
  5. t=[0];
  6. % m=[0;0;0;0]
  7. m=[0;0;0;0;0;0;0;0;];%温庭玮+
  8. p = plot(t,m);%温庭玮+
  9. Total_step_left = 0;%温庭玮+
  10. Total_step_right = 0;
  11. pre_num = 0;%温庭玮+
  12. pre_num2 = 0;
  13. num2 = 0;
  14. step = 0;
  15. up_flag1 = 0;
  16. RK = 10000;
  17. LK = 10000;
  18. acc_flag = 0;
  19. step2 = 0;
  20. up_flag2 = 0;
  21. TimingMark = 0;
  22. down_step = 0;
  23. Total_Down_Step = 0;
  24. %global counter;
  25. %counter = 0;
  26. global front_flag;
  27. global back_flag;
  28. front_flag = 0;
  29. back_flag = 0;
  30. global front_flag2;
  31. global back_flag2;
  32. front_flag2 = 0;
  33. back_flag2 = 0;
  34. global counter;
  35. counter=0;
  36. global counter2;
  37. counter2 = 0;
  38. global accl;
  39. global accl2;
  40. accl = 0;
  41. accl2 = 0;
  42. %{
  43. global FrontLiftPoint2;
  44. FrontLiftPoint2=0;
  45. global BackLiftPoint2;
  46. BackLiftPoint2=0;
  47. global FrontDownPoint2;
  48. FrontDownPoint2=0;
  49. global BackDownPoint2;
  50. BackDownPoint2=0;
  51. %}
  52. global FrontHighPoint;
  53. global FrontLowPoint;
  54. global BackHighPoint;
  55. global BackLowPoint;
  56. FrontHighPoint = 10000;
  57. FrontLowPoint = 10000;
  58. BackHighPoint = 10000;
  59. BackLowPoint = 10000;
  60. data = [];
  61. buf = [];
  62. x=0;
  63. %legend('左脚气压','右脚气压','r2','r3','b0','b1','b2','b3')
  64. legend('right_front_mag','right_back_mag','left_front_mag', 'left_back_mag','h_zpt','down','zupt')
  65. grid on;
  66. set(gcf,'unit','normalized','position',[0.2,0.2,0.64,0.7]);
  67. delete(instrfindall('Type','serial'));%清理串口
  68. object = serial('com3','BaudRate',115200);%配置串口
  69. fopen(object);
  70. state = 0;
  71. Len = 0;
  72. Lenf = 0;
  73. Zarr = [];
  74. Zarr(1) = 0;
  75. Zarr_en = 0;
  76. run = 0;
  77. left = 0;
  78. right = 0;
  79. acc3_b = 0;
  80. acc3_bb = 0;
  81. press_data = [];
  82. press_var = [];
  83. gyr_var = [];
  84. pos_data = [];
  85. imu_data = [];
  86. is_down = 0;
  87. is_up = 0;
  88. is_left = 0;
  89. is_right = 0;
  90. count = 0;
  91. mag_last = [0;0;0];
  92. transfer =[0 -1 0; -1 0 0; 0 0 1];
  93. press = zeros(1,18);
  94. gyr_data = zeros(1, 3);
  95. mag_data = zeros(1, 3);
  96. rotate_matrix = eye(3);
  97. right_pos_data = [0,0,0];
  98. right_att_data = [0,0,0];
  99. left_pos_data = [0,0,0];
  100. left_att_data = [0,0,0];
  101. down = 0;
  102. jump = 0;
  103. s_rssi = 0;
  104. rssi_buff =[];
  105. last_rssi = 0.1;
  106. time_stamp = 0;
  107. pfcount = 0;
  108. pfTime = 0;
  109. deltaPos = [0;0;0];
  110. lastPos = [0;0;0];
  111. particleNumber = 500;
  112. lastWeight = ones(1,particleNumber)/(particleNumber);
  113. lastParticle = zeros(particleNumber, 2) + 0.1 *(randn(particleNumber, 2));
  114. pos_time = 0;
  115. att_time = 0;
  116. traj_rotate_matrix = [1, 0; 0, 1];
  117. right_pos_buf = [];
  118. last_s_zupt = 1;
  119. s_zupt = 1;
  120. h_zupt = 1;
  121. last_h_zupt = 1;
  122. right_global_pos = [0;0;];
  123. delta_pos = [0,0];
  124. time_stam = 0;
  125. acc_data = [];
  126. mag_data = [];
  127. h_acc_buff = [];
  128. h_zupt_last = 1;
  129. s_acc_buff = [];
  130. s_zupt_count = 0;
  131. h_zupt_count = 0;
  132. max_rssi_val = 0;
  133. min_rssi_val = 100;
  134. rssi_buff = [];
  135. left_press = 0;
  136. right_press = 0;
  137. zupt_count = 0;
  138. left_acc_norm = 0;
  139. right_acc_norm = 0;
  140. left_press_buff = [];
  141. right_press_buff = [];
  142. last_jump = 0;
  143. left_last_z = 0;
  144. right_last_z = 0;
  145. s_zupt_last = 0;
  146. press_wait_count = 0;
  147. right_data_norm_buff = [];
  148. acc_wait_count = 0;
  149. right_data_x_buff = [];
  150. right_data_y_buff = [];
  151. right_data_z_buff = [];
  152. left_data_z_buff = [];
  153. zero_x_switch = 0;
  154. zero_y_switch = 0;
  155. zero_z_switch = 0;
  156. zero_x_wait = 0;
  157. zero_y_wait = 0;
  158. zero_z_wait = 0;
  159. z_raised = 0;
  160. z_hollow = 0;
  161. x_raised = 0;
  162. x_hollow = 0;
  163. y_raised = 0;
  164. y_hollow = 0;
  165. last_show_z = 0;
  166. last_show_y = 0;
  167. last_show_x = 0;
  168. poly_val_3 = [ -0.4484 -0.3138 -0.2030 -0.1159 -0.0525 -0.0128 0.0031 -0.0047 -0.0362];
  169. poly_val_3 = [-0.4657 -0.3077 -0.1806 -0.0844 -0.0191 0.0153 0.0188 -0.0087 -0.0670];%慢走
  170. acc_wait_cout = 0;
  171. left_median_buff=[3000,3000,3000,3000,3000];
  172. right_median_buff=[3000,3000,3000,3000,3000];
  173. left_acc_buff = [];
  174. right_acc_buff = [];
  175. step_count = -1;
  176. right_front_mag = 0;
  177. right_back_mag = 0;
  178. left_front_mag = 0;
  179. left_back_mag = 0;
  180. left_front_mag_buff = [];
  181. right_front_mag_buff = [];
  182. left_back_mag_buff = [];
  183. right_back_mag_buff = [];
  184. right_acc_z_buff = [];
  185. left_acc_z_buff = [];
  186. cancel_down = 0;
  187. last_down = 0;
  188. left_on_floor = 1;
  189. right_on_floor = 1;
  190. virus_flag = 0;
  191. special_front_down = 0;
  192. real_max_val_right = 0;
  193. real_max_val_left = 0;
  194. up_flag = 0;
  195. right_back_stage = 0;
  196. left_back_stage = 0;
  197. left_acc_buff =[];
  198. right_acc_buff =[];
  199. special_down_count_down = 0;
  200. left_up_trend = 0;
  201. right_up_trend = 0;
  202. % for i=1:shuliang
  203. while true
  204. data=fread(object,320);%读取数据
  205. while ~isempty(data)
  206. buf = [buf data(1)];
  207. data(1) = [];
  208. switch state
  209. case 0
  210. if length(buf)>=5
  211. if buf(1)==170
  212. Len = buf(2);
  213. Lenf = 255 - buf(2);
  214. if buf(3) == Lenf
  215. state = 1;
  216. else
  217. buf(1) = [];
  218. end
  219. else
  220. buf(1) = [];
  221. end
  222. end
  223. case 1
  224. if length(buf)>=Len
  225. ver = 0;
  226. for i=1:(Len-1)
  227. ver = ver + buf(i);
  228. end
  229. ver = mod(ver,256);
  230. % disp(ver);
  231. % disp(buf(Len));
  232. if ver==buf(Len)
  233. if buf(4) == 4
  234. disp(buf(Len));
  235. right_pos_data(1) = double(bitshift(int16(buf(5)),24)+bitshift(int16(buf(6)),16)+bitshift(int16(buf(7)),8)+bitshift(int16(buf(8)),0))/100;
  236. right_pos_data(2) = double(bitshift(int16(buf(9)),24)+bitshift(int16(buf(10)),16)+bitshift(int16(buf(11)),8)+bitshift(int16(buf(12)),0))/100;
  237. right_pos_data(3) = double(bitshift(int16(buf(13)),24)+bitshift(int16(buf(14)),16)+bitshift(int16(buf(15)),8)+bitshift(int16(buf(16)),0))/100;
  238. left_pos_data(1) = double(bitshift(int16(buf(17)),24)+bitshift(int16(buf(18)),16)+bitshift(int16(buf(19)),8)+bitshift(int16(buf(20)),0))/100;
  239. left_pos_data(2) = double(bitshift(int16(buf(21)),24)+bitshift(int16(buf(22)),16)+bitshift(int16(buf(23)),8)+bitshift(int16(buf(24)),0))/100;
  240. left_pos_data(3) = double(bitshift(int16(buf(25)),24)+bitshift(int16(buf(26)),16)+bitshift(int16(buf(27)),8)+bitshift(int16(buf(28)),0))/100;
  241. right_att_data(1) = double(bitshift(int16(buf(29)),8)+bitshift(int16(buf(30)),0));
  242. right_att_data(2) = double(bitshift(int16(buf(31)),8)+bitshift(int16(buf(32)),0));
  243. right_att_data(3) = double(bitshift(int16(buf(33)),8)+bitshift(int16(buf(34)),0));
  244. left_att_data(1) = double(bitshift(int16(buf(35)),8)+bitshift(int16(buf(36)),0));
  245. left_att_data(2) = double(bitshift(int16(buf(37)),8)+bitshift(int16(buf(38)),0));
  246. left_att_data(3) = double(bitshift(int16(buf(39)),8)+bitshift(int16(buf(40)),0));
  247. yaw = left_att_data(1);
  248. pitch = left_att_data(2);
  249. roll = left_att_data(3);
  250. rotate_matrix= [cos(yaw)*cos(pitch), -sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll), sin(yaw)*sin(roll)+ cos(yaw)*sin(pitch)*cos(roll);
  251. sin(yaw)*cos(pitch), cos(yaw)*cos(roll)+sin(yaw)*sin(pitch)*sin(roll), -cos(yaw)*sin(roll)+sin(yaw)*sin(pitch)* cos(roll);
  252. -sin(pitch), cos(pitch)* sin(roll), cos(pitch)*cos(roll)];
  253. right_acc_data(1) = double(bitshift(int16(buf(41)),8)+bitshift(int16(buf(42)),0));
  254. right_acc_data(2) = double(bitshift(int16(buf(43)),8)+bitshift(int16(buf(44)),0));
  255. right_acc_data(3) = double(bitshift(int16(buf(45)),8)+bitshift(int16(buf(46)),0));
  256. %right_acc_buff = [right_acc_buff,[right_acc_data(1);right_acc_data(2);right_acc_data(3)]./2048];
  257. right_acc_buff = [right_acc_buff,[right_acc_data(1);right_acc_data(2);right_acc_data(3)]];
  258. left_acc_data(1) = double(bitshift(int16(buf(47)),8)+bitshift(int16(buf(48)),0));
  259. left_acc_data(2) = double(bitshift(int16(buf(49)),8)+bitshift(int16(buf(50)),0));
  260. left_acc_data(3) = double(bitshift(int16(buf(51)),8)+bitshift(int16(buf(52)),0));
  261. %left_acc_buff = [left_acc_buff,[left_acc_data(1);left_acc_data(2);left_acc_data(3)]./2048];
  262. left_acc_buff = [left_acc_buff,[left_acc_data(1);left_acc_data(2);left_acc_data(3)]];
  263. s_zupt = double(bitand(buf(53),8))/8;
  264. h_zupt = double(bitand(buf(53),4))/4;
  265. down = double(bitand(buf(53),2))/2;
  266. jump = double(bitand(buf(53),1))/1;
  267. rssi = double(int8(buf(54)));
  268. att_time = double(uint8(buf(55)));
  269. right_front_mag = double(bitshift(uint16(buf(56)),8)+bitshift(uint16(buf(57)),0));
  270. right_back_mag = double(bitshift(uint16(buf(58)),8)+bitshift(uint16(buf(59)),0));
  271. left_front_mag = double(bitshift(uint16(buf(60)),8)+bitshift(uint16(buf(61)),0));
  272. left_back_mag = double(bitshift(uint16(buf(62)),8)+bitshift(uint16(buf(63)),0));
  273. left_front_mag_buff = [left_front_mag_buff, left_front_mag];
  274. right_front_mag_buff = [right_front_mag_buff, right_front_mag];
  275. left_back_mag_buff = [left_back_mag_buff, left_back_mag];
  276. right_back_mag_buff = [right_back_mag_buff, right_back_mag];
  277. R_acc = right_acc_buff(3,1:1:end);
  278. R_front = right_front_mag_buff(1:1:end);
  279. R_back = right_back_mag_buff(1:1:end);
  280. num = length( R_front);
  281. if (num>10||num==10) && (num>pre_num)
  282. Acc(3) = R_acc(num);
  283. Acc(2) = R_acc(num-1);
  284. Acc(1) = R_acc(num-2);
  285. %Acc(2) = R_acc(num-3);
  286. %Acc(1) = R_acc(num-4);
  287. [step,up_flag1] = func_count5(R_front,R_back,up_flag1,Acc);
  288. Total_step_right = Total_step_right + step;
  289. k(num) = step*23000;
  290. RFF(num) = up_flag1*15000;
  291. FF_R(num) = front_flag2*2000;
  292. BF_R(num) = back_flag2*3000;
  293. RFC(num) = accl2;
  294. end
  295. pre_num = num;
  296. L_acc = left_acc_buff(3,1:1:end);
  297. L_front = left_front_mag_buff(1:1:end);
  298. L_back = left_back_mag_buff(1:1:end);
  299. num2 = length( L_front);
  300. if (num2>10||num2==10) && (num2>pre_num2)
  301. Acc(3) = L_acc(num);
  302. Acc(2) = L_acc(num-1);
  303. Acc(1) = L_acc(num-2);
  304. %Acc(2) = L_acc(num2-3);
  305. %Acc(1) = L_acc(num2-4);
  306. [step2,up_flag2] = func_count6(L_front,L_back,up_flag2,Acc);
  307. Total_step_left = Total_step_left + step2;
  308. k2(num2) = step2*23000;
  309. LFF(num2) = up_flag2*15000;
  310. FF(num2) = front_flag*2000;
  311. BF(num2) = back_flag*3000;
  312. LFC(num2) = accl;
  313. end
  314. pre_num2 = num2;
  315. TS = Total_step_right + Total_step_left;
  316. end
  317. if(count == 0)
  318. count = 0;
  319. end
  320. dt = dt + 1;
  321. t=[t dt];
  322. if dt>250
  323. x=x+1;
  324. end
  325. % disp([left_press, right_press])
  326. % m=[m [ left_pos_data(3) - left_last_z; left_att_data(1)*0.15; left_att_data(2)*0.15; left_att_data(3)*0.15; h_zupt *0.40;0;0;0; ]];
  327. m=[m [ right_front_mag; right_back_mag; left_front_mag; left_back_mag;0;
  328. s_zupt * 3500; 0 * 35000;0;]];
  329. % m=[m [ right_acc_data(3); 0; 0; 0;0;
  330. % s_zupt * 3500; 0 * 35000;0;]];
  331. % m=[m [ right_acc_data(1); right_acc_data(2);right_acc_data(3);left_acc_data(1); left_acc_data(2);left_acc_data(3);0;
  332. % s_zupt * 3500; 0 * 35000;0;]];
  333. set(p(1),'XData',t,'YData',m(1,:))
  334. set(p(2),'XData',t,'YData',m(2,:))
  335. set(p(3),'XData',t,'YData',m(3,:))
  336. set(p(4),'XData',t,'YData',m(4,:))
  337. set(p(5),'XData',t,'YData',m(5,:))
  338. set(p(6),'XData',t,'YData',m(6,:))
  339. set(p(7),'XData',t,'YData',m(7,:))
  340. set(p(8),'XData',t,'YData',m(8,:))
  341. %++++温庭玮++++%
  342. % set(p(9),'XData',t,'YData',m(9,:))
  343. % set(p(10),'XData',t,'YData',m(10,:))
  344. % set(p(3),'XData',t,'YData',m(7,:))
  345. % set(p(8),'XData',t,'YData',m(8,:))
  346. % set(p(9),'XData',t,'YData',m(9,:))
  347. % set(p(10),'XData',t,'YData',m(10,:))
  348. buf = buf(Len:end);
  349. else
  350. buf(1) = [];
  351. end
  352. state = 0;
  353. end
  354. otherwise
  355. state = 0;
  356. end
  357. end
  358. drawnow limitrate nocallbacks
  359. axis([x x+300 0 30000]);
  360. end
  361. fclose(object);
  362. delete(object);
  363. clear object;