优化脚环跑酷游戏

footRing_sdk_1.0/include/RunGame.h

@@ -24,6 +24,9 @@ private:
 	deque<int> left_roll_vec;
 	deque<int> right_roll_vec;
 
+	int last_left_cmd = -1;
+	int last_right_cmd = -1;
+
 
 	deque<shoes_data_cell> left_shoes_data_vector;
 	deque<shoes_data_cell> right_shoes_data_vector;

footRing_sdk_1.0/include/pub.h

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

footRing_sdk_1.0/src/RunGame.cpp

@@ -22,167 +22,217 @@ void RunGame::Process(int time_stamp, int* right_pos, int* right_att, int* right
 	int jump, int down, int rssi)
 {
 
-	if (rssi_delay.size() == 0)
+
+	int left_cmd = -1;
+	int right_cmd = -1;
+
+	if (left_roll_vec.size() == 0)
 	{
-		rssi_delay.push_back(rssi);
+		left_roll_vec.push_back(left_att[2]);
 	}
 	else
 	{
-		if (rssi <= rssi_delay.back())
+		if (left_att[2] >= left_roll_vec.back())
 		{
-			rssi_delay.push_back(rssi);
+			left_roll_vec.push_back(left_att[2]);
 		}
 		else
 		{
-			rssi_delay.clear();
+			left_roll_vec.clear();
 		}
-
-		rssi_delay.push_back(rssi);
 	}
 
-	if (rssi < 25 && rssi_delay.front() > 32)
+	if (right_roll_vec.size() == 0)
 	{
-		std::cout << "if (rssi < 21 && rssi_delay.front() > 30)" << std::endl;
-		left_zupt = 1;
-		right_zupt = 1;
+		right_roll_vec.push_back(right_att[2]);
 	}
-
-
-
-	//存放一步的空中数据到shoes_data_vector 队列中， 头必须是触地时刻信息
-	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);
-
-	//计算全局位置，选择当前的旋转量为这一步即将离地时候的方向，额外的旋转量为10度
-	//hoes_data_vector存的是鞋子上传的数据，它的方向是鞋子烧录时候的方向，并不是玩游戏时候的方向，需要旋转至玩游戏的方向
-	//step_data_vector 的位置已经选择到游戏的方向了，位置是相对位移，即相对于left_step_data_vector[0]的位置偏移
-	calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_shoes_data_vector[0].heading, 10.0f / 180 * PI, RIGHT_FOOT);
-	calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_shoes_data_vector[0].heading, -10.0f / 180 * PI, LEFT_FOOT);
-
-	//这里为了脚本容易观测，设置全局位置为0
-	if (left_step_data_vector.front().zupt == 0)
-	{
-		memset(left_global_pos, 0, 3 * sizeof(float));
-	}
-
-	if (right_step_data_vector.front().zupt == 0)
-	{
-		memset(right_global_pos, 0, 3 * sizeof(float));
-	}
-
-	if (left_step_data_vector.size() > 100)
+	else
 	{
-		left_step_data_vector.pop_front();
+		if (right_att[2] >= right_roll_vec.back())
+		{
+			right_roll_vec.push_back(right_att[2]);
+		}
+		else
+		{
+			right_roll_vec.clear();
+		}
 	}
 
-	if (right_step_data_vector.size() > 100)
+	if (right_roll_vec.size() > 9 && left_roll_vec.size() > 9)
 	{
-		right_step_data_vector.pop_front();
-	}
+		int right_dist = right_roll_vec.back() - right_roll_vec.front();
 
+		int left_dist = left_roll_vec.back() - left_roll_vec.front();
 
-	//判断命令触发
-	int left_cmd = -1;
-	if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1)
-	{
-		//跑酷游戏只要有空中的数据（不想其他游戏一样需要落地时候判断就好），就需要进行游戏
-		if (left_step_data_vector.back().zupt || left_step_data_vector.back().pitch < 0.3f)
+		if (right_dist > 800 && left_dist > 800)
 		{
-			//垫脚跑不触发命令。如果硬要垫脚跑，那么就等到他触地的时候再判断了
-
-			float max_val = left_step_data_vector.front().pos_x;
-
-			for (int i = 0; i < left_step_data_vector.size(); i++)
-			{
-				if (left_step_data_vector[i].pos_x > max_val)
-				{
-					max_val = left_step_data_vector[i].pos_x;
-				}
-			}
-
-			int roll_valid = 0;
-			float front_roll = left_step_data_vector.front().roll;
-			for (int i = 1; i < left_step_data_vector.size(); i++)
+			if (right_dist > 2000 || left_dist > 2000)
 			{
-				if (left_step_data_vector[i].roll > left_step_data_vector[i - 1].roll - 0.000001f)
+				std::cout << "right_dist : " << right_dist << " left_dist :" << left_dist << endl;
+				if (right_dist > left_dist + 500)
 				{
-					if (left_step_data_vector[i].roll - front_roll > 0.3f)
-					{
-						roll_valid = 1;
-					}
-					continue;
+					right_cmd = MOTION_RIGHT;
 				}
-				front_roll = left_step_data_vector[i].roll;
-			}
-			
-
-			//有效距离为0.23m
-			if (max_val - left_step_data_vector.back().pos_x > 0.23f && roll_valid)
-			{
-				if (!(rssi_delay.size() > 0 && rssi < 25 && rssi_delay.front() > 32))
+				else if (left_dist > right_dist + 500)
 				{
 					left_cmd = MOTION_LEFT;
-					std::cout << "motion: left_side, " << left_step_data_vector.back().pitch << std::endl;
 				}
-
-				left_step_data_vector.pop_front();
 			}
 		}
 	}
 
-	int right_cmd = -1;
-	if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1)
-	{
-
-		if (right_step_data_vector.back().zupt || right_step_data_vector.back().pitch < 0.3f)
-		{
-
-			float min_val = right_step_data_vector.front().pos_x;
-
-			for (int i = 0; i < right_step_data_vector.size(); i++)
-			{
-				if (right_step_data_vector[i].pos_x < min_val)
-				{
-					min_val = right_step_data_vector[i].pos_x;
-				}
-			}
-
-			int roll_valid = 0;
-			float front_roll = right_step_data_vector.front().roll;
-			for (int i = 1; i < right_step_data_vector.size(); i++)
-			{
-				if (right_step_data_vector[i].roll > right_step_data_vector[i - 1].roll - 0.000001f)
-				{
-					if (right_step_data_vector[i].roll - front_roll > 0.3f)
-					{
-						roll_valid = 1;
-					}
-					continue;
-				}
-				front_roll = right_step_data_vector[i].roll;
-			}
-
-			if (right_step_data_vector.back().pos_x - min_val > 0.23f && roll_valid)
-			{
-				if (!(rssi_delay.size() > 0 && rssi < 25 && rssi_delay.front() > 32))
-				{
-					right_cmd = MOTION_RIGHT;
+	result[0] = -1;
 
-					std::cout << "motion: right_side, " << left_step_data_vector.back().pitch << std::endl;
-				}
-				right_step_data_vector.pop_front();
-			}
-		}
+	if (left_cmd != last_left_cmd && left_cmd == MOTION_LEFT)
+	{
+		result[0] = MOTION_LEFT;
 	}
 
+	result[1] = -1;
+	if (right_cmd != last_right_cmd && right_cmd == MOTION_RIGHT)
+	{
+		result[1] = MOTION_RIGHT;
+	}
 
-
-	result[0] = left_cmd;
-
-	result[1] = right_cmd;
+	last_left_cmd = left_cmd;
+	
+	last_right_cmd = right_cmd;
+
+
+	////存放一步的空中数据到shoes_data_vector 队列中， 头必须是触地时刻信息
+	//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);
+
+	////计算全局位置，选择当前的旋转量为这一步即将离地时候的方向，额外的旋转量为10度
+	////hoes_data_vector存的是鞋子上传的数据，它的方向是鞋子烧录时候的方向，并不是玩游戏时候的方向，需要旋转至玩游戏的方向
+	////step_data_vector 的位置已经选择到游戏的方向了，位置是相对位移，即相对于left_step_data_vector[0]的位置偏移
+	//calGlobalPos(right_shoes_data_vector, right_step_data_vector, right_global_pos, right_shoes_data_vector[0].heading, 10.0f / 180 * PI, RIGHT_FOOT);
+	//calGlobalPos(left_shoes_data_vector, left_step_data_vector, left_global_pos, left_shoes_data_vector[0].heading, -10.0f / 180 * PI, LEFT_FOOT);
+
+	////这里为了脚本容易观测，设置全局位置为0
+	//if (left_step_data_vector.front().zupt == 0)
+	//{
+	//	memset(left_global_pos, 0, 3 * sizeof(float));
+	//}
+
+	//if (right_step_data_vector.front().zupt == 0)
+	//{
+	//	memset(right_global_pos, 0, 3 * sizeof(float));
+	//}
+
+	//if (left_step_data_vector.size() > 100)
+	//{
+	//	left_step_data_vector.pop_front();
+	//}
+
+	//if (right_step_data_vector.size() > 100)
+	//{
+	//	right_step_data_vector.pop_front();
+	//}
+
+
+	////判断命令触发
+	//int left_cmd = -1;
+	//if (left_step_data_vector.size() > 1 && left_step_data_vector.front().zupt == 1)
+	//{
+	//	//跑酷游戏只要有空中的数据（不想其他游戏一样需要落地时候判断就好），就需要进行游戏
+	//	if (left_step_data_vector.back().zupt || left_step_data_vector.back().pitch < 0.3f)
+	//	{
+	//		//垫脚跑不触发命令。如果硬要垫脚跑，那么就等到他触地的时候再判断了
+
+	//		float max_val = left_step_data_vector.front().pos_x;
+
+	//		for (int i = 0; i < left_step_data_vector.size(); i++)
+	//		{
+	//			if (left_step_data_vector[i].pos_x > max_val)
+	//			{
+	//				max_val = left_step_data_vector[i].pos_x;
+	//			}
+	//		}
+
+	//		int roll_valid = 0;
+	//		float front_roll = left_step_data_vector.front().roll;
+	//		for (int i = 1; i < left_step_data_vector.size(); i++)
+	//		{
+	//			if (left_step_data_vector[i].roll > left_step_data_vector[i - 1].roll - 0.000001f)
+	//			{
+	//				if (left_step_data_vector[i].roll - front_roll > 0.3f)
+	//				{
+	//					roll_valid = 1;
+	//				}
+	//				continue;
+	//			}
+	//			front_roll = left_step_data_vector[i].roll;
+	//		}
+	//		
+
+	//		//有效距离为0.23m
+	//		if (max_val - left_step_data_vector.back().pos_x > 0.23f && roll_valid)
+	//		{
+	//			if (!(rssi_delay.size() > 0 && rssi < 25 && rssi_delay.front() > 32))
+	//			{
+	//				left_cmd = MOTION_LEFT;
+	//				std::cout << "motion: left_side, " << left_step_data_vector.back().pitch << std::endl;
+	//			}
+
+	//			left_step_data_vector.pop_front();
+	//		}
+	//	}
+	//}
+
+	//int right_cmd = -1;
+	//if (right_step_data_vector.size() > 1 && right_step_data_vector.front().zupt == 1)
+	//{
+
+	//	if (right_step_data_vector.back().zupt || right_step_data_vector.back().pitch < 0.3f)
+	//	{
+
+	//		float min_val = right_step_data_vector.front().pos_x;
+
+	//		for (int i = 0; i < right_step_data_vector.size(); i++)
+	//		{
+	//			if (right_step_data_vector[i].pos_x < min_val)
+	//			{
+	//				min_val = right_step_data_vector[i].pos_x;
+	//			}
+	//		}
+
+	//		int roll_valid = 0;
+	//		float front_roll = right_step_data_vector.front().roll;
+	//		for (int i = 1; i < right_step_data_vector.size(); i++)
+	//		{
+	//			if (right_step_data_vector[i].roll > right_step_data_vector[i - 1].roll - 0.000001f)
+	//			{
+	//				if (right_step_data_vector[i].roll - front_roll > 0.3f)
+	//				{
+	//					roll_valid = 1;
+	//				}
+	//				continue;
+	//			}
+	//			front_roll = right_step_data_vector[i].roll;
+	//		}
+
+	//		if (right_step_data_vector.back().pos_x - min_val > 0.23f && roll_valid)
+	//		{
+	//			if (!(rssi_delay.size() > 0 && rssi < 25 && rssi_delay.front() > 32))
+	//			{
+	//				right_cmd = MOTION_RIGHT;
+
+	//				std::cout << "motion: right_side, " << left_step_data_vector.back().pitch << std::endl;
+	//			}
+	//			right_step_data_vector.pop_front();
+	//		}
+	//	}
+	//}
+
+
+
+	//result[0] = left_cmd;
+
+	//result[1] = right_cmd;
 
 	result[2] = getResultJump(jump);