优化跑酷内容-脚环

footRing_sdk_1.0/include/RunGame.h

@@ -19,6 +19,7 @@ public:
 
 private:
 
+	deque<int> rssi_delay;
 
 	deque<int> left_roll_vec;
 	deque<int> right_roll_vec;

footRing_sdk_1.0/include/pub.h

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

footRing_sdk_1.0/src/RunGame.cpp

@@ -22,62 +22,160 @@ void RunGame::Process(int time_stamp, int* right_pos, int* right_att, int* right
 	int jump, int down, int rssi)
 {
 
-
-	//判断命令触发
-	int left_cmd = -1;
-
-	if (left_roll_vec.size() == 0)
+	if (rssi_delay.size() == 0)
 	{
-		left_roll_vec.push_back(left_att[2]);
+		rssi_delay.push_back(rssi);
 	}
 	else
 	{
-		if (left_roll_vec.back() < left_att[2])
+		if (rssi <= rssi_delay.back())
 		{
-			left_roll_vec.push_back(left_att[2]);
-
-			if (left_roll_vec.back() > left_roll_vec.front() + 3000)
-			{
-
-				left_cmd = MOTION_LEFT;
-				std::cout << "left side" << endl;
-				right_roll_vec.clear();
-				left_roll_vec.clear();
-			}
+			rssi_delay.push_back(rssi);
 		}
 		else
 		{
-			left_roll_vec.clear();
+			rssi_delay.clear();
 		}
-	
+
+		rssi_delay.push_back(rssi);
 	}
 
-	int right_cmd = -1;
+	if (rssi < 25 && rssi_delay.front() > 32)
+	{
+		std::cout << "if (rssi < 21 && rssi_delay.front() > 30)" << std::endl;
+		left_zupt = 1;
+		right_zupt = 1;
+	}
+
+
+
+	//存放一步的空中数据到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);
 
-	if (right_roll_vec.size() == 0 )
+	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)
 	{
-		right_roll_vec.push_back(right_att[2]);
+		memset(left_global_pos, 0, 3 * sizeof(float));
 	}
-	else
+
+	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 (right_roll_vec.back() < right_att[2])
+		//跑酷游戏只要有空中的数据（不想其他游戏一样需要落地时候判断就好），就需要进行游戏
+		if (left_step_data_vector.back().zupt || left_step_data_vector.back().pitch < 0.3f)
 		{
-			right_roll_vec.push_back(right_att[2]);
+			//垫脚跑不触发命令。如果硬要垫脚跑，那么就等到他触地的时候再判断了
 
-			if (right_roll_vec.back() > right_roll_vec.front() + 3000)
+			float max_val = left_step_data_vector.front().pos_x;
+
+			for (int i = 0; i < left_step_data_vector.size(); i++)
 			{
-				right_cmd = MOTION_RIGHT;
+				if (left_step_data_vector[i].pos_x > max_val)
+				{
+					max_val = left_step_data_vector[i].pos_x;
+				}
+			}
 
-				std::cout << "right side" << endl;
-				left_roll_vec.clear();
-				right_roll_vec.clear();
+			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();
 			}
 		}
-		else
+	}
+
+	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)
 		{
-			right_roll_vec.clear();
-		}
 
+			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();
+			}
+		}
 	}
 
 
@@ -90,6 +188,10 @@ void RunGame::Process(int time_stamp, int* right_pos, int* right_att, int* right
 
 	result[3] = getResultDown(down);
 
+	//result[2] = left_zupt;
+
+	//result[3] = right_zupt;
+
 
 	left_acc_deque.push_back(left_acc[2]);
 	right_acc_deque.push_back(right_acc[2]);