Interaction.cpp 3.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150
  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(uint32_t _time, 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(_time, 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(_time, right_pos_tmp[0], right_pos_tmp[1], right_pos_tmp[2], right_zupt, rssi, right_att[1] * 0.0001f, right_acc_f);
  61. FootActionState left_motion = leftFootAction.get_interation_state();
  62. FootActionState right_motion = rightFootAction.get_interation_state();
  63. if (left_motion.FootState == BACK_LEFT && left_motion.StateCount != lastLeftState.StateCount)
  64. {
  65. motion_state = left_motion.FootState;
  66. std::cout << "Interation 输出 左脚" << std::endl;
  67. if (motion_state == BACK_LEFT)
  68. {
  69. std::cout << "左脚 : 左下" << endl;
  70. }
  71. else if (motion_state == FRONT_LEFT)
  72. {
  73. std::cout << "左脚 : 左上" << endl;
  74. }
  75. }
  76. else if (right_motion.FootState == BACK_RIGHT && right_motion.StateCount != lastRightState.StateCount)
  77. {
  78. motion_state = right_motion.FootState;
  79. if (motion_state == ENTER_KEY)
  80. {
  81. motion_state = CANCLE_KEY;
  82. }
  83. if (motion_state == FRONT_RIGHT)
  84. {
  85. std::cout << "右脚 : 右上" << endl;
  86. }
  87. else if (motion_state == BACK_RIGHT)
  88. {
  89. std::cout << "右脚 : 右下" << endl;
  90. }
  91. //std::cout << "Interation 输出 右脚" << std::endl;
  92. }
  93. else if (left_motion.FootState == ENTER_KEY && left_motion.StateCount != lastLeftState.StateCount)
  94. {
  95. motion_state = left_motion.FootState;
  96. }
  97. else if (right_motion.FootState == ENTER_KEY && right_motion.StateCount != lastRightState.StateCount)
  98. {
  99. motion_state = CANCLE_KEY;
  100. }
  101. else
  102. {
  103. motion_state = -1;
  104. }
  105. if (motion_state == CANCLE_KEY || motion_state == ENTER_KEY)
  106. {
  107. std::cout << "取消or确认键 time: " << _time <<endl;
  108. }
  109. memcpy(left_last_pos_tmp, left_pos_tmp, 3 * sizeof(int32_t));
  110. memcpy(right_last_pos_tmp, right_pos_tmp, 3 * sizeof(int32_t));
  111. left_zupt_last = left_zupt;
  112. right_zupt_last = right_zupt;
  113. lastLeftState = left_motion;
  114. lastRightState = right_motion;
  115. }