【大德鲁伊】按键森林、树查找表方式实现按键处理和解耦

controller/include/msp_MK32.h

@@ -66,6 +66,22 @@ extern "C"{
 /*==============================================*
  *      constants or macros define              *
  *----------------------------------------------*/
+typedef enum {
+	MODE_NULL = 0,
+	MODE_DEFAULT,
+	MODE_VERTICAL_LEFT,
+	MODE_VERTICAL_RIGHT,
+	MODE_MAX
+} E_BHBF_RBMode;
+
+typedef enum {
+	SUB_MODE_NULL = 0,
+	SUB_MODE_MAX
+} E_BHBF_RBSubMode;
+
+typedef void (*ActionFunc)(void);
+
+#ifdef IS_TABLE
 typedef enum {
 	KEY_SA_UP = 0,
 	KEY_SA_DOWN,
@@ -92,23 +108,8 @@ typedef enum {
 	JOY_DEFAULT,
 	E_STOP, // 急停，MK32指SE和SF值-1000
 	KEY_MAX
-} E_BHBF_RBMode;
-
-typedef enum {
-	MODE_NULL = 0,
-	MODE_DEFAULT,
-	MODE_VERTICAL_LEFT,
-	MODE_VERTICAL_RIGHT,
-	MODE_MAX
 } E_BHBF_KEY;
 
-typedef enum {
-	SUB_MODE_NULL = 0,
-	SUB_MODE_MAX
-} E_BHBF_MODE3;
-
-typedef void (*ActionFunc)(void);
-
 extern const ActionFunc g_ActionTable[KEY_MAX][MODE_MAX][SUB_MODE_MAX];
 
 #define SAFE_DISPATCH(key)                      \
@@ -119,7 +120,77 @@ extern const ActionFunc g_ActionTable[KEY_MAX][MODE_MAX][SUB_MODE_MAX];
             if (_fn) _fn();                     \
         }                                       \
     } while (0)
+#else
+typedef struct{
+	E_BHBF_RBMode m_iMode;			//主模式
+	E_BHBF_RBSubMode m_iSubMode;	//副模式
+	ActionFunc m_ptFunc;			//执行函数
+} TKeyTree;
+
+typedef struct {
+	TKeyTree E_Stop[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree SA_Up[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree SA_Down[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree SA_Default[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree SB_Up[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree SB_Down[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree SC_Up[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree SC_Down[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree SC_Default[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree Joy_Default[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree Joy_Left_Up[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree Joy_Left_Down[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree Joy_Left_Left[MODE_MAX*SUB_MODE_MAX];
+	TKeyTree Joy_Left_Right[MODE_MAX*SUB_MODE_MAX];
+} TKeyForest;
+
+extern const TKeyForest g_ActionTable;
+
+static inline void SafeDispatch(const TKeyTree *table, int count, int mode, int sub_mode)
+{
+    for (int i = 0; i < count; i++) 
+	{
+        if (table[i].m_iMode == mode && table[i].m_iSubMode == sub_mode) 
+		{
+            if (table[i].m_ptFunc != NULL) 
+			{
+                table[i].m_ptFunc();
+            }
+            return;
+        }
+    }
+
+	// for (int i = 0; i < count; i++) 
+	// {
+    //     if (table[i].m_iMode == mode && table[i].m_iSubMode == SUB_MODE_NULL) 
+	// 	{
+    //         if (table[i].m_ptFunc != NULL) 
+	// 		{
+    //             table[i].m_ptFunc();
+    //         }
+    //         return;
+    //     }
+    // }
+
+	for (int i = 0; i < count; i++) 
+	{
+        if (table[i].m_iMode == MODE_DEFAULT && table[i].m_iSubMode == SUB_MODE_NULL) 
+		{
+            if (table[i].m_ptFunc != NULL) 
+			{
+                table[i].m_ptFunc();
+            }
+            return;
+        }
+    }
+}
+
+#define SAFE_DISPATCH(field) \
+    SafeDispatch(g_ActionTable.field, \
+                 sizeof(g_ActionTable.field) / sizeof(TKeyTree), \
+                 _iMode, _iSubMode)
 
+#endif
 /*==============================================*
  *      project-wide global variables           *
  *----------------------------------------------*/

controller/msp_MK32.c

@@ -145,6 +145,7 @@ void controller_init(void)
   (void)osThreadNew(Read_MK32, NULL, &MK32_Task_attributes);
 }
 
+#ifdef IS_TABLE
 WEAK void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode)
 {
 	if (_iMode <= MODE_NULL || _iMode >= MODE_MAX) return;
@@ -230,3 +231,90 @@ WEAK void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode)
 		MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
 	}
 }
+#else
+WEAK void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode)
+{
+	if (_iMode <= MODE_NULL || _iMode >= MODE_MAX) return;
+	// 急停
+	if (_pstMK32->CH8_SE == -1000 && _pstMK32->CH9_SF == -1000)
+	{
+		SAFE_DISPATCH(E_Stop);
+		return;
+	}
+
+	// 根据模式判断换道（仅在竖直向左或向右生效）
+	if (_pstMK32->CH4_SA == 1000)
+	{
+		SAFE_DISPATCH(SA_Up);
+		return;
+	}
+	else if (_pstMK32->CH4_SA == -1000)
+	{
+		SAFE_DISPATCH(SA_Down);
+		return;
+	}
+	else // 重置前进计时器以便下一次换道重新计数
+	{
+		SAFE_DISPATCH(SA_Default);
+	}
+
+	// 自动巡航
+	if (_pstMK32->CH5_SB == -1000)
+	{
+		SAFE_DISPATCH(SB_Up);
+		return;
+	}
+	else if(_pstMK32->CH5_SB == 1000)
+	{
+		SAFE_DISPATCH(SB_Down);
+		return;
+	}
+
+
+	// 开关喷枪
+	if (_pstMK32->CH6_SC == -1000)
+	{
+		SAFE_DISPATCH(SC_Up);
+	}
+	else if (_pstMK32->CH6_SC == 1000)
+	{
+		SAFE_DISPATCH(SC_Down);
+	}
+	else
+	{
+		SAFE_DISPATCH(SC_Default);
+	}
+
+	// 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去
+	if (abs(_pstMK32->CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(_pstMK32->CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance)
+	{
+		SAFE_DISPATCH(Joy_Default);
+		return;
+	}
+	// 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效
+
+	// 摇杆角度死区判断
+	int angle = atan2(_pstMK32->CH2_LY_V, _pstMK32->CH3_LY_H) * 180 / M_PI;
+
+	if (abs(angle - 90) <= g_stCV.Joy_Sticker_Angle_Allowance)
+	{
+		SAFE_DISPATCH(Joy_Left_Up);
+	}
+	else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
+	{
+		SAFE_DISPATCH(Joy_Left_Down);
+	}
+	else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)
+	{
+		SAFE_DISPATCH(Joy_Left_Right);
+	}
+	else if (abs(angle - 180) <= g_stCV.Joy_Sticker_Angle_Allowance || abs(angle - (-180)) <= g_stCV.Joy_Sticker_Angle_Allowance)
+	{
+		SAFE_DISPATCH(Joy_Left_Left);
+	}
+	else
+	{
+		MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
+	}
+}
+#endif

project/paint_robot_new/paint_robot_new.c

@@ -328,6 +328,7 @@ static void Robot_Move_Right(void)
 	}
 }
 
+#ifdef IS_TABLE
 const ActionFunc g_ActionTable[KEY_MAX][MODE_MAX][SUB_MODE_MAX] = {
 	[E_STOP] = {
 		[MODE_DEFAULT] = {
@@ -457,6 +458,68 @@ const ActionFunc g_ActionTable[KEY_MAX][MODE_MAX][SUB_MODE_MAX] = {
 		},
     }
 };
+#else
+
+const TKeyForest g_ActionTable = {
+	.E_Stop = {
+		{MODE_DEFAULT, SUB_MODE_NULL, E_Robot_Stop}
+	},
+	.SA_Up = {
+		{MODE_DEFAULT, SUB_MODE_NULL, NULL},
+		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, Vertical_Lane_Change_From_Right_To_Left_UP_Control},
+		{MODE_VERTICAL_RIGHT, SUB_MODE_NULL, Vertical_Lane_Change_From_Left_To_Right_UP_Control}
+	},
+	.SA_Down = {
+		{MODE_DEFAULT, SUB_MODE_NULL, NULL},
+		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, Vertical_Lane_Change_From_Right_To_Left_Down_Control},
+		{MODE_VERTICAL_RIGHT, SUB_MODE_NULL, Vertical_Lane_Change_From_Left_To_Right_Down_Control}
+	},
+	.SA_Default = {
+		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, Lane_Change_State_Reset}
+	},
+	.SB_Up = {
+		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Move_Forward},
+		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, Robot_Move_Forward_PID}
+	},
+	.SB_Down = {
+		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Move_Backward},
+		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, Robot_Move_Backward_PID}
+	},
+	.SC_Up = {
+		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Paint_ON},
+		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, NULL},
+		{MODE_VERTICAL_RIGHT, SUB_MODE_NULL, NULL}
+	},
+	.SC_Down = {
+		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Paint_OFF},
+		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, NULL},
+		{MODE_VERTICAL_RIGHT, SUB_MODE_NULL, NULL}
+	},
+	.SC_Default = {
+		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Paint_OFF},
+		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, NULL},
+		{MODE_VERTICAL_RIGHT, SUB_MODE_NULL, NULL}
+	},
+	.Joy_Default = {
+		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Stop},
+		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, Robot_Stop_After}
+	},
+	.Joy_Left_Up = {
+		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Move_Forward},
+		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, Robot_Move_Forward_PID}
+	},
+	.Joy_Left_Down = {
+		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Move_Backward},
+		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, Robot_Move_Backward_PID}
+	},
+	.Joy_Left_Left = {
+		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Move_Left}
+	},
+	.Joy_Left_Right = {
+		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Move_Right}
+	}
+};
+#endif
 
 #define IV_SEND_TIME 50 // IV上报周期（单位毫秒）