From 0152170f6f3e2214254c892f882cc2ecfc605d3c Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Wed, 23 Sep 2026 14:22:33 +0800 Subject: [PATCH] =?UTF-8?q?=E3=80=90=E5=A4=A7=E5=BE=B7=E9=B2=81=E4=BC=8A?= =?UTF-8?q?=E3=80=91=E6=8C=89=E9=94=AE=E6=A3=AE=E6=9E=97=E3=80=81=E6=A0=91?= =?UTF-8?q?=E6=9F=A5=E6=89=BE=E8=A1=A8=E6=96=B9=E5=BC=8F=E5=AE=9E=E7=8E=B0?= =?UTF-8?q?=E6=8C=89=E9=94=AE=E5=A4=84=E7=90=86=E5=92=8C=E8=A7=A3=E8=80=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- controller/include/msp_MK32.h | 101 ++++++++++++++++++---- controller/msp_MK32.c | 88 +++++++++++++++++++ project/paint_robot_new/paint_robot_new.c | 63 ++++++++++++++ 3 files changed, 237 insertions(+), 15 deletions(-) diff --git a/controller/include/msp_MK32.h b/controller/include/msp_MK32.h index 8f3f3a0..0e85084 100644 --- a/controller/include/msp_MK32.h +++ b/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 * *----------------------------------------------*/ diff --git a/controller/msp_MK32.c b/controller/msp_MK32.c index 42ff8c2..f991613 100644 --- a/controller/msp_MK32.c +++ b/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 diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 99f4fad..9096914 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/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上报周期（单位毫秒）