|
@@ -17,22 +17,14 @@ JumpHouse::JumpHouse()
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- left_back_press_offset = 40000;
|
|
|
-
|
|
|
- right_back_press_offset = 40000;
|
|
|
-
|
|
|
- left_back_press_detect_time = 50;
|
|
|
-
|
|
|
-
|
|
|
- right_back_press_detect_time = 50;
|
|
|
|
|
|
}
|
|
|
|
|
|
bool JumpHouse::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) && max_acc[2] > 3000)
|
|
|
+ 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)
|
|
|
{
|
|
|
- std::cout << "JumpHouse::unzuptValid" << endl;
|
|
|
+
|
|
|
return true;
|
|
|
}
|
|
|
|
|
@@ -44,151 +36,107 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
int* left_pos, int* left_att, int* left_acc, int left_zupt, int left_front_press, int left_back_press,
|
|
|
int jump, int down, int rssi)
|
|
|
{
|
|
|
- //缓存一步内的空中数据
|
|
|
- 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);
|
|
|
-
|
|
|
- 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);
|
|
|
-
|
|
|
- //根据新来的数据,计算的全局位置
|
|
|
- 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);
|
|
|
-
|
|
|
- //补充后脚跟落地的问题
|
|
|
|
|
|
- left_back_press_vector.push_back(left_back_press);
|
|
|
+ //锟教斤拷锟斤拷为锟斤拷锟节碉拷锟斤拷
|
|
|
+ //
|
|
|
|
|
|
- if(left_back_press_vector.size()> 10)
|
|
|
+ if (has_init == 0)
|
|
|
{
|
|
|
- //表示有触地命令时候,触发检测, 是不是为假触地
|
|
|
- left_back_press_vector.pop_front();
|
|
|
- auto max_point = std::max_element(left_back_press_vector.begin(), left_back_press_vector.end());
|
|
|
- auto min_point = std::min_element(left_back_press_vector.begin(), left_back_press_vector.end());
|
|
|
+ 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 (left_back_press > *min_point + 2000 && max_point > min_point)
|
|
|
- {
|
|
|
- int offser_temp = *min_point;
|
|
|
-
|
|
|
- if (left_back_press_detect_time == 0)
|
|
|
- {
|
|
|
- left_back_press_detect_time = 50;
|
|
|
- }
|
|
|
|
|
|
-
|
|
|
- if (offser_temp < left_back_press_offset + 10)
|
|
|
- {
|
|
|
- left_back_press_detect_time = 50;
|
|
|
- left_back_press_offset = offser_temp;
|
|
|
- }
|
|
|
- }
|
|
|
- if (left_back_press_detect_time > 0)
|
|
|
+ if (rssi_vec.size() > 50)
|
|
|
{
|
|
|
- if (left_back_press < left_back_press_offset + 2000)
|
|
|
- {
|
|
|
- left_back_press_detect_time = 0;
|
|
|
-
|
|
|
- left_back_press_offset = 40000;
|
|
|
- }
|
|
|
+ rssi_vec.pop_front();
|
|
|
+ pitch_vec.pop_front();
|
|
|
+ roll_vec.pop_front();
|
|
|
+ left_acc_vec.pop_front();
|
|
|
+ right_acc_vec.pop_front();
|
|
|
}
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- right_back_press_vector.push_back(right_back_press);
|
|
|
-
|
|
|
- if (right_back_press_vector.size() > 10)
|
|
|
- {
|
|
|
- //表示有触地命令时候,触发检测, 是不是为假触地
|
|
|
- right_back_press_vector.pop_front();
|
|
|
- auto max_point = std::max_element(right_back_press_vector.begin(), right_back_press_vector.end());
|
|
|
- auto min_point = std::min_element(right_back_press_vector.begin(), right_back_press_vector.end());
|
|
|
|
|
|
- if (right_back_press > *min_point + 2000 && max_point > min_point)
|
|
|
+ if (rssi_vec.size() == 50)
|
|
|
{
|
|
|
- int offser_temp = *min_point;
|
|
|
-
|
|
|
- if (right_back_press_detect_time == 0)
|
|
|
- {
|
|
|
- right_back_press_detect_time = 50;
|
|
|
- right_back_press_offset = offser_temp;
|
|
|
- }
|
|
|
+ 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 (offser_temp < right_back_press_offset + 10)
|
|
|
+ if (max_rssi < 25 && left_max_acc - left_min_acc < 510 && right_max_acc - right_min_acc < 510)
|
|
|
{
|
|
|
- right_back_press_detect_time = 50;
|
|
|
- right_back_press_offset = offser_temp;
|
|
|
+ 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 (right_back_press_detect_time > 0)
|
|
|
- {
|
|
|
- if ( right_back_press < right_back_press_offset + 2000)
|
|
|
- {
|
|
|
- right_back_press_detect_time = 0;
|
|
|
|
|
|
- right_back_press_offset = 40000;
|
|
|
+ }
|
|
|
+ if (left_zupt == 1 && left_att[1] * 0.0001f > 0.5f)
|
|
|
+ {
|
|
|
+ std::cout << "if (left_zupt == 1 && left_att[1] * 0.0001f > 0.6f) " << left_att[1] * 0.0001f << endl;
|
|
|
+ left_zupt = 0;
|
|
|
+ }
|
|
|
+ if (right_zupt == 1 && right_att[1] * 0.0001f > 0.5f)
|
|
|
+ {
|
|
|
+ std::cout << "if (right_zupt == 1 && right_att[1] * 0.0001f > 0.6f) " << right_att[1] * 0.0001f << endl;
|
|
|
+ right_zupt = 0;
|
|
|
+ }
|
|
|
|
|
|
-
|
|
|
- }
|
|
|
- }
|
|
|
+ if (left_zupt)
|
|
|
+ {
|
|
|
+ left_zupt_extension_time = 5;
|
|
|
}
|
|
|
|
|
|
- left_front_press_vector.push_back(left_front_press);
|
|
|
- right_front_press_vector.push_back(right_front_press);
|
|
|
- if (left_front_press_vector.size() > 10)
|
|
|
+ if (right_zupt)
|
|
|
{
|
|
|
- left_front_press_vector.pop_front(); right_front_press_vector.pop_front();
|
|
|
+ right_zupt_extension_time = 5;
|
|
|
}
|
|
|
|
|
|
- if (left_front_press_vector.size() == 10)
|
|
|
+ if (left_zupt_extension_time > 0)
|
|
|
{
|
|
|
- auto front_max_point = max_element(left_front_press_vector.begin(), left_front_press_vector.end());
|
|
|
- auto front_min_point = min_element(left_front_press_vector.begin(), left_front_press_vector.end());
|
|
|
+ left_zupt = 1;
|
|
|
|
|
|
- auto back_max_point = max_element(left_back_press_vector.begin(), left_back_press_vector.end());
|
|
|
- auto back_min_point = min_element(left_back_press_vector.begin(), left_back_press_vector.end());
|
|
|
+ left_zupt_extension_time--;
|
|
|
+ }
|
|
|
|
|
|
- if (front_max_point > front_min_point && *front_max_point - *front_min_point > 2000 && *front_max_point - left_front_press < 50)
|
|
|
- {
|
|
|
- left_press_valid = 10;
|
|
|
- }
|
|
|
+ if (right_zupt_extension_time > 0)
|
|
|
+ {
|
|
|
+ right_zupt = 1;
|
|
|
|
|
|
- if (back_max_point > back_min_point && *back_max_point - *back_min_point > 2000 && *back_max_point - left_back_press < 50)
|
|
|
- {
|
|
|
- left_press_valid = 10;
|
|
|
- }
|
|
|
+ right_zupt_extension_time--;
|
|
|
}
|
|
|
|
|
|
- if (right_front_press_vector.size() == 10)
|
|
|
- {
|
|
|
- auto front_max_point = max_element(right_front_press_vector.begin(), right_front_press_vector.end());
|
|
|
- auto front_min_point = min_element(right_front_press_vector.begin(), right_front_press_vector.end());
|
|
|
+ //锟斤拷锟斤拷一锟斤拷锟节的匡拷锟斤拷锟斤拷锟斤拷
|
|
|
+ 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);
|
|
|
|
|
|
- auto back_max_point = max_element(right_back_press_vector.begin(), right_back_press_vector.end());
|
|
|
- auto back_min_point = min_element(right_back_press_vector.begin(), right_back_press_vector.end());
|
|
|
+ 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);
|
|
|
|
|
|
- if (front_max_point > front_min_point && *front_max_point - *front_min_point > 2000 && *front_max_point - right_front_press < 50)
|
|
|
- {
|
|
|
- right_press_valid = 10;
|
|
|
- }
|
|
|
+ //锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟捷o拷锟斤拷锟斤拷锟饺�拷锟轿伙拷锟�
|
|
|
+ calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_heading, 0, RIGHT_FOOT);
|
|
|
|
|
|
- if (back_max_point > back_min_point && *back_max_point - *back_min_point > 2000 && *back_max_point - right_back_press < 50)
|
|
|
- {
|
|
|
- right_press_valid = 10;
|
|
|
- }
|
|
|
- }
|
|
|
+ calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_heading, 0, LEFT_FOOT);
|
|
|
|
|
|
- //通过加速度、以及vector已经缓存了空中的数据来定位判断命令所需的时间戳,这个时间戳是起延时判断的效果,延时是为了等合适的RSSI
|
|
|
- if (left_step_data_vector.size() > 4 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1
|
|
|
- && (left_back_press_detect_time == 0 || left_back_press_detect_time == 50))
|
|
|
+ //通锟斤拷锟斤拷锟劫度★拷锟皆硷拷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) && press_valid)
|
|
|
- if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left))
|
|
|
+ if (unzuptValid(max_acc_unzupt_left, min_acc_unzupt_left) || left_front_press & 0x01)
|
|
|
{
|
|
|
+ left_zupt_extension_time = 20;
|
|
|
|
|
|
left_cmd_wait_time = time_stamp;
|
|
|
|
|
@@ -198,13 +146,12 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
|
|
|
}
|
|
|
|
|
|
- if (right_step_data_vector.size() > 4 && right_step_data_vector.front().zupt == 1 && right_step_data_vector.back().zupt == 1
|
|
|
- && (right_back_press_detect_time == 0 || right_back_press_detect_time == 50))
|
|
|
+ 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) && press_valid)
|
|
|
- if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right))
|
|
|
+ if (unzuptValid(max_acc_unzupt_right, min_acc_unzupt_right) || right_front_press & 0x01)
|
|
|
{
|
|
|
+ right_zupt_extension_time = 20;
|
|
|
|
|
|
right_cmd_wait_time = time_stamp;
|
|
|
|
|
@@ -214,90 +161,63 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
|
|
|
}
|
|
|
|
|
|
- /*添加加速度过滤器
|
|
|
- for (int i = 1; i < 3; i++)
|
|
|
- {
|
|
|
- for (int k = 0; k < 3; k++)
|
|
|
- {
|
|
|
- left_acc_buff[i - 1][k] = left_acc_buff[i][k];
|
|
|
- right_acc_buff[i - 1][k] = right_acc_buff[i][k];
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- for (int i = 0; i < 3; i++)
|
|
|
- {
|
|
|
- left_acc_buff[2][i] = left_acc[i]; right_acc_buff[2][i] = right_acc[i];
|
|
|
- }
|
|
|
-
|
|
|
- for (int i = 0; i < 3; i++)
|
|
|
- {
|
|
|
- left_acc[i] = max(max(left_acc_buff[0][i], left_acc_buff[1][i]), left_acc_buff[2][i]);
|
|
|
-
|
|
|
- right_acc[i] = max(max(right_acc_buff[0][i], right_acc_buff[1][i]), right_acc_buff[2][i]);
|
|
|
-
|
|
|
- left_acc[i] = (left_acc_buff[0][i] + left_acc_buff[1][i] + left_acc_buff[2][i]) * 0.3333;
|
|
|
+ //std::cout << left_acc[0] << ", " << left_acc[1] << ", " << left_acc[2] << std::endl;
|
|
|
+ setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
|
|
|
|
|
|
- right_acc[i] = (right_acc_buff[0][i] + right_acc_buff[1][i] + right_acc_buff[2][i]) * 0.3333;
|
|
|
-
|
|
|
- }*/
|
|
|
-
|
|
|
- if (left_step_data_vector.size() > 1 && left_zupt == 0)
|
|
|
- {
|
|
|
- setPolarAccUnzupt(max_acc_unzupt_left, min_acc_unzupt_left, left_acc, left_zupt);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- for (int k = 0; k < 3; k++)
|
|
|
- {
|
|
|
- max_acc_unzupt_left[k] = left_acc[k];
|
|
|
- min_acc_unzupt_left[k] = left_acc[k];
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- if (right_step_data_vector.size() > 1 && right_zupt == 0)
|
|
|
- {
|
|
|
- setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- for (int k = 0; k < 3; k++)
|
|
|
- {
|
|
|
- max_acc_unzupt_right[k] = right_acc[k];
|
|
|
- min_acc_unzupt_right[k] = left_acc[k];
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- static int count = 0;
|
|
|
-
|
|
|
- count++;
|
|
|
+ setPolarAccUnzupt(max_acc_unzupt_right, min_acc_unzupt_right, right_acc, right_zupt);
|
|
|
+
|
|
|
|
|
|
- //重置命令
|
|
|
+ //锟斤拷锟斤拷锟斤拷锟斤拷
|
|
|
memset(result, -1, 4 * sizeof(int));
|
|
|
|
|
|
- //判断双脚触地的时候, rssi < 23 必须归位,不然也没办法处理
|
|
|
- //看起来rssi的延迟有够烂的
|
|
|
|
|
|
- if (left_init == 1 && time_stamp - left_cmd_wait_time >= 9 && time_stamp - left_cmd_wait_time < 20 && left_press_valid > 0)
|
|
|
+ //锟叫讹拷双锟脚达拷锟截碉拷时锟斤拷, rssi < 23 锟斤拷锟斤拷锟轿伙拷锟斤拷锟饺灰裁伙拷旆�拷锟斤拷锟�
|
|
|
+ //锟斤拷锟斤拷锟斤拷rssi锟斤拷锟接筹拷锟叫癸拷锟矫碉拷
|
|
|
+
|
|
|
+ if (left_init == 1 && time_stamp - left_cmd_wait_time >= 9 && time_stamp - left_cmd_wait_time < 30)
|
|
|
{
|
|
|
- if (right_init == 1 && right_press_valid > 0)
|
|
|
+ std::cout << " left_att[1] : " << left_att[1] << " right_att[1] : " << right_att[1] << std::endl;
|
|
|
+ if (right_init == 1)
|
|
|
{
|
|
|
if (rssi < rssi_threshold)
|
|
|
{
|
|
|
result[0] = MOTION_BOTH_ON_FLOOR_CLOSE;
|
|
|
|
|
|
- std::cout << " MOTION_BOTH_ON_FLOOR_CLOSE " << count << " " << rssi << std::endl;
|
|
|
+ std::cout << " MOTION_BOTH_ON_FLOOR_CLOSE " << rssi << ", " << " 锟较斤拷" <<std::endl;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
result[0] = MOTION_BOTH_ON_FLOOR_OPEN;
|
|
|
- std::cout << " MOTION_BOTH_ON_FLOOR_OPEN " << count << " " << rssi << std::endl;
|
|
|
+
|
|
|
+ std::cout << " MOTION_BOTH_ON_FLOOR_OPEN "<< rssi << ", " << " 锟斤拷锟斤拷"<<std::endl;
|
|
|
}
|
|
|
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- result[0] = MOTION_LEFT_ON_FLOOR;
|
|
|
- std::cout << " MOTION_LEFT_ON_FLOOR " << count << " "<<time_stamp << std::endl;
|
|
|
+ std::cout << "pitch_reference_val : " << pitch_reference_val << endl;
|
|
|
+ if (abs(left_att[1] - right_att[1] - pitch_reference_val) < 2000)
|
|
|
+ {
|
|
|
+ if (rssi < rssi_threshold)
|
|
|
+ {
|
|
|
+ result[0] = MOTION_BOTH_ON_FLOOR_CLOSE;
|
|
|
+
|
|
|
+ std::cout << " MOTION_BOTH_ON_FLOOR_CLOSE " << rssi << ", " << " 锟较斤拷" << std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ result[0] = MOTION_BOTH_ON_FLOOR_OPEN;
|
|
|
+ std::cout << " MOTION_BOTH_ON_FLOOR_OPEN " << rssi << ", " << " 锟斤拷锟斤拷" << std::endl;
|
|
|
+ }
|
|
|
+
|
|
|
+ right_shoes_data_vector.pop_front();
|
|
|
+ right_step_data_vector.pop_front();
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ result[0] = MOTION_LEFT_ON_FLOOR;
|
|
|
+ std::cout << " MOTION_LEFT_ON_FLOOR " << time_stamp << ", " << " 锟襟单斤拷" << std::endl;
|
|
|
+ }
|
|
|
|
|
|
}
|
|
|
|
|
@@ -305,25 +225,24 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
right_cmd_wait_time = 0;
|
|
|
left_init = 0;
|
|
|
left_cmd_wait_time = 0;
|
|
|
- left_press_valid = 0;
|
|
|
|
|
|
- right_press_valid = 0;
|
|
|
|
|
|
}
|
|
|
- else if (right_init == 1 && time_stamp - right_cmd_wait_time >= 9 && time_stamp - right_cmd_wait_time < 20 && right_press_valid > 0)
|
|
|
+ else if (right_init == 1 && time_stamp - right_cmd_wait_time >= 9 && time_stamp - right_cmd_wait_time < 30)
|
|
|
{
|
|
|
- if (left_init == 1 && left_press_valid > 0)
|
|
|
+ std::cout << " left_att[1] : " << left_att[1] << " right_att[1] : " << right_att[1] << std::endl;
|
|
|
+ if (left_init == 1)
|
|
|
{
|
|
|
if (rssi < rssi_threshold)
|
|
|
{
|
|
|
result[0] = MOTION_BOTH_ON_FLOOR_CLOSE;
|
|
|
|
|
|
- std::cout << " MOTION_BOTH_ON_FLOOR_CLOSE " << count << " "<< rssi << std::endl;
|
|
|
+ std::cout << " MOTION_BOTH_ON_FLOOR_CLOSE " << " "<< rssi << ", " << " 锟较斤拷" << std::endl;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
result[0] = MOTION_BOTH_ON_FLOOR_OPEN;
|
|
|
- std::cout << " MOTION_BOTH_ON_FLOOR_OPEN " << count << " " << rssi << std::endl;
|
|
|
+ std::cout << " MOTION_BOTH_ON_FLOOR_OPEN " << " " << rssi << ", " << " 锟斤拷锟斤拷"<<std::endl;
|
|
|
}
|
|
|
|
|
|
left_init = 0;
|
|
@@ -332,8 +251,29 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- result[0] = MOTION_RIGHT_ON_FLOOR;
|
|
|
- std::cout << " MOTION_RIGHT_ON_FLOOR " << count << std::endl;
|
|
|
+ std::cout << "pitch_reference_val : " << pitch_reference_val << endl;
|
|
|
+ if (abs(left_att[1] - right_att[1] - pitch_reference_val) < 2000)
|
|
|
+ {
|
|
|
+ if (rssi < rssi_threshold)
|
|
|
+ {
|
|
|
+ result[0] = MOTION_BOTH_ON_FLOOR_CLOSE;
|
|
|
+
|
|
|
+ std::cout << " MOTION_BOTH_ON_FLOOR_CLOSE " << rssi << ", " << " 锟较斤拷" << std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ result[0] = MOTION_BOTH_ON_FLOOR_OPEN;
|
|
|
+ std::cout << " MOTION_BOTH_ON_FLOOR_OPEN " << rssi << ", " << " 锟斤拷锟斤拷" << std::endl;
|
|
|
+ }
|
|
|
+
|
|
|
+ left_shoes_data_vector.pop_front();
|
|
|
+ left_step_data_vector.pop_front();
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ result[0] = MOTION_RIGHT_ON_FLOOR;
|
|
|
+ std::cout << " MOTION_RIGHT_ON_FLOOR " << time_stamp << ", " << "锟揭碉拷锟斤拷" << std::endl;
|
|
|
+ }
|
|
|
|
|
|
}
|
|
|
right_init = 0;
|
|
@@ -341,43 +281,24 @@ void JumpHouse::Process(int time_stamp, int* right_pos, int* right_att, int* rig
|
|
|
left_init = 0;
|
|
|
left_cmd_wait_time = 0;
|
|
|
|
|
|
- left_press_valid = 0;
|
|
|
-
|
|
|
- right_press_valid = 0;
|
|
|
}
|
|
|
- if (right_init == 1 && time_stamp - right_cmd_wait_time >= 20)
|
|
|
+
|
|
|
+ if (right_init == 1 && time_stamp - right_cmd_wait_time >= 30)
|
|
|
{
|
|
|
right_init = 0;
|
|
|
right_cmd_wait_time = 0;
|
|
|
}
|
|
|
|
|
|
- if (left_init == 1 && time_stamp - left_cmd_wait_time >= 20)
|
|
|
+ if (left_init == 1 && time_stamp - left_cmd_wait_time >= 30)
|
|
|
{
|
|
|
left_init = 0;
|
|
|
left_cmd_wait_time = 0;
|
|
|
}
|
|
|
|
|
|
- //else if (count < 850 && count > 830)
|
|
|
- //{
|
|
|
- // std::cout << "right_init : " << right_init << " time_stamp - right_cmd_wait_time : " << time_stamp - right_cmd_wait_time
|
|
|
- // << " right_press_valid : " << right_press_valid << endl;
|
|
|
- //}
|
|
|
|
|
|
if (result[0] != -1)
|
|
|
{
|
|
|
addMotionCount(JUMP_COUNT);
|
|
|
}
|
|
|
|
|
|
- if (left_back_press_detect_time > 0) left_back_press_detect_time--;
|
|
|
- if (right_back_press_detect_time > 0) right_back_press_detect_time--;
|
|
|
-
|
|
|
- if (left_press_valid > 0)
|
|
|
- {
|
|
|
- left_press_valid--;
|
|
|
- }
|
|
|
-
|
|
|
- if (right_press_valid > 0)
|
|
|
- {
|
|
|
- right_press_valid--;
|
|
|
- }
|
|
|
}
|