diff --git a/controller/include/msp_MK32.h b/controller/include/msp_MK32.h index dc768d3..8f3f3a0 100644 --- a/controller/include/msp_MK32.h +++ b/controller/include/msp_MK32.h @@ -109,6 +109,17 @@ typedef enum { typedef void (*ActionFunc)(void); +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) + /*==============================================* * project-wide global variables * *----------------------------------------------*/ diff --git a/controller/msp_MK32.c b/controller/msp_MK32.c index dffc159..42ff8c2 100644 --- a/controller/msp_MK32.c +++ b/controller/msp_MK32.c @@ -145,53 +145,41 @@ void controller_init(void) (void)osThreadNew(Read_MK32, NULL, &MK32_Task_attributes); } -extern const ActionFunc g_ActionTable[KEY_MAX][MODE_MAX][SUB_MODE_MAX]; - -#define SAFE_DISPATCH(key, mode, submode) \ - do { \ - if ((key) < KEY_MAX && \ - (mode) < MODE_MAX && \ - (submode) < SUB_MODE_MAX) { \ - ActionFunc _fn = g_ActionTable[(key)][(mode)][(submode)]; \ - if (_fn) _fn(); \ - } \ - } while (0) - 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, _iMode, _iSubMode); + SAFE_DISPATCH(E_STOP); return; } // 根据模式判断换道（仅在竖直向左或向右生效） if (_pstMK32->CH4_SA == 1000) { - SAFE_DISPATCH(KEY_SA_UP, _iMode, _iSubMode); + SAFE_DISPATCH(KEY_SA_UP); return; } else if (_pstMK32->CH4_SA == -1000) { - SAFE_DISPATCH(KEY_SA_DOWN, _iMode, _iSubMode); + SAFE_DISPATCH(KEY_SA_DOWN); return; } else // 重置前进计时器以便下一次换道重新计数 { - SAFE_DISPATCH(KEY_SA_DEFAULT, _iMode, _iSubMode); + SAFE_DISPATCH(KEY_SA_DEFAULT); } // 自动巡航 if (_pstMK32->CH5_SB == -1000) { - SAFE_DISPATCH(KEY_SB_UP, _iMode, _iSubMode); + SAFE_DISPATCH(KEY_SB_UP); return; } else if(_pstMK32->CH5_SB == 1000) { - SAFE_DISPATCH(KEY_SB_DOWN, _iMode, _iSubMode); + SAFE_DISPATCH(KEY_SB_DOWN); return; } @@ -199,21 +187,21 @@ WEAK void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode) // 开关喷枪 if (_pstMK32->CH6_SC == -1000) { - SAFE_DISPATCH(KEY_SC_UP, _iMode, _iSubMode); + SAFE_DISPATCH(KEY_SC_UP); } else if (_pstMK32->CH6_SC == 1000) { - SAFE_DISPATCH(KEY_SC_DOWN, _iMode, _iSubMode); + SAFE_DISPATCH(KEY_SC_DOWN); } else { - SAFE_DISPATCH(KEY_SC_DEFAULT, _iMode, _iSubMode); + 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, _iMode, _iSubMode); + SAFE_DISPATCH(JOY_DEFAULT); return; } // 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效 @@ -223,19 +211,19 @@ WEAK void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode) if (abs(angle - 90) <= g_stCV.Joy_Sticker_Angle_Allowance) { - SAFE_DISPATCH(JOY_LEFT_UP, _iMode, _iSubMode); + SAFE_DISPATCH(JOY_LEFT_UP); } else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance) { - SAFE_DISPATCH(JOY_LEFT_DWON, _iMode, _iSubMode); + SAFE_DISPATCH(JOY_LEFT_DWON); } else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance) { - SAFE_DISPATCH(JOY_LEFT_RIGHT, _iMode, _iSubMode); + 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, _iMode, _iSubMode); + SAFE_DISPATCH(JOY_LEFT_LEFT); } else { diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c index 945adf7..e6d3ada 100644 --- a/project/paint_robot_new/paint_robot_new.c +++ b/project/paint_robot_new/paint_robot_new.c @@ -50,6 +50,7 @@ static PV_struct_define g_stPV; static IV_struct_define g_stIV; static int g_Is_All_Button_Reset = 0; // 1表示按钮正常工作，0表示需要复位 static int g_ichangLineState = 0; // 换道状态，0表示起始状态，1表示第一次旋转完成，2表示计时行走 完成 +static int g_bNotTable = 0; static int g_iLeft_Compensation = 0; static int g_iRight_Compensation = 0; @@ -294,6 +295,10 @@ static void Robot_Stop_After(void) { MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP_AFTER, NULL, 0); } + else + { + MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0); + } } static void Robot_Stop(void) @@ -493,6 +498,7 @@ static void Move_Halt_AngleError(void) void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode) { + if (0 == g_bNotTable) g_bNotTable = 1; // 急停 if (_pstMK32->CH8_SE == -1000 && _pstMK32->CH9_SF == -1000) { @@ -925,8 +931,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg) lua_print("PV Info\n"); lua_print("{%d, %d, %d, %d, %d, %ld}\n", g_stPV.RunMode, g_stPV.RobotSpeed, g_stPV.LaneChangeDistance, (int)g_stPV.Vertical_Calibration, g_stPV.IV_IsRestart_Notified, (long long)g_stPV.TimeStamp); - lua_print("\niPaint = %d\ng_stAngleError_Ctl.m_iPaintState = %d\ng_stAngleError_Ctl.m_iAnglelock = %d\ng_stAngleError_Ctl.m_ipaintOffCount = %d\ng_Is_All_Button_Reset = %d\n", - g_iPaint, g_stAngleError_Ctl.m_iPaintState, g_stAngleError_Ctl.m_iAnglelock, g_stAngleError_Ctl.m_ipaintOffCount, g_Is_All_Button_Reset); + lua_print("\niPaint = %d\ng_stAngleError_Ctl.m_iPaintState = %d\ng_stAngleError_Ctl.m_iAnglelock = %d\ng_stAngleError_Ctl.m_ipaintOffCount = %d\ng_Is_All_Button_Reset = %d\ng_bNotTable = %d\n", + g_iPaint, g_stAngleError_Ctl.m_iPaintState, g_stAngleError_Ctl.m_iAnglelock, g_stAngleError_Ctl.m_ipaintOffCount, g_Is_All_Button_Reset, g_bNotTable); break; } default: