优化脚环猜拳游戏--现在用角度来判断

footRing_sdk_1.0/include/FingerGuessing.h

@@ -26,6 +26,18 @@ public:
 		int jump, int down, int rssi);
 
 private:
+	
+	deque<int> rssi_vec;
+	deque<int> pitch_vec;
+	deque<int> roll_vec;
+	deque<int> left_acc_vec;
+	deque<int> right_acc_vec;
+
+	int pitch_reference_val = 0;
+	int roll_reference_val = 0;
+
+	int has_init = 0;
+
 
 	deque<shoes_data_cell> left_shoes_data_vector;
 	deque<shoes_data_cell> right_shoes_data_vector;
@@ -64,7 +76,7 @@ private:
 	int right_init;
 
 
-	const int rssi_threshold = 25;
+	const int rssi_threshold = 30;
 
 
 };

footRing_sdk_1.0/include/pub.h

@@ -44,7 +44,7 @@ using namespace std;
 
 #define PRESS_MIN 41000
 
-#define GAME_VERSION "0.6.4"
+#define GAME_VERSION "0.6.5"
 
 enum CMD_MOTION
 {

footRing_sdk_1.0/src/FingerGuessing.cpp

@@ -1,5 +1,7 @@
 #include "FingerGuessing.h"
 
+#
+
 FingerGuessing::FingerGuessing()
 {
 	left_foot_has_init = 0;
@@ -207,6 +209,49 @@ void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int
 	//	right_zupt = 1;
 	//}
 
+	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)
+			{
+				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;
+			}
+
+		}
+
+	}
 
 
 	//缓存一步内的空中数据
@@ -228,7 +273,7 @@ void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int
 
 
 	//通过加速度、以及vector已经缓存了空中的数据来定位判断命令所需的时间戳，这个时间戳是起延时判断的效果，延时是为了等合适的RSSI
-	if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1 && left_step_data_vector.back().zupt == 1)
+	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))
 		{
@@ -254,7 +299,7 @@ void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int
 
 	}
 
-	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))
 		{
@@ -296,101 +341,56 @@ void FingerGuessing::Process(int time_stamp, int* right_pos, int* right_att, int
 		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)
 	{
 
-		//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)
-		{
 			addMotionCount(ROCK_COUNT);
 
 			result[0] = MOTION_ROCK;
 
-			std::cout << "FingureResult : MOTION_ROCK" << endl;
+			std::cout << "FingureResult : MOTION_ROCK " << "rssi  拳头"<<endl;
 		}
 		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);
-
-				result[0] = MOTION_PAPER;
+				//统计剪刀
+				addMotionCount(SCISSORS_COUNT);
 
-				std::cout << "MOTION_PAPER, RSSI : " << rssi << endl;
+				result[0] = MOTION_SCISSORS;
 
-				std::cout << "FingureResult : MOTION_PAPER" << endl;
+				std::cout << "FingureResult : MOTION_SCISSORS " << " 剪刀" << 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);
-
-				result[0] = MOTION_SCISSORS;
+				addMotionCount(PAPER_COUNT);
 
-				std::cout << "FingureResult : MOTION_SCISSORS" << endl;
+				result[0] = MOTION_PAPER;
 
+				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
 			{
-				if (fabsf(dy) > fabsf(dx))
-				{
-					addMotionCount(SCISSORS_COUNT);
+				addMotionCount(ROCK_COUNT);
 
-					result[0] = MOTION_SCISSORS;
+				result[0] = MOTION_ROCK;
 
-					std::cout << "dy > dx FingureResult : MOTION_SCISSORS" << endl;
-				}
-				else
-				{
-					addMotionCount(PAPER_COUNT);
-
-					result[0] = MOTION_PAPER;
-
-					std::cout << "dy < dx FingureResult : MOTION_PAPER" << endl;
-				}
+				std::cout << "FingureResult : MOTION_ROCK "<< "default 石头" << endl;
 			}
-
-
 		}
 
 		left_cmd_wait_time = 0;
 		right_cmd_wait_time = 0;
 
+		left_init = 0;
+		right_init = 0;
 	}
 
 }