LiZongdi
/
robot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

查表法调通

master
Lizongdi 1 day ago
parent
b6bed4bf13
commit
5e04364201
4 changed files with 67 additions and 259 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 46
      controller/include/msp_MK32.h
  2. 88
      controller/msp_MK32.c
  3. 1
      library/common/common_cfg.h.in
  4. 191
      project/paint_robot_new/paint_robot_new.c

46
controller/include/msp_MK32.h
View File

@ -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 *
*----------------------------------------------*/

88
controller/msp_MK32.c
View File

@ -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

1
library/common/common_cfg.h.in
View File

@ -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

191
project/paint_robot_new/paint_robot_new.c
View File

@ -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

Write Preview
Loading…
Cancel
Save