|
@@ -189,7 +189,7 @@ int FingerGuessing::ProcessDualStepVector(deque<shoes_data_cell>& left_cmd_step_
|
|
|
|
|
|
bool FingerGuessing::unzuptValid(int* max_acc, int* min_acc)
|
|
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) && min_acc[0] < 500)
|
|
|
|
|
|
+ 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 true;
|
|
}
|
|
}
|
|
@@ -238,6 +238,7 @@ void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int
|
|
|
|
|
|
if (rssi_vec.size() == 50)
|
|
if (rssi_vec.size() == 50)
|
|
{
|
|
{
|
|
|
|
+
|
|
int max_rssi = *max_element(rssi_vec.begin(), rssi_vec.end());
|
|
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_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 left_min_acc = *min_element(left_acc_vec.begin(), left_acc_vec.end());
|
|
@@ -245,8 +246,9 @@ void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int
|
|
int right_max_acc = *max_element(right_acc_vec.begin(), right_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());
|
|
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)
|
|
|
|
|
|
+ if (max_rssi < 30 && 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++)
|
|
for (int i = 0; i < 50; i++)
|
|
{
|
|
{
|
|
pitch_reference_val += pitch_vec[i];
|
|
pitch_reference_val += pitch_vec[i];
|
|
@@ -336,19 +338,16 @@ void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int
|
|
|
|
|
|
left_init = 1;
|
|
left_init = 1;
|
|
|
|
|
|
- right_shoes_data_vector.pop_front();
|
|
|
|
- right_step_data_vector.pop_front();
|
|
|
|
-
|
|
|
|
- left_shoes_data_vector.pop_front();
|
|
|
|
- left_step_data_vector.pop_front();
|
|
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+
|
|
if (right_step_data_vector.size() > 5 && 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);
|
|
//ProcessResult(right_global_pos, right_step_data_vector, right_global_pos_vector, RIGHT_FOOT);
|
|
|
|
|
|
@@ -385,8 +384,11 @@ void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int
|
|
|
|
|
|
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))
|
|
|
|
|
|
+ //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)
|
|
|
|
+ // )
|
|
{
|
|
{
|
|
|
|
|
|
if (rssi < rssi_threshold)
|
|
if (rssi < rssi_threshold)
|