diff --git a/controller/include/msp_MK32.h b/controller/include/msp_MK32.h index 2d6109c..4afc3b6 100644 --- a/controller/include/msp_MK32.h +++ b/controller/include/msp_MK32.h @@ -82,46 +82,6 @@ typedef enum { typedef void (*ActionFunc)(void); -#ifdef IS_TABLE -typedef enum { - KEY_SA_UP = 0, - KEY_SA_DOWN, - KEY_SA_DEFAULT, - KEY_SB_UP, - KEY_SB_DOWN, - KEY_SB_DEFAULT, - KEY_SC_UP, - KEY_SC_DOWN, - KEY_SC_DEFAULT, - KEY_SD_UP, - KEY_SD_DOWN, - KEY_SD_DEFAULT, - BTN_S1_CLICK, - BTN_S2_CLICK, - JOY_LEFT_UP, - JOY_LEFT_DWON, - JOY_LEFT_LEFT, - JOY_LEFT_RIGHT, - JOY_RIGHT_UP, - JOY_RIGHT_DWON, - JOY_RIGHT_LEFT, - JOY_RIGHT_RIGHT, - JOY_DEFAULT, - E_STOP, // 急停,MK32指SE和SF值-1000 - KEY_MAX -} E_BHBF_KEY; - -extern const ActionFunc g_ActionTable[KEY_MAX][MODE_MAX][SUB_MODE_MAX]; - -#define SAFE_DISPATCH(key) \ - do { \ - if ((key) < KEY_MAX) \ - { \ - ActionFunc _fn = g_ActionTable[(key)][(_iMode)][(_iSubMode)]; \ - if (_fn) _fn(); \ - } \ - } while (0) -#else typedef struct{ E_BHBF_RBMode m_iMode; //主模式 E_BHBF_RBSubMode m_iSubMode; //副模式 @@ -160,10 +120,10 @@ static inline void SafeDispatch(const TKeyTree *table, int count, int mode, int return; } } - +#ifdef TREE_SUPPORT_NULL // 如果不显式写NULL则表示空无动作 for (int i = 0; i < count; i++) { - if (table[i].m_iMode == mode && table[i].m_iSubMode == SUB_MODE_DEFAULT) + if (table[i].m_iMode == MODE_VERTICAL_LEFT && table[i].m_iSubMode == SUB_MODE_DEFAULT) { if (table[i].m_ptFunc != NULL) { @@ -184,6 +144,7 @@ static inline void SafeDispatch(const TKeyTree *table, int count, int mode, int return; } } +#endif } #define SAFE_DISPATCH(field) \ @@ -191,7 +152,6 @@ static inline void SafeDispatch(const TKeyTree *table, int count, int mode, int sizeof(g_ActionTable.field) / sizeof(TKeyTree), \ _iMode, _iSubMode) -#endif /*==============================================* * project-wide global variables * *----------------------------------------------*/ diff --git a/controller/msp_MK32.c b/controller/msp_MK32.c index f991613..9159320 100644 --- a/controller/msp_MK32.c +++ b/controller/msp_MK32.c @@ -145,93 +145,6 @@ 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; - // 急停 - if (_pstMK32->CH8_SE == -1000 && _pstMK32->CH9_SF == -1000) - { - SAFE_DISPATCH(E_STOP); - return; - } - - // 根据模式判断换道(仅在竖直向左或向右生效) - if (_pstMK32->CH4_SA == 1000) - { - SAFE_DISPATCH(KEY_SA_UP); - return; - } - else if (_pstMK32->CH4_SA == -1000) - { - SAFE_DISPATCH(KEY_SA_DOWN); - return; - } - else // 重置前进计时器以便下一次换道重新计数 - { - SAFE_DISPATCH(KEY_SA_DEFAULT); - } - - // 自动巡航 - if (_pstMK32->CH5_SB == -1000) - { - SAFE_DISPATCH(KEY_SB_UP); - return; - } - else if(_pstMK32->CH5_SB == 1000) - { - SAFE_DISPATCH(KEY_SB_DOWN); - return; - } - - - // 开关喷枪 - if (_pstMK32->CH6_SC == -1000) - { - SAFE_DISPATCH(KEY_SC_UP); - } - else if (_pstMK32->CH6_SC == 1000) - { - SAFE_DISPATCH(KEY_SC_DOWN); - } - else - { - SAFE_DISPATCH(KEY_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_DWON); - } - 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); - } -} -#else WEAK void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode) { if (_iMode <= MODE_NULL || _iMode >= MODE_MAX) return; @@ -317,4 +230,3 @@ WEAK void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode) MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0); } } -#endif diff --git a/library/common/common_cfg.h.in b/library/common/common_cfg.h.in index 83ad9ab..95c8a94 100644 --- a/library/common/common_cfg.h.in +++ b/library/common/common_cfg.h.in @@ -14,6 +14,7 @@ extern "C"{ #cmakedefine CONFIG_CAN_NEW_VERSION @CONFIG_CAN_NEW_VERSION@ #cmakedefine RD_VERSION "@RD_VERSION@" +#cmakedefine TREE_SUPPORT_NULL #cmakedefine IOC_100PIN #cmakedefine USE_TALNET #cmakedefine USE_LUA_ST diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index d164dfc..8d477b9 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -328,135 +328,70 @@ static void Robot_Move_Right(void) } } -#ifdef IS_TABLE -const ActionFunc g_ActionTable[KEY_MAX][MODE_MAX][SUB_MODE_MAX] = { - [E_STOP] = { - [MODE_DEFAULT] = { - [SUB_MODE_DEFAULT] = E_Robot_Stop, - }, - [MODE_VERTICAL_LEFT] = { - [SUB_MODE_DEFAULT] = E_Robot_Stop, - }, - [MODE_VERTICAL_RIGHT] = { - [SUB_MODE_DEFAULT] = E_Robot_Stop, - }, +#ifndef TREE_SUPPORT_NULL +const TKeyForest g_ActionTable = { + .E_Stop = { + {MODE_DEFAULT, SUB_MODE_DEFAULT, E_Robot_Stop}, + {MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, E_Robot_Stop}, + {MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, E_Robot_Stop} }, - [KEY_SA_UP] = { - [MODE_VERTICAL_LEFT] = { - [SUB_MODE_DEFAULT] = Vertical_Lane_Change_From_Right_To_Left_UP_Control, - }, - [MODE_VERTICAL_RIGHT] = { - [SUB_MODE_DEFAULT] = Vertical_Lane_Change_From_Left_To_Right_UP_Control, - }, - }, - [KEY_SA_DOWN] = { - [MODE_VERTICAL_LEFT] = { - [SUB_MODE_DEFAULT] = Vertical_Lane_Change_From_Right_To_Left_Down_Control, - }, - [MODE_VERTICAL_RIGHT] = { - [SUB_MODE_DEFAULT] = Vertical_Lane_Change_From_Left_To_Right_Down_Control, - }, - }, - [KEY_SA_DEFAULT] = { - [MODE_VERTICAL_LEFT] = { - [SUB_MODE_DEFAULT] = Lane_Change_State_Reset, - }, - [MODE_VERTICAL_RIGHT] = { - [SUB_MODE_DEFAULT] = Lane_Change_State_Reset, - }, - }, - [KEY_SB_UP] = { - [MODE_DEFAULT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Forward, - }, - [MODE_VERTICAL_LEFT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Forward_PID, - }, - [MODE_VERTICAL_RIGHT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Forward_PID, - } - }, - [KEY_SB_DOWN] = { - [MODE_DEFAULT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Backward, - }, - [MODE_VERTICAL_LEFT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Backward_PID, - }, - [MODE_VERTICAL_RIGHT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Backward_PID, - } - }, - [KEY_SC_DOWN] = { - [MODE_DEFAULT] = { - [SUB_MODE_DEFAULT] = Robot_Paint_OFF, - } - }, - [KEY_SC_DEFAULT] = { - [MODE_DEFAULT] = { - [SUB_MODE_DEFAULT] = Robot_Paint_OFF, - } - }, - [KEY_SC_UP] = { - [MODE_DEFAULT] = { - [SUB_MODE_DEFAULT] = Robot_Paint_ON, - } - }, - [JOY_DEFAULT] = { - [MODE_DEFAULT] = { - [SUB_MODE_DEFAULT] = Robot_Stop, - }, - [MODE_VERTICAL_LEFT] = { - [SUB_MODE_DEFAULT] = Robot_Stop_After, - }, - [MODE_VERTICAL_RIGHT] = { - [SUB_MODE_DEFAULT] = Robot_Stop_After, - }, - }, - [JOY_LEFT_UP] = { - [MODE_DEFAULT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Forward, - }, - [MODE_VERTICAL_LEFT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Forward_PID, - }, - [MODE_VERTICAL_RIGHT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Forward_PID, - }, - }, - [JOY_LEFT_DWON] = { - [MODE_DEFAULT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Backward, - }, - [MODE_VERTICAL_LEFT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Backward_PID, - }, - [MODE_VERTICAL_RIGHT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Backward_PID, - }, - }, - [JOY_LEFT_LEFT] = { - [MODE_DEFAULT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Left, - }, - [MODE_VERTICAL_LEFT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Left, - }, - [MODE_VERTICAL_RIGHT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Left, - }, - }, - [JOY_LEFT_RIGHT] = { - [MODE_DEFAULT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Right, - }, - [MODE_VERTICAL_LEFT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Right, - }, - [MODE_VERTICAL_RIGHT] = { - [SUB_MODE_DEFAULT] = Robot_Move_Right, - }, - } + .SA_Up = { + {MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Vertical_Lane_Change_From_Right_To_Left_UP_Control}, + {MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, Vertical_Lane_Change_From_Left_To_Right_UP_Control} + }, + .SA_Down = { + {MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Vertical_Lane_Change_From_Right_To_Left_Down_Control}, + {MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, Vertical_Lane_Change_From_Left_To_Right_Down_Control} + }, + .SA_Default = { + {MODE_DEFAULT, SUB_MODE_DEFAULT, Lane_Change_State_Reset}, + {MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Lane_Change_State_Reset}, + {MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, Lane_Change_State_Reset} + }, + .SB_Up = { + {MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Forward}, + {MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Forward_PID}, + {MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, Robot_Move_Forward_PID} + }, + .SB_Down = { + {MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Backward}, + {MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Backward_PID}, + {MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, Robot_Move_Backward_PID} + }, + .SC_Up = { + {MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Paint_ON} + }, + .SC_Down = { + {MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Paint_OFF} + }, + .SC_Default = { + {MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Paint_OFF} + }, + .Joy_Default = { + {MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Stop}, + {MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Stop_After}, + {MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, Robot_Stop_After} + }, + .Joy_Left_Up = { + {MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Forward}, + {MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Forward_PID}, + {MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, Robot_Move_Forward_PID} + }, + .Joy_Left_Down = { + {MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Backward}, + {MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Backward_PID}, + {MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, Robot_Move_Backward_PID} + }, + .Joy_Left_Left = { + {MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Left}, + {MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Left}, + {MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, Robot_Move_Left} + }, + .Joy_Left_Right = { + {MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Right}, + {MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Right}, + {MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, Robot_Move_Right} + } }; #else