【三维数组实践！！！】初版

controller/include/msp_MK32.h

@@ -0,0 +1,129 @@
+/******************************************************************************
+
+                  版权所有 (C), 2018-2099, Radkil
+
+ ******************************************************************************
+  文 件 名   : msp_MK32.h
+  版 本 号   : 初稿
+  作    者   : radkil
+  生成日期   : 2026年9月22日
+  最近修改   :
+  功能描述   : msp_MK32.c 的头文件
+  
+  修改历史   :
+  1.日    期   : 2026年9月22日
+    作    者   : radkil
+    修改内容   : 创建文件
+
+******************************************************************************/
+
+/*----------------------------------------------*
+ * 外部变量说明                                 *
+ *----------------------------------------------*/
+
+/*----------------------------------------------*
+ * 外部函数原型说明                             *
+ *----------------------------------------------*/
+
+/*----------------------------------------------*
+ * 内部函数原型说明                             *
+ *----------------------------------------------*/
+
+/*----------------------------------------------*
+ * 全局变量                                     *
+ *----------------------------------------------*/
+
+/*----------------------------------------------*
+ * 模块级变量                                   *
+ *----------------------------------------------*/
+
+/*----------------------------------------------*
+ * 常量定义                                     *
+ *----------------------------------------------*/
+
+/*----------------------------------------------*
+ * 宏定义                                       *
+ *----------------------------------------------*/
+
+#ifndef __MSP_MK32_H__
+#define __MSP_MK32_H__
+
+
+#ifdef __cplusplus
+#if __cplusplus
+extern "C"{
+#endif
+#endif /* __cplusplus */
+
+
+/*==============================================*
+ *      include header files                    *
+ *----------------------------------------------*/
+
+
+
+
+/*==============================================*
+ *      constants or macros define              *
+ *----------------------------------------------*/
+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_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);
+
+/*==============================================*
+ *      project-wide global variables           *
+ *----------------------------------------------*/
+
+
+
+/*==============================================*
+ *      routines' or functions' implementations *
+ *----------------------------------------------*/
+
+#ifdef __cplusplus
+#if __cplusplus
+}
+#endif
+#endif /* __cplusplus */
+
+
+#endif /* __MSP_MK32_H__ */

controller/msp_MK32.c

@@ -20,6 +20,7 @@
 #include "msg_center.h"
 #include "msp_MK32.pb.h"
 #include "rd_time.h"
+#include "msp_MK32.h"
 
 /*----------------------------------------------*
  * 外部变量说明                                 *
@@ -143,3 +144,101 @@ 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);
+		return;
+	}
+
+	// 根据模式判断换道（仅在竖直向左或向右生效）
+	if (_pstMK32->CH4_SA == 1000)
+	{
+		SAFE_DISPATCH(KEY_SA_UP, _iMode, _iSubMode);
+		return;
+	}
+	else if (_pstMK32->CH4_SA == -1000)
+	{
+		SAFE_DISPATCH(KEY_SA_DOWN, _iMode, _iSubMode);
+		return;
+	}
+	else // 重置前进计时器以便下一次换道重新计数
+	{
+		SAFE_DISPATCH(KEY_SA_DEFAULT, _iMode, _iSubMode);
+	}
+
+	// 自动巡航
+	if (_pstMK32->CH5_SB == -1000)
+	{
+		SAFE_DISPATCH(KEY_SB_UP, _iMode, _iSubMode);
+		return;
+	}
+	else if(_pstMK32->CH5_SB == 1000)
+	{
+		SAFE_DISPATCH(KEY_SB_DOWN, _iMode, _iSubMode);
+		return;
+	}
+
+
+	// 开关喷枪
+	if (_pstMK32->CH6_SC == -1000)
+	{
+		SAFE_DISPATCH(KEY_SC_UP, _iMode, _iSubMode);
+	}
+	else if (_pstMK32->CH6_SC == 1000)
+	{
+		SAFE_DISPATCH(KEY_SC_DOWN, _iMode, _iSubMode);
+	}
+	else
+	{
+		SAFE_DISPATCH(KEY_SC_DEFAULT, _iMode, _iSubMode);
+	}
+
+	// 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去
+	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);
+		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, _iMode, _iSubMode);
+	}
+	else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
+	{
+		SAFE_DISPATCH(JOY_LEFT_DWON, _iMode, _iSubMode);
+	}
+	else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)
+	{
+		SAFE_DISPATCH(JOY_LEFT_RIGHT, _iMode, _iSubMode);
+	}
+	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);
+	}
+	else
+	{
+		MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
+	}
+}

project/paint_robot_new/CMakeLists.txt

@@ -20,3 +20,9 @@ if(TARGET motor)
 else()
     message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'motor' not found.")
 endif()
+
+if(TARGET controller)
+    target_link_libraries(${TARGET_NAME} PUBLIC controller)
+else()
+    message(FATAL_ERROR "[${TARGET_NAME}] Dependency 'controller' not found.")
+endif()

project/paint_robot_new/paint_robot_new.c

@@ -23,6 +23,7 @@
 #include "Protobuf/PSource/msp_MK32.pb.h"
 #include "Protobuf/PSource/bsp_CV.pb.h"
 #include "Protobuf/PSource/bsp_IV.pb.h"
+#include "msp_MK32.h"
 
 /*----------------------------------------------*
  * 外部变量说明                                 *
@@ -64,15 +65,31 @@ static int g_iS2LastValue = 0;
  * 宏定义                                       *
  *----------------------------------------------*/
 
-#define IV_SEND_TIME 50 // IV上报周期（单位毫秒）
+static struct {
+    int m_iPaintState;       // 喷枪状态：0=打开，1=关闭
+    int m_iAnglelock;        // 1=角度异常，触发 Move_Halt_AngleError 并屏蔽互斥代码
+    int m_ipaintOffCount;    // Move_Halt_AngleError 中的计时，当前为 1 秒
+} g_stAngleError_Ctl = {
+    .m_iPaintState = -1
+};
 
 static int32_t RunTime_DistanceCm_SpeedE_2MPMin(void)
 {
 	return (600 * (g_stPV.LaneChangeDistance + g_stCV.Vertical_ChangeLane_Compensation) / g_stCV.Lane_Change_Speed_m_per_min);
 }
 
+/*----------------------------------------------*
+ * 二维数组映射表！！！                          *
+ *----------------------------------------------*/
+static void E_Robot_Stop(void)
+{
+	MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_DISABLE, NULL, 0);
+	MsgCenter_SendTo(MODULE_NAME_RBCORE, RBCORE_CMD_STOP_ALL, NULL, 0);
+	g_Is_All_Button_Reset = 0;
+}
+
 // 竖直从左往右作业 上端 向右换道 最终头朝上
-static void Vertical_Lane_Change_From_Left_To_Right_UP_Control()
+static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(void)
 {
 	int iTargetAngle = 0;
 	if (0 == g_ichangLineState)
@@ -98,7 +115,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_UP_Control()
 }
 
 // 竖直从左往右作业 下端 向右换道 最终头朝上
-static void Vertical_Lane_Change_From_Left_To_Right_Down_Control()
+static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(void)
 {
 	int iTargetAngle = 0;
 	if (0 == g_ichangLineState)
@@ -124,7 +141,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_Down_Control()
 }
 
 // 竖直从右往左作业 上端 向左换道 最终头朝上
-static void Vertical_Lane_Change_From_Right_To_Left_UP_Control()
+static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(void)
 {
 	int iTargetAngle = 0;
 	if (0 == g_ichangLineState)
@@ -150,7 +167,7 @@ static void Vertical_Lane_Change_From_Right_To_Left_UP_Control()
 }
 
 // 竖直从右往左作业 下端 向左换道 最终头朝上
-static void Vertical_Lane_Change_From_Right_To_Left_Down_Control()
+static void Vertical_Lane_Change_From_Right_To_Left_Down_Control(void)
 {
 	int iTargetAngle = 0;
 	if (0 == g_ichangLineState)
@@ -175,13 +192,268 @@ static void Vertical_Lane_Change_From_Right_To_Left_Down_Control()
 	}
 }
 
-static struct {
-    int m_iPaintState;       // 喷枪状态：0=打开，1=关闭
-    int m_iAnglelock;        // 1=角度异常，触发 Move_Halt_AngleError 并屏蔽互斥代码
-    int m_ipaintOffCount;    // Move_Halt_AngleError 中的计时，当前为 1 秒
-} g_stAngleError_Ctl = {
-    .m_iPaintState = -1
+static void Lane_Change_State_Reset(void)
+{
+	g_ichangLineState = 0;
+	MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
+}
+
+static void Robot_Move_Forward(void)
+{
+	if (g_iVehicleSpeed >= 0)
+	{
+		int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
+		aiMotorSpeed[0] = g_iVehicleSpeed * 10;
+		aiMotorSpeed[1] = g_iVehicleSpeed * 10;
+		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+	}
+}
+
+static void Robot_Paint_SP(void)
+{
+	if (g_stMK32.CH13_S2 != g_iS2LastValue)
+	{
+		if (0 == g_stAngleError_Ctl.m_iAnglelock)
+		{
+			if(g_stIV.CurrentSpeed != 0)
+			{
+				g_stAngleError_Ctl.m_iPaintState = 0;
+				MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
+			}
+		}
+	}
+	g_iS2LastValue = g_stMK32.CH13_S2;
+}
+
+static void Robot_Move_Forward_PID(void)
+{
+	Robot_Paint_SP();
+	if (0 == g_stAngleError_Ctl.m_iAnglelock)
+	{
+		BHBF_straight_drive_Cmd stCmd = {
+			.m_iMode = 1,
+			.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
+			.m_iTime = -1,
+			.m_iSpeed = g_iVehicleSpeed
 			};
+		MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
+	}
+}
+
+static void Robot_Move_Backward(void)
+{
+	if (g_iVehicleSpeed >= 0)
+	{
+		int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
+		aiMotorSpeed[0] = -g_iVehicleSpeed * 10;
+		aiMotorSpeed[1] = -g_iVehicleSpeed * 10;
+		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+	}
+}
+
+static void Robot_Move_Backward_PID(void)
+{
+	Robot_Paint_SP();
+	if (0 == g_stAngleError_Ctl.m_iAnglelock)
+	{
+		BHBF_straight_drive_Cmd stCmd = {
+			.m_iMode = -1,
+			.m_iAngle = g_stCV.RobotUpAngleValue + g_stPV.Vertical_Calibration,
+			.m_iTime = -1,
+			.m_iSpeed = g_iVehicleSpeed
+			};
+		MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
+	}
+}
+
+static void Robot_Paint_ON(void)
+{
+	if (0 == g_stAngleError_Ctl.m_iAnglelock)
+	{
+		if (0 == g_bIsStopOffPaint)
+		{
+			g_stAngleError_Ctl.m_iPaintState = 0;
+			MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
+		}
+	}
+}
+
+static void Robot_Paint_OFF(void)
+{
+	if (0 == g_stAngleError_Ctl.m_iAnglelock)
+	{
+		g_bIsStopOffPaint = 0;
+		g_stAngleError_Ctl.m_iPaintState = 1;
+		MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
+	}
+}
+
+static void Robot_Stop_After(void)
+{
+	if (0 == g_iPaint)
+	{
+		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP_AFTER, NULL, 0);
+	}
+}
+
+static void Robot_Stop(void)
+{
+	MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
+}
+
+static void Robot_Move_Left(void)
+{
+	if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
+	{
+		int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
+		aiMotorSpeed[0] = -g_stCV.LeftTurnSpeed;
+		aiMotorSpeed[1] = g_stCV.RightTurnSpeed;
+		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+	}
+}
+
+static void Robot_Move_Right(void)
+{
+	if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
+	{
+		int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
+		aiMotorSpeed[0] = g_stCV.LeftTurnSpeed;
+		aiMotorSpeed[1] = -g_stCV.RightTurnSpeed;
+		MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+	}
+}
+
+const ActionFunc g_ActionTable[KEY_MAX][MODE_MAX][SUB_MODE_MAX] = {
+	[E_STOP] = {
+		[MODE_DEFAULT] = {
+			[SUB_MODE_NULL] = E_Robot_Stop,
+		},
+		[MODE_VERTICAL_LEFT] = {
+			[SUB_MODE_NULL] = E_Robot_Stop,
+		},
+		[MODE_VERTICAL_RIGHT] = {
+			[SUB_MODE_NULL] = E_Robot_Stop,
+		},
+	},
+    [KEY_SA_UP] = {
+		[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,
+		},
+    },
+	[KEY_SA_DOWN] = {
+		[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,
+		},
+    },
+	[KEY_SA_DEFAULT] = {
+		[MODE_VERTICAL_LEFT] = {
+			[SUB_MODE_NULL] = Lane_Change_State_Reset,
+		},
+		[MODE_VERTICAL_RIGHT] = {
+			[SUB_MODE_NULL] = Lane_Change_State_Reset,
+		},
+    },
+	[KEY_SB_UP] = {
+		[MODE_DEFAULT] = {
+			[SUB_MODE_NULL] = Robot_Move_Forward,
+		},
+		[MODE_VERTICAL_LEFT] = {
+			[SUB_MODE_NULL] = Robot_Move_Forward_PID,
+		},
+		[MODE_VERTICAL_RIGHT] = {
+			[SUB_MODE_NULL] = Robot_Move_Forward_PID,
+		}
+    },
+	[KEY_SB_DOWN] = {
+		[MODE_DEFAULT] = {
+			[SUB_MODE_NULL] = Robot_Move_Backward,
+		},
+		[MODE_VERTICAL_LEFT] = {
+			[SUB_MODE_NULL] = Robot_Move_Backward_PID,
+		},
+		[MODE_VERTICAL_RIGHT] = {
+			[SUB_MODE_NULL] = Robot_Move_Backward_PID,
+		}
+    },
+	[KEY_SC_DOWN] = {
+		[MODE_DEFAULT] = {
+			[SUB_MODE_NULL] = Robot_Paint_OFF,
+		}
+    },
+	[KEY_SC_DEFAULT] = {
+		[MODE_DEFAULT] = {
+			[SUB_MODE_NULL] = Robot_Paint_OFF,
+		}
+    },
+	[KEY_SC_UP] = {
+		[MODE_DEFAULT] = {
+			[SUB_MODE_NULL] = Robot_Paint_ON,
+		}
+    },
+	[JOY_DEFAULT] = {
+		[MODE_DEFAULT] = {
+			[SUB_MODE_NULL] = Robot_Stop,
+		},
+		[MODE_VERTICAL_LEFT] = {
+			[SUB_MODE_NULL] = Robot_Stop_After,
+		},
+		[MODE_VERTICAL_RIGHT] = {
+			[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,
+		},
+		[MODE_VERTICAL_RIGHT] = {
+			[SUB_MODE_NULL] = Robot_Move_Forward_PID,
+		},
+    },
+	[JOY_LEFT_DWON] = {
+		[MODE_DEFAULT] = {
+			[SUB_MODE_NULL] = Robot_Move_Backward,
+		},
+		[MODE_VERTICAL_LEFT] = {
+			[SUB_MODE_NULL] = Robot_Move_Backward_PID,
+		},
+		[MODE_VERTICAL_RIGHT] = {
+			[SUB_MODE_NULL] = Robot_Move_Backward_PID,
+		},
+    },
+	[JOY_LEFT_LEFT] = {
+		[MODE_DEFAULT] = {
+			[SUB_MODE_NULL] = Robot_Move_Left,
+		},
+		[MODE_VERTICAL_LEFT] = {
+			[SUB_MODE_NULL] = Robot_Move_Left,
+		},
+		[MODE_VERTICAL_RIGHT] = {
+			[SUB_MODE_NULL] = Robot_Move_Left,
+		},
+    },
+	[JOY_LEFT_RIGHT] = {
+		[MODE_DEFAULT] = {
+			[SUB_MODE_NULL] = Robot_Move_Right,
+		},
+		[MODE_VERTICAL_LEFT] = {
+			[SUB_MODE_NULL] = Robot_Move_Right,
+		},
+		[MODE_VERTICAL_RIGHT] = {
+			[SUB_MODE_NULL] = Robot_Move_Right,
+		},
+    }
+};
+
+#define IV_SEND_TIME 50 // IV上报周期（单位毫秒）
 
 static void Move_Halt_AngleError(void)
 {
@@ -219,7 +491,7 @@ static void Move_Halt_AngleError(void)
 	}
 }
 
-void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode)
+void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode, int _iSubMode)
 {
 	// 急停
 	if (_pstMK32->CH8_SE == -1000 && _pstMK32->CH9_SF == -1000)
@@ -235,11 +507,11 @@ void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode)
 	{
 		if (_iMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
 		{
-			Vertical_Lane_Change_From_Right_To_Left_Down_Control(g_iLeft_Compensation);
+			Vertical_Lane_Change_From_Right_To_Left_Down_Control();
 		}
 		else if (_iMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
 		{
-			Vertical_Lane_Change_From_Left_To_Right_Down_Control(g_iRight_Compensation);
+			Vertical_Lane_Change_From_Left_To_Right_Down_Control();
 		}
 		return;
 	}
@@ -247,11 +519,11 @@ void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode)
 	{
 		if (_iMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
 		{
-			Vertical_Lane_Change_From_Right_To_Left_UP_Control(g_iLeft_Compensation);
+			Vertical_Lane_Change_From_Right_To_Left_UP_Control();
 		}
 		else if (_iMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
 		{
-			Vertical_Lane_Change_From_Left_To_Right_UP_Control(g_iRight_Compensation);
+			Vertical_Lane_Change_From_Left_To_Right_UP_Control();
 		}
 		return;
 	}
@@ -628,7 +900,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 
 			Move_Halt_AngleError();
 
-			MK32_Key_Func(&g_stMK32, g_stPV.RunMode);
+			MK32_Key_Func(&g_stMK32, g_stPV.RunMode, SUB_MODE_NULL);
 			break;
 		}
 		case CMD_SHOW_INFO: