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

【已调用】规整三维数组代码

paint_robot_new-v1.5
Lizongdi 1 week ago
parent
0d130c03af
commit
6cb97928a6
3 changed files with 33 additions and 28 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. 11
      controller/include/msp_MK32.h
  2. 40
      controller/msp_MK32.c
  3. 10
      project/paint_robot_new/paint_robot_new.c

11
controller/include/msp_MK32.h
View File

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

40
controller/msp_MK32.c
View File

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

10
project/paint_robot_new/paint_robot_new.c
View File

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

Write Preview
Loading…
Cancel
Save