|
@@ -1,5 +1,7 @@
|
|
#include "FingerGuessing.h"
|
|
#include "FingerGuessing.h"
|
|
|
|
|
|
|
|
+#
|
|
|
|
+
|
|
FingerGuessing::FingerGuessing()
|
|
FingerGuessing::FingerGuessing()
|
|
{
|
|
{
|
|
left_foot_has_init = 0;
|
|
left_foot_has_init = 0;
|
|
@@ -21,7 +23,7 @@ FingerGuessing::FingerGuessing()
|
|
left_init = 0;
|
|
left_init = 0;
|
|
right_init = 0;
|
|
right_init = 0;
|
|
|
|
|
|
- //原点
|
|
|
|
|
|
+ //原锟斤拷
|
|
left_global_pos_vector.push_back({ 0, 0, 0, 0 });
|
|
left_global_pos_vector.push_back({ 0, 0, 0, 0 });
|
|
right_global_pos_vector.push_back({ 0, 0, 0, 0 });
|
|
right_global_pos_vector.push_back({ 0, 0, 0, 0 });
|
|
|
|
|
|
@@ -46,7 +48,7 @@ void FingerGuessing::ProcessResult(float* global_pos, deque<shoes_data_cell>& st
|
|
std::cout << "FingerGuessing::ProcessResult" << endl;
|
|
std::cout << "FingerGuessing::ProcessResult" << endl;
|
|
std::cout << endl;
|
|
std::cout << endl;
|
|
|
|
|
|
- //寻找到距离最小的值
|
|
|
|
|
|
+ //寻锟揭碉拷锟斤拷锟斤拷锟斤拷小锟斤拷值
|
|
int vector_length = global_pos_vector.size();
|
|
int vector_length = global_pos_vector.size();
|
|
|
|
|
|
float distance_point = 0.05f;
|
|
float distance_point = 0.05f;
|
|
@@ -56,7 +58,7 @@ void FingerGuessing::ProcessResult(float* global_pos, deque<shoes_data_cell>& st
|
|
|
|
|
|
for (int i = 0; i < vector_length; i++)
|
|
for (int i = 0; i < vector_length; i++)
|
|
{
|
|
{
|
|
- //回溯寻找曾经踩过的点
|
|
|
|
|
|
+ //锟斤拷锟斤拷寻锟斤拷锟斤拷锟斤拷锟饺癸拷锟侥碉拷
|
|
float dx = global_pos[0] - global_pos_vector.at(i).pos_x;
|
|
float dx = global_pos[0] - global_pos_vector.at(i).pos_x;
|
|
float dy = global_pos[1] - global_pos_vector.at(i).pos_y;
|
|
float dy = global_pos[1] - global_pos_vector.at(i).pos_y;
|
|
|
|
|
|
@@ -72,7 +74,7 @@ void FingerGuessing::ProcessResult(float* global_pos, deque<shoes_data_cell>& st
|
|
distance_point = distance_temp;
|
|
distance_point = distance_temp;
|
|
|
|
|
|
}
|
|
}
|
|
- else if (i != 0 && distance_temp < distance_point)
|
|
|
|
|
|
+ else if (i != 0 && distance_temp < distance_point)
|
|
{
|
|
{
|
|
|
|
|
|
index = i;
|
|
index = i;
|
|
@@ -86,8 +88,8 @@ void FingerGuessing::ProcessResult(float* global_pos, deque<shoes_data_cell>& st
|
|
if (index == -1)
|
|
if (index == -1)
|
|
{
|
|
{
|
|
std::cout << "no find similar point" << endl;
|
|
std::cout << "no find similar point" << endl;
|
|
- //没有找到基准点的时候,检测到往返点的时候,再做判断
|
|
|
|
- //当检测到接近原点的时候,应当设置为0
|
|
|
|
|
|
+ //没锟斤拷锟揭碉拷锟斤拷准锟斤拷锟绞憋拷颍�锟解到锟斤拷锟斤拷锟斤拷锟绞憋拷锟�,锟斤拷锟斤拷锟叫讹拷
|
|
|
|
+ //锟斤拷锟斤拷獾斤拷咏锟皆�拷锟斤拷时锟斤拷应锟斤拷锟斤拷锟斤拷为0
|
|
|
|
|
|
global_pos_vector.push_back({ 0, global_pos[0], global_pos[1],global_pos[2] });
|
|
global_pos_vector.push_back({ 0, global_pos[0], global_pos[1],global_pos[2] });
|
|
|
|
|
|
@@ -121,7 +123,7 @@ void FingerGuessing::ProcessResult(float* global_pos, deque<shoes_data_cell>& st
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
- //为了防止误触点,用五个触地点来测试
|
|
|
|
|
|
+ //为锟剿凤拷止锟襟触点,锟斤拷锟斤拷锟斤拷锟斤拷氐锟斤拷锟斤拷锟斤拷锟�
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
@@ -144,7 +146,7 @@ int FingerGuessing::ProcessDualStepVector(deque<shoes_data_cell>& left_cmd_step_
|
|
return -1;
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
|
|
- //还是判断拳头
|
|
|
|
|
|
+ //锟斤拷锟斤拷锟叫讹拷拳头
|
|
|
|
|
|
if (rssi > 30)
|
|
if (rssi > 30)
|
|
{
|
|
{
|
|
@@ -152,39 +154,48 @@ int FingerGuessing::ProcessDualStepVector(deque<shoes_data_cell>& left_cmd_step_
|
|
float dx = right_dx - left_dx;
|
|
float dx = right_dx - left_dx;
|
|
float dy = right_dy - left_dy;
|
|
float dy = right_dy - left_dy;
|
|
|
|
|
|
- if (right_dx - left_dx > 0.3f)
|
|
|
|
- {
|
|
|
|
- left_cmd_global_pos[0] = -0.25f; left_cmd_global_pos[1] = 0.0f;
|
|
|
|
- right_cmd_global_pos[0] = 0.25f; right_cmd_global_pos[1] = 0.0f;
|
|
|
|
-
|
|
|
|
- return MOTION_PAPER;
|
|
|
|
- }
|
|
|
|
|
|
+ if (right_dx - left_dx > 0.3f)
|
|
|
|
+ {
|
|
|
|
+ left_cmd_global_pos[0] = -0.25f; left_cmd_global_pos[1] = 0.0f;
|
|
|
|
+ right_cmd_global_pos[0] = 0.25f; right_cmd_global_pos[1] = 0.0f;
|
|
|
|
|
|
- //判断剪刀有点麻烦
|
|
|
|
- if (right_dx - left_dx < -0.3f)
|
|
|
|
- {
|
|
|
|
|
|
+ return MOTION_PAPER;
|
|
|
|
+ }
|
|
|
|
|
|
- if (left_dy - right_dy < -0.3f)
|
|
|
|
- {
|
|
|
|
- left_cmd_global_pos[0] = 0.0f; left_cmd_global_pos[1] = -0.15f;
|
|
|
|
- right_cmd_global_pos[0] = 0.0f; right_cmd_global_pos[1] = 0.15f;
|
|
|
|
- return MOTION_SCISSORS;
|
|
|
|
- }
|
|
|
|
|
|
+ //锟叫断硷拷锟斤拷锟叫碉拷锟介烦
|
|
|
|
+ if (right_dx - left_dx < -0.3f)
|
|
|
|
+ {
|
|
|
|
|
|
- if (left_dy - right_dy > 0.3f)
|
|
|
|
- {
|
|
|
|
- left_cmd_global_pos[0] = 0.0f; left_cmd_global_pos[1] = 0.15f;
|
|
|
|
- right_cmd_global_pos[0] = 0.0f; right_cmd_global_pos[1] = -0.15f;
|
|
|
|
- return MOTION_SCISSORS;
|
|
|
|
- }
|
|
|
|
|
|
+ if (left_dy - right_dy < -0.3f)
|
|
|
|
+ {
|
|
|
|
+ left_cmd_global_pos[0] = 0.0f; left_cmd_global_pos[1] = -0.15f;
|
|
|
|
+ right_cmd_global_pos[0] = 0.0f; right_cmd_global_pos[1] = 0.15f;
|
|
|
|
+ return MOTION_SCISSORS;
|
|
|
|
+ }
|
|
|
|
|
|
|
|
+ if (left_dy - right_dy > 0.3f)
|
|
|
|
+ {
|
|
|
|
+ left_cmd_global_pos[0] = 0.0f; left_cmd_global_pos[1] = 0.15f;
|
|
|
|
+ right_cmd_global_pos[0] = 0.0f; right_cmd_global_pos[1] = -0.15f;
|
|
|
|
+ return MOTION_SCISSORS;
|
|
}
|
|
}
|
|
|
|
+
|
|
|
|
+ }
|
|
}
|
|
}
|
|
|
|
|
|
-
|
|
|
|
|
|
+
|
|
return -1;
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+bool FingerGuessing::unzuptValid(int* max_acc, int* min_acc)
|
|
|
|
+{
|
|
|
|
+ if ((max_acc[0] - min_acc[0] > 2048 || max_acc[1] - min_acc[1] > 2048 || max_acc[2] - min_acc[2] > 2048))
|
|
|
|
+ {
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ return false;
|
|
|
|
+}
|
|
|
|
|
|
|
|
|
|
void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
|
|
void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int* right_acc, int right_zupt, int right_front_press, int right_back_press,
|
|
@@ -192,51 +203,125 @@ void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int
|
|
int jump, int down, int rssi)
|
|
int jump, int down, int rssi)
|
|
{
|
|
{
|
|
|
|
|
|
- //由于刚触地的时候,会偶尔丢掉触地信号的关键帧,现在在线补充一下
|
|
|
|
-
|
|
|
|
- int online_zupt = extract_motion.online_detect_touch_floor( right_front_press, left_front_press, right_back_press, left_back_press);
|
|
|
|
|
|
+ //锟斤拷锟节刚达拷锟截碉拷时锟斤拷,锟斤拷偶锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟脚号的关硷拷帧锟斤拷锟斤拷锟斤拷锟斤拷锟竭诧拷锟斤拷一锟斤拷
|
|
|
|
+
|
|
|
|
+ //int online_zupt = extract_motion.online_detect_touch_floor(right_front_press, left_front_press, right_back_press, left_back_press);
|
|
|
|
+
|
|
|
|
+ //if (online_zupt & 0x01)
|
|
|
|
+ //{
|
|
|
|
+ // std::cout << "test :: left_zupt" << endl;
|
|
|
|
+ // left_zupt = 1;
|
|
|
|
+ //}
|
|
|
|
+ //if (online_zupt & 0x02)
|
|
|
|
+ //{
|
|
|
|
+ // std::cout << "test :: right_zupt" << endl;
|
|
|
|
+ // right_zupt = 1;
|
|
|
|
+ //}
|
|
|
|
|
|
- if (online_zupt & 0x01)
|
|
|
|
|
|
+ if (has_init == 0)
|
|
|
|
+ {
|
|
|
|
+ rssi_vec.push_back(rssi);
|
|
|
|
+ pitch_vec.push_back(left_att[1] - right_att[1]);
|
|
|
|
+ roll_vec.push_back((left_att[2] + right_att[2]) * 0.5f);
|
|
|
|
+ left_acc_vec.push_back(sqrt(left_acc[0] * left_acc[0] + left_acc[1] * left_acc[1] + left_acc[2] * left_acc[2]));
|
|
|
|
+ right_acc_vec.push_back(sqrt(right_acc[0] * right_acc[0] + right_acc[1] * right_acc[1] + right_acc[2] * right_acc[2]));
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ if (rssi_vec.size() > 50)
|
|
|
|
+ {
|
|
|
|
+ rssi_vec.pop_front();
|
|
|
|
+ pitch_vec.pop_front();
|
|
|
|
+ roll_vec.pop_front();
|
|
|
|
+ left_acc_vec.pop_front();
|
|
|
|
+ right_acc_vec.pop_front();
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ if (rssi_vec.size() == 50)
|
|
|
|
+ {
|
|
|
|
+
|
|
|
|
+ int max_rssi = *max_element(rssi_vec.begin(), rssi_vec.end());
|
|
|
|
+ int left_max_acc = *max_element(left_acc_vec.begin(), left_acc_vec.end());
|
|
|
|
+ int left_min_acc = *min_element(left_acc_vec.begin(), left_acc_vec.end());
|
|
|
|
+
|
|
|
|
+ int right_max_acc = *max_element(right_acc_vec.begin(), right_acc_vec.end());
|
|
|
|
+ int right_min_acc = *min_element(right_acc_vec.begin(), right_acc_vec.end());
|
|
|
|
+
|
|
|
|
+ if (max_rssi < 25 && left_max_acc - left_min_acc < 510 && right_max_acc - right_min_acc < 510)
|
|
|
|
+ {
|
|
|
|
+ std::cout << "test" << endl;
|
|
|
|
+ for (int i = 0; i < 50; i++)
|
|
|
|
+ {
|
|
|
|
+ pitch_reference_val += pitch_vec[i];
|
|
|
|
+ roll_reference_val += pitch_vec[i];
|
|
|
|
+ }
|
|
|
|
+ pitch_reference_val /= 50;
|
|
|
|
+ roll_reference_val /= 50;
|
|
|
|
+
|
|
|
|
+ std::cout << "init roll and pitch !!!!" << endl;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ /*if (left_zupt || right_zupt)
|
|
|
|
+ {
|
|
|
|
+ left_zupt = 1; right_zupt = 1;
|
|
|
|
+ }*/
|
|
|
|
+
|
|
|
|
+ if (left_zupt)
|
|
|
|
+ {
|
|
|
|
+ left_zupt_extension_time = 5;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ if (right_zupt)
|
|
|
|
+ {
|
|
|
|
+ right_zupt_extension_time = 5;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ if (left_zupt_extension_time > 0)
|
|
{
|
|
{
|
|
- std::cout << "test :: left_zupt" << endl;
|
|
|
|
left_zupt = 1;
|
|
left_zupt = 1;
|
|
|
|
+
|
|
|
|
+ left_zupt_extension_time--;
|
|
}
|
|
}
|
|
- if (online_zupt & 0x02)
|
|
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ if (right_zupt_extension_time > 0)
|
|
{
|
|
{
|
|
- std::cout << "test :: right_zupt" << endl;
|
|
|
|
right_zupt = 1;
|
|
right_zupt = 1;
|
|
- }
|
|
|
|
|
|
|
|
|
|
+ right_zupt_extension_time--;
|
|
|
|
+ }
|
|
|
|
|
|
|
|
|
|
- //缓存一步内的空中数据
|
|
|
|
|
|
+ //锟斤拷锟斤拷一锟斤拷锟节的匡拷锟斤拷锟斤拷锟斤拷
|
|
setData(right_shoes_data_vector, time_stamp, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
|
|
setData(right_shoes_data_vector, time_stamp, right_pos[0] * 0.001f, right_pos[1] * 0.001f, right_pos[2] * 0.001f,
|
|
right_att[0] * 0.0001f, right_att[1] * 0.0001f, right_att[2] * 0.0001f, right_zupt, rssi);
|
|
right_att[0] * 0.0001f, right_att[1] * 0.0001f, right_att[2] * 0.0001f, right_zupt, rssi);
|
|
|
|
|
|
setData(left_shoes_data_vector, time_stamp, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
|
|
setData(left_shoes_data_vector, time_stamp, left_pos[0] * 0.001f, left_pos[1] * 0.001f, left_pos[2] * 0.001f,
|
|
left_att[0] * 0.0001f, left_att[1] * 0.0001f, left_att[2] * 0.0001f, left_zupt, rssi);
|
|
left_att[0] * 0.0001f, left_att[1] * 0.0001f, left_att[2] * 0.0001f, left_zupt, rssi);
|
|
|
|
|
|
- //根据新来的数据,计算的全局位置
|
|
|
|
|
|
+ //锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟捷o拷锟斤拷锟斤拷锟饺�拷锟轿伙拷锟�
|
|
calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_heading, 0, RIGHT_FOOT);
|
|
calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_heading, 0, RIGHT_FOOT);
|
|
|
|
|
|
calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_heading, 0, LEFT_FOOT);
|
|
calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_heading, 0, LEFT_FOOT);
|
|
|
|
|
|
|
|
|
|
- //不断根据rssi和触地信号来估计归位,以及方向
|
|
|
|
- resetZerosPointByRssi(left_zupt, 10 ,left_zupt_count, left_foot_has_init, left_att[0] * 0.0001f, left_heading, rssi, 25,left_global_pos);
|
|
|
|
|
|
+ //锟斤拷锟较革拷锟斤拷rssi锟酵达拷锟斤拷锟脚猴拷锟斤拷锟斤拷锟狡癸拷位锟斤拷锟皆硷拷锟斤拷锟斤拷
|
|
|
|
+ resetZerosPointByRssi(left_zupt, 10, left_zupt_count, left_foot_has_init, left_att[0] * 0.0001f, left_heading, rssi, 25, left_global_pos);
|
|
resetZerosPointByRssi(right_zupt, 10, right_zupt_count, right_foot_has_init, right_att[0] * 0.0001f, right_heading, rssi, 25, right_global_pos);
|
|
resetZerosPointByRssi(right_zupt, 10, right_zupt_count, right_foot_has_init, right_att[0] * 0.0001f, right_heading, rssi, 25, right_global_pos);
|
|
|
|
|
|
|
|
|
|
- //通过加速度、以及vector已经缓存了空中的数据来定位判断命令所需的时间戳,这个时间戳是起延时判断的效果,延时是为了等合适的RSSI
|
|
|
|
- if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1)
|
|
|
|
|
|
+ //通锟斤拷锟斤拷锟劫度★拷锟皆硷拷vector锟窖撅拷锟斤拷锟斤拷锟剿匡拷锟叫碉拷锟斤拷锟斤拷锟斤拷锟斤拷位锟叫讹拷锟斤拷锟斤拷锟斤拷锟斤拷锟绞憋拷锟斤拷锟斤拷锟斤拷锟绞憋拷锟斤拷锟斤拷锟斤拷锟斤拷时锟叫断碉拷效锟斤拷锟斤拷锟斤拷时锟斤拷为锟剿等猴拷锟绞碉拷RSSI
|
|
|
|
+ if (left_step_data_vector.size() > 5 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1)
|
|
{
|
|
{
|
|
if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
|
|
if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
|
|
{
|
|
{
|
|
- //根据过往记录的位置点来,来优化
|
|
|
|
|
|
+ //锟斤拷锟捷癸拷锟斤拷锟斤拷录锟斤拷位锟矫碉拷锟斤拷锟斤拷锟斤拷锟脚伙拷
|
|
//ProcessResult(left_global_pos, left_step_data_vector, left_global_pos_vector, LEFT_FOOT);
|
|
//ProcessResult(left_global_pos, left_step_data_vector, left_global_pos_vector, LEFT_FOOT);
|
|
|
|
|
|
- addMotionCount(STEP_COUNT);
|
|
|
|
-
|
|
|
|
left_cmd_step_data_vector = left_step_data_vector;
|
|
left_cmd_step_data_vector = left_step_data_vector;
|
|
|
|
|
|
float dy = left_step_data_vector.back().pos_y - left_step_data_vector.front().pos_y;
|
|
float dy = left_step_data_vector.back().pos_y - left_step_data_vector.front().pos_y;
|
|
@@ -252,18 +337,19 @@ void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int
|
|
left_cmd_wait_time = time_stamp;
|
|
left_cmd_wait_time = time_stamp;
|
|
|
|
|
|
left_init = 1;
|
|
left_init = 1;
|
|
|
|
+
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
- if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1)
|
|
|
|
|
|
+
|
|
|
|
+ if (right_step_data_vector.size() > 5 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1)
|
|
{
|
|
{
|
|
if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
|
|
if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
|
|
{
|
|
{
|
|
- //根据过往记录的位置点来,来优化
|
|
|
|
- //ProcessResult(right_global_pos, right_step_data_vector, right_global_pos_vector, RIGHT_FOOT);
|
|
|
|
|
|
|
|
- addMotionCount(STEP_COUNT);
|
|
|
|
|
|
+ //锟斤拷锟捷癸拷锟斤拷锟斤拷录锟斤拷位锟矫碉拷锟斤拷锟斤拷锟斤拷锟脚伙拷
|
|
|
|
+ //ProcessResult(right_global_pos, right_step_data_vector, right_global_pos_vector, RIGHT_FOOT);
|
|
|
|
|
|
right_cmd_step_data_vector = right_step_data_vector;
|
|
right_cmd_step_data_vector = right_step_data_vector;
|
|
|
|
|
|
@@ -289,112 +375,73 @@ void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int
|
|
setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
|
|
setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
|
|
setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);
|
|
setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);
|
|
|
|
|
|
- //重置命令
|
|
|
|
|
|
+ //锟斤拷锟斤拷锟斤拷锟斤拷
|
|
memset(result, -1, 4 * sizeof(int));
|
|
memset(result, -1, 4 * sizeof(int));
|
|
|
|
|
|
- //判断双脚触地的时候, rssi < 23 必须归位,不然也没办法处理
|
|
|
|
- //看起来rssi的延迟有够烂的
|
|
|
|
|
|
+ //锟叫讹拷双锟脚达拷锟截碉拷时锟斤拷, rssi < 23 锟斤拷锟斤拷锟轿伙拷锟斤拷锟饺灰裁伙拷旆�拷锟斤拷锟�
|
|
|
|
+ //锟斤拷锟斤拷锟斤拷rssi锟斤拷锟接筹拷锟叫癸拷锟矫碉拷
|
|
|
|
|
|
|
|
|
|
if (left_init == 1 && right_init == 1 &&
|
|
if (left_init == 1 && right_init == 1 &&
|
|
- time_stamp - left_cmd_wait_time > 9 && time_stamp - left_cmd_wait_time < 40 && time_stamp - right_cmd_wait_time > 9 && time_stamp - right_cmd_wait_time < 40)
|
|
|
|
|
|
+ time_stamp - left_cmd_wait_time > 9 && time_stamp - left_cmd_wait_time < 40 && time_stamp - right_cmd_wait_time > 9 && time_stamp - right_cmd_wait_time < 40)
|
|
|
|
+ //if (
|
|
|
|
+ // (left_init == 1 && time_stamp - left_cmd_wait_time > 9 && time_stamp - left_cmd_wait_time < 40)
|
|
|
|
+ // ||
|
|
|
|
+ // (right_init == 1 && time_stamp - right_cmd_wait_time > 9 && time_stamp - right_cmd_wait_time < 40)
|
|
|
|
+ // )
|
|
{
|
|
{
|
|
-
|
|
|
|
- //rssi 有延迟,判断延迟50ms, 直接判断如果这时候的RSSI 小于阈值, 直接设置为0
|
|
|
|
- if ( rssi < rssi_threshold)
|
|
|
|
- {
|
|
|
|
- for (int i = 0; i < 3; i++)
|
|
|
|
- {
|
|
|
|
- left_global_pos[i] -= left_cmd_global_pos[i];
|
|
|
|
- right_global_pos[i] -= right_cmd_global_pos[i];
|
|
|
|
- }
|
|
|
|
- memset(left_cmd_global_pos, 0, 3 * sizeof(float));
|
|
|
|
- memset(right_cmd_global_pos, 0, 3 * sizeof(float));
|
|
|
|
-
|
|
|
|
- right_global_pos_vector.clear();
|
|
|
|
- left_global_pos_vector.clear();
|
|
|
|
-
|
|
|
|
- left_global_pos_vector.push_back({ 0,0,0,0 });
|
|
|
|
- right_global_pos_vector.push_back({ 0,0,0,0 });
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- //尝试用特殊位移来计算结果, 左右想外移动,就肯定是布,如果左右脚收缩,上下有移动,rssi超过阈值就是剪刀了
|
|
|
|
- if (ProcessDualStepVector(left_cmd_step_data_vector, right_cmd_step_data_vector, left_cmd_global_pos, right_cmd_global_pos, rssi) != -1)
|
|
|
|
- {
|
|
|
|
- memcpy(right_global_pos, right_cmd_global_pos, 3 * sizeof(float));
|
|
|
|
- memcpy(left_global_pos, left_cmd_global_pos, 3 * sizeof(float));
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
-
|
|
|
|
-
|
|
|
|
- float dx = left_cmd_global_pos[0] - right_cmd_global_pos[0];
|
|
|
|
-
|
|
|
|
- float dy = left_cmd_global_pos[1] - right_cmd_global_pos[1];
|
|
|
|
|
|
|
|
- //根据位移来先处理
|
|
|
|
- if (sqrt(dx * dx + dy * dy) < 0.1f)
|
|
|
|
|
|
+ if (rssi < rssi_threshold)
|
|
{
|
|
{
|
|
addMotionCount(ROCK_COUNT);
|
|
addMotionCount(ROCK_COUNT);
|
|
|
|
|
|
result[0] = MOTION_ROCK;
|
|
result[0] = MOTION_ROCK;
|
|
|
|
|
|
- std::cout << "FingureResult : MOTION_ROCK" << endl;
|
|
|
|
|
|
+ std::cout << "FingureResult : MOTION_ROCK " << "rssi 拳头"<<endl;
|
|
}
|
|
}
|
|
else
|
|
else
|
|
{
|
|
{
|
|
|
|
+ int pitch_diff = left_att[1] - right_att[1];
|
|
|
|
|
|
- std::cout << "dx : " << dx << " dy : " << dy << endl;
|
|
|
|
|
|
+ int roll_mean = 0.5f*(left_att[2] + right_att[2]);
|
|
|
|
|
|
- if ((dx > 0.12f || dx < -0.12f) && fabsf(dy) < 3.0 / 4 * fabsf(dx))
|
|
|
|
|
|
+ if (pitch_diff - pitch_reference_val > 2000 || pitch_diff - pitch_reference_val < -2000)
|
|
{
|
|
{
|
|
- //统计步数
|
|
|
|
- addMotionCount(PAPER_COUNT);
|
|
|
|
|
|
+ //统锟狡硷拷锟斤拷
|
|
|
|
+ addMotionCount(SCISSORS_COUNT);
|
|
|
|
|
|
- result[0] = MOTION_PAPER;
|
|
|
|
|
|
+ result[0] = MOTION_SCISSORS;
|
|
|
|
|
|
- std::cout << "MOTION_PAPER, RSSI : " << rssi << endl;
|
|
|
|
|
|
+ std::cout << "FingureResult : MOTION_SCISSORS " << " 锟斤拷锟斤拷" << endl;
|
|
|
|
|
|
- std::cout << "FingureResult : MOTION_PAPER" << endl;
|
|
|
|
-
|
|
|
|
}
|
|
}
|
|
- else if ((dy > 0.12f || dy < -0.12f) && fabsf(dx) < 3.0 / 4 * fabsf(dy))
|
|
|
|
|
|
+ else if (roll_mean - roll_reference_val > 1500)
|
|
{
|
|
{
|
|
- //统计步数
|
|
|
|
- addMotionCount(SCISSORS_COUNT);
|
|
|
|
|
|
+ //统锟狡诧拷锟斤拷
|
|
|
|
+ addMotionCount(PAPER_COUNT);
|
|
|
|
|
|
- result[0] = MOTION_SCISSORS;
|
|
|
|
|
|
+ result[0] = MOTION_PAPER;
|
|
|
|
|
|
- std::cout << "FingureResult : MOTION_SCISSORS" << endl;
|
|
|
|
|
|
+ std::cout << "MOTION_PAPER, RSSI : " << rssi << endl;
|
|
|
|
|
|
-
|
|
|
|
|
|
+ std::cout << "FingureResult : MOTION_PAPER" << " 锟斤拷" << endl;
|
|
}
|
|
}
|
|
- /*else if (dy > 0.3f || dy < -0.3f || dx > 0.3f || dx < -0.3f)*/
|
|
|
|
else
|
|
else
|
|
{
|
|
{
|
|
- if (fabsf(dy) > fabsf(dx))
|
|
|
|
- {
|
|
|
|
- addMotionCount(SCISSORS_COUNT);
|
|
|
|
-
|
|
|
|
- result[0] = MOTION_SCISSORS;
|
|
|
|
-
|
|
|
|
- std::cout << "dy > dx FingureResult : MOTION_SCISSORS" << endl;
|
|
|
|
- }
|
|
|
|
- else
|
|
|
|
- {
|
|
|
|
- addMotionCount(PAPER_COUNT);
|
|
|
|
|
|
+ addMotionCount(ROCK_COUNT);
|
|
|
|
|
|
- result[0] = MOTION_PAPER;
|
|
|
|
|
|
+ result[0] = MOTION_ROCK;
|
|
|
|
|
|
- std::cout << "dy < dx FingureResult : MOTION_PAPER" << endl;
|
|
|
|
- }
|
|
|
|
|
|
+ std::cout << "FingureResult : MOTION_ROCK "<< "default 石头" << endl;
|
|
}
|
|
}
|
|
-
|
|
|
|
-
|
|
|
|
}
|
|
}
|
|
|
|
|
|
left_cmd_wait_time = 0;
|
|
left_cmd_wait_time = 0;
|
|
right_cmd_wait_time = 0;
|
|
right_cmd_wait_time = 0;
|
|
|
|
|
|
|
|
+ left_init = 0;
|
|
|
|
+ right_init = 0;
|
|
|
|
+
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
}
|