Interaction.cpp 2.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119
  1. #include "Interaction.h"
  2. Interaction::Interaction()
  3. {
  4. rotateTrajLeft = InertialTrajProcess();
  5. rotateTrajRight = InertialTrajProcess();
  6. same_zupt_count = 0;
  7. canEnter = 0;
  8. memset(left_last_pos, 0, 3 * sizeof(float));
  9. memset(right_last_pos, 0, 3 * sizeof(float));
  10. memset(left_last_pos_tmp, 0, 3 * sizeof(float));
  11. memset(right_last_pos_tmp, 0, 3 * sizeof(float));
  12. motion_state = -1;
  13. can_report_motion_state = 0;
  14. leftFootAction = SingleFootAction(LEFT_FOOT);
  15. rightFootAction = SingleFootAction(RIGHT_FOOT);
  16. time_interval = 0;
  17. }
  18. int Interaction::get_motion_state()
  19. {
  20. return motion_state;
  21. }
  22. void Interaction::Process(int32_t* right_pos, int* right_att, int *right_acc,int right_zupt, int right_press,
  23. int32_t* left_pos, int* left_att, int* left_acc, int left_zupt, int left_press, int jump, int down, int rssi)
  24. {
  25. int32_t right_pos_org[3]; int32_t left_pos_org[3];
  26. memcpy(right_pos_org, right_pos, 3 * sizeof(int32_t));
  27. memcpy(left_pos_org, left_pos, 3 * sizeof(int32_t));
  28. if (left_zupt && right_zupt && rssi < 30)
  29. {
  30. rotateTrajLeft.ResetHeading(left_att[0] - 1745);
  31. rotateTrajRight.ResetHeading(right_att[0] + 1745);
  32. }
  33. rotateTrajRight.TrajRotate(right_pos_org);
  34. rotateTrajLeft.TrajRotate(left_pos_org);
  35. if (left_zupt)
  36. {
  37. memcpy(left_last_pos, left_pos_org, 3 * sizeof(int32_t));
  38. }
  39. if (right_zupt)
  40. {
  41. memcpy(right_last_pos, right_pos_org, 3 * sizeof(int32_t));
  42. }
  43. int left_pos_tmp[3];
  44. int right_pos_tmp[3];
  45. float right_acc_f[3];
  46. float left_acc_f[3];
  47. for (int i = 0; i < 3; i++)
  48. {
  49. left_pos_tmp[i] = left_pos_org[i] - left_last_pos[i];
  50. right_pos_tmp[i] = right_pos_org[i] - right_last_pos[i];
  51. right_acc_f[i] = right_acc[i] / 2048.0f;
  52. left_acc_f[i] = left_acc[i] / 2048.0f;
  53. }
  54. //std::cout << "right_acc_f : " << right_acc_f[0] << ", " << right_acc_f[1] << ", " << right_acc_f[2] << endl;
  55. //if (left_zupt == 0)
  56. //{
  57. // std::cout << "left_pos_tmp : " << left_pos_tmp[0] << ", " << left_pos_tmp[1] << ", " << left_pos_tmp[2] << endl;
  58. //}
  59. leftFootAction.run(left_pos_tmp[0], left_pos_tmp[1], left_pos_tmp[2], left_zupt, rssi, left_att[1] * 0.0001f, left_acc_f);
  60. rightFootAction.run(right_pos_tmp[0], right_pos_tmp[1], right_pos_tmp[2], right_zupt, rssi, right_att[1] * 0.0001f, right_acc_f);
  61. int left_motion = leftFootAction.get_interation_state();
  62. int right_motion = rightFootAction.get_interation_state();
  63. if (left_motion > -1)
  64. {
  65. motion_state = left_motion;
  66. std::cout << "Interation Êä³ö ×ó½Å" << std::endl;
  67. }
  68. else if (right_motion > -1)
  69. {
  70. motion_state = right_motion;
  71. if (right_motion == ENTER_KEY)
  72. {
  73. motion_state = CANCLE_KEY;
  74. }
  75. std::cout << "Interation Êä³ö ÓÒ½Å" << std::endl;
  76. }
  77. else
  78. {
  79. motion_state = -1;
  80. }
  81. memcpy(left_last_pos_tmp, left_pos_tmp, 3 * sizeof(int32_t));
  82. memcpy(right_last_pos_tmp, right_pos_tmp, 3 * sizeof(int32_t));
  83. left_zupt_last = left_zupt;
  84. right_zupt_last = right_zupt;
  85. }