diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c
index fda2a6e..c5eef36 100644
--- a/project/paint_robot_new/paint_robot_new.c
+++ b/project/paint_robot_new/paint_robot_new.c
@@ -50,6 +50,12 @@ 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_iLeft_Compensation = 0;
+static int g_iRight_Compensation = 0;
+static int g_bIsStopOffPaint = 0;
+static int g_iVehicleSpeed = 1;
+static int g_iPaint = 1;
+static int g_iS2LastValue = 0;
 /*----------------------------------------------*
  * 常量定义                                     *
  *----------------------------------------------*/
@@ -66,7 +72,7 @@ static int32_t RunTime_DistanceCm_SpeedE_2MPMin(void)
 }
 
 // 竖直从左往右作业 上端 向右换道 最终头朝上
-static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(int _iRight_Compensation)
+static void Vertical_Lane_Change_From_Left_To_Right_UP_Control()
 {
 	int iTargetAngle = 0;
 	if (0 == g_ichangLineState)
@@ -78,7 +84,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(int _iRight_Compe
 	{
 		BHBF_straight_drive_Cmd stCmd = {
 			.m_iMode = -1,
-			.m_iAngle = g_stCV.RobotLeftAngleValue - _iRight_Compensation,
+			.m_iAngle = g_stCV.RobotLeftAngleValue - g_iRight_Compensation,
 			.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
 			.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
 			};
@@ -92,7 +98,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_UP_Control(int _iRight_Compe
 }
 
 // 竖直从左往右作业 下端 向右换道 最终头朝上
-static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(int _iRight_Compensation)
+static void Vertical_Lane_Change_From_Left_To_Right_Down_Control()
 {
 	int iTargetAngle = 0;
 	if (0 == g_ichangLineState)
@@ -104,7 +110,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(int _iRight_Com
 	{
 		BHBF_straight_drive_Cmd stCmd = {
 			.m_iMode = 1,
-			.m_iAngle = g_stCV.RobotRightAngleValue - _iRight_Compensation,
+			.m_iAngle = g_stCV.RobotRightAngleValue - g_iRight_Compensation,
 			.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
 			.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
 			};
@@ -118,7 +124,7 @@ static void Vertical_Lane_Change_From_Left_To_Right_Down_Control(int _iRight_Com
 }
 
 // 竖直从右往左作业 上端 向左换道 最终头朝上
-static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(int _iLeft_Compensation)
+static void Vertical_Lane_Change_From_Right_To_Left_UP_Control()
 {
 	int iTargetAngle = 0;
 	if (0 == g_ichangLineState)
@@ -130,7 +136,7 @@ static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(int _iLeft_Compen
 	{
 		BHBF_straight_drive_Cmd stCmd = {
 			.m_iMode = -1,
-			.m_iAngle = g_stCV.RobotRightAngleValue + _iLeft_Compensation,
+			.m_iAngle = g_stCV.RobotRightAngleValue + g_iLeft_Compensation,
 			.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
 			.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
 			};
@@ -144,7 +150,7 @@ static void Vertical_Lane_Change_From_Right_To_Left_UP_Control(int _iLeft_Compen
 }
 
 // 竖直从右往左作业 下端 向左换道 最终头朝上
-static void Vertical_Lane_Change_From_Right_To_Left_Down_Control(int _iLeft_Compensation)
+static void Vertical_Lane_Change_From_Right_To_Left_Down_Control()
 {
 	int iTargetAngle = 0;
 	if (0 == g_ichangLineState)
@@ -156,7 +162,7 @@ static void Vertical_Lane_Change_From_Right_To_Left_Down_Control(int _iLeft_Comp
 	{
 		BHBF_straight_drive_Cmd stCmd = {
 			.m_iMode = 1,
-			.m_iAngle = g_stCV.RobotLeftAngleValue + _iLeft_Compensation,
+			.m_iAngle = g_stCV.RobotLeftAngleValue + g_iLeft_Compensation,
 			.m_iTime = RunTime_DistanceCm_SpeedE_2MPMin(),
 			.m_iSpeed = g_stCV.Lane_Change_Speed_m_per_min
 			};
@@ -213,6 +219,236 @@ static void Move_Halt_AngleError(void)
 	}
 }
 
+void MK32_Key_Func(SP_MSP_MK32_Button *_pstMK32, int _iMode)
+{
+	// 急停
+	if (_pstMK32->CH8_SE == -1000 && _pstMK32->CH9_SF == -1000)
+	{
+		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;
+		return;
+	}
+
+	// 根据模式判断换道（仅在竖直向左或向右生效）
+	if (_pstMK32->CH4_SA == 1000)
+	{
+		if (_iMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
+		{
+			Vertical_Lane_Change_From_Right_To_Left_Down_Control(g_iLeft_Compensation);
+		}
+		else if (_iMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
+		{
+			Vertical_Lane_Change_From_Left_To_Right_Down_Control(g_iRight_Compensation);
+		}
+		return;
+	}
+	else if (_pstMK32->CH4_SA == -1000)
+	{
+		if (_iMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
+		{
+			Vertical_Lane_Change_From_Right_To_Left_UP_Control(g_iLeft_Compensation);
+		}
+		else if (_iMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
+		{
+			Vertical_Lane_Change_From_Left_To_Right_UP_Control(g_iRight_Compensation);
+		}
+		return;
+	}
+	else // 重置前进计时器以便下一次换道重新计数
+	{
+		if (_iMode == 2 || _iMode == 3)
+		{
+			g_ichangLineState = 0;
+			MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
+		}
+	}
+
+	if (0 == g_stAngleError_Ctl.m_iAnglelock)
+	{
+		// 开关喷枪
+		if (_iMode == 1)
+		{
+			if (_pstMK32->CH6_SC == -1000)
+			{
+				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));
+				}
+			}
+			else
+			{
+				g_bIsStopOffPaint = 0;
+				g_stAngleError_Ctl.m_iPaintState = 1;
+				MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
+			}
+		}
+		else if(_iMode == 2 || _iMode == 3)
+		{
+			if (_pstMK32->CH13_S2 != g_iS2LastValue)
+			{
+				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));
+				}
+				else
+				{
+					//log_w("cannot open paint with 0 move speed !");
+				}
+			}
+		}
+		g_iS2LastValue = _pstMK32->CH13_S2;
+	}
+
+	int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
+	// 自动巡航
+	if (_pstMK32->CH5_SB == -1000)
+	{
+		if (_iMode == 1)
+		{
+			if (g_iVehicleSpeed >= 0)
+			{
+				aiMotorSpeed[0] = g_iVehicleSpeed * 10;
+				aiMotorSpeed[1] = g_iVehicleSpeed * 10;
+				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+			}
+		}
+		else if (_iMode == 2 || _iMode == 3)
+		{
+			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));
+			}
+		}
+		return;
+	}
+	else if(_pstMK32->CH5_SB == 1000)
+	{
+		if (_iMode == 1)
+		{
+			if (g_iVehicleSpeed >= 0)
+			{
+				aiMotorSpeed[0] = -g_iVehicleSpeed * 10;
+				aiMotorSpeed[1] = -g_iVehicleSpeed * 10;
+				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+			}
+		}
+		else if (_iMode == 2 || _iMode == 3)
+		{
+			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));
+			}
+		}
+		return;
+	}
+
+	// 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去
+	if (abs(_pstMK32->CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(_pstMK32->CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance)
+	{
+		if ((_iMode == 2 || _iMode == 3) && 0 == g_iPaint)
+		{
+			MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP_AFTER, NULL, 0);
+		}
+		else
+		{
+			MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
+		}
+		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)
+	{
+		if (_iMode == 1)
+		{
+			if (g_iVehicleSpeed >= 0)
+			{
+				aiMotorSpeed[0] = g_iVehicleSpeed * 10;
+				aiMotorSpeed[1] = g_iVehicleSpeed * 10;
+				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+			}
+		}
+		else if (_iMode == 2 || _iMode == 3)
+		{
+			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));
+			}
+		}
+	}
+	else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
+	{
+		if (_iMode == 1)
+		{
+			if (g_iVehicleSpeed >= 0)
+			{
+				aiMotorSpeed[0] = -g_iVehicleSpeed * 10;
+				aiMotorSpeed[1] = -g_iVehicleSpeed * 10;
+				MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+			}
+		}
+		else if (_iMode == 2 || _iMode == 3)
+		{
+			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));
+			}
+		}
+	}
+	else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)
+	{
+		if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
+		{
+			aiMotorSpeed[0] = g_stCV.LeftTurnSpeed;
+			aiMotorSpeed[1] = -g_stCV.RightTurnSpeed;
+			MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+		}
+	}
+	else if (abs(angle - 180) <= g_stCV.Joy_Sticker_Angle_Allowance || abs(angle - (-180)) <= g_stCV.Joy_Sticker_Angle_Allowance)
+	{
+		if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
+		{
+			aiMotorSpeed[0] = -g_stCV.LeftTurnSpeed;
+			aiMotorSpeed[1] = g_stCV.RightTurnSpeed;
+			MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
+		}
+	}
+	else
+	{
+		MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
+	}
+}
+
 static void Custom_ModuleHandler(const Msg_t *pstMsg)
 {
     if (NULL == pstMsg)
@@ -220,23 +456,17 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
         return;
     }
 
-    static int iLeft_Compensation = 0;
-    static int iRight_Compensation = 0;
-	static int iVehicleSpeed = 1;
-	static int iIsStopOffPaint = 0;
-	static int iPaint = 1;
 	static int iMotorOK = 0;
-	static int CH13_S2_Value = 0;
     switch (pstMsg->m_uiMsgID)
     {
 		case CUSTOM_RESET_PAINT:
 		{
-			iPaint = 1;
+			g_iPaint = 1;
         	break;
         }
 		case CUSTOM_SET_STOP_OFF_PAINT:
 		{
-			iIsStopOffPaint = 1;
+			g_bIsStopOffPaint = 1;
 		}
     	case CUSTOM_GET_DAEMON_CODE:
 		{
@@ -328,7 +558,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 				RD_MEMCPY(&paintstate, pstMsg->m_aucData, sizeof(paintstate));
 				HAL_GPIO_WritePin(OUT_1_GPIO_Port, OUT_1_Pin, paintstate);
 			}
-			if (0 == paintstate) iPaint = 0;
+			if (0 == paintstate) g_iPaint = 0;
 			break;
 		}
 		case CUSTOM_GET_MK32:
@@ -343,7 +573,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 				if (g_stMK32.CH4_SA == 0 && g_stMK32.CH5_SB == 0 && g_stMK32.CH6_SC == 0
 						&& g_stMK32.CH7_SD == 0 && g_stMK32.IsOnline == 1)
 				{
-					CH13_S2_Value = g_stMK32.CH13_S2; //防止第一次值不正确
+					g_iS2LastValue = g_stMK32.CH13_S2; //防止第一次值不正确
 					MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_POWER_ENABLE, NULL, 0);
 					g_Is_All_Button_Reset = 1;
 				}
@@ -358,9 +588,8 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 				break;
 			}
 
-			// 急停或者遥控器失联
-			if ((g_stMK32.CH8_SE == -1000 && g_stMK32.CH9_SF == -1000)
-			 || g_stMK32.IsOnline == 0 || g_stIV.Left_Motor_Err != 0 || g_stIV.Right_Motor_Err != 0)
+			// 遥控器失联或者电机报错
+			if (g_stMK32.IsOnline == 0 || g_stIV.Left_Motor_Err != 0 || g_stIV.Right_Motor_Err != 0)
 			{
 				if (g_stIV.Left_Motor_Err != 0 || g_stIV.Right_Motor_Err != 0)
 				{
@@ -392,230 +621,14 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 			int iSpeedSelection = 3 * (g_stMK32.CH11_RD1 + 1000) / 200;
 			if (iSpeedSelection > 0)
 			{
-				iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30;
+				g_iVehicleSpeed = g_stCV.Speed_m_per_min*iSpeedSelection/30;
 			}
-			g_stIV.RobotMoveSpeed = iVehicleSpeed;
-			MsgCenter_SendTo(MODULE_NAME_MOTOR, GET_VEHICLE_SPEED, (void *)&iVehicleSpeed, sizeof(int));
+			g_stIV.RobotMoveSpeed = g_iVehicleSpeed;
+			MsgCenter_SendTo(MODULE_NAME_MOTOR, GET_VEHICLE_SPEED, (void *)&g_iVehicleSpeed, sizeof(int));
 
 			Move_Halt_AngleError();
 
-			// 根据模式判断换道（仅在竖直向左或向右生效）
-			if (g_stMK32.CH4_SA == 1000)
-			{
-				if (g_stPV.RunMode == 2)//竖直从右往左作业 下端 向左换道 最终头朝上
-				{
-					Vertical_Lane_Change_From_Right_To_Left_Down_Control(iLeft_Compensation);
-				}
-				else if (g_stPV.RunMode == 3)//竖直从左往右作业 下端 向右换道 最终头朝上
-				{
-					Vertical_Lane_Change_From_Left_To_Right_Down_Control(iRight_Compensation);
-				}
-				break;
-			}
-			else if (g_stMK32.CH4_SA == -1000)
-			{
-				if (g_stPV.RunMode == 2)//竖直从右往左作业 上端 向左换道 最终头朝上
-				{
-					Vertical_Lane_Change_From_Right_To_Left_UP_Control(iLeft_Compensation);
-				}
-				else if (g_stPV.RunMode == 3)//竖直从左往右作业 上端 向右换道 最终头朝上
-				{
-					Vertical_Lane_Change_From_Left_To_Right_UP_Control(iRight_Compensation);
-				}
-				break;
-			}
-			else // 重置前进计时器以便下一次换道重新计数
-			{
-				if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
-				{
-					g_ichangLineState = 0;
-					MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_RESET_STRAIGHT, NULL, 0);
-				}
-			}
-
-			if (0 == g_stAngleError_Ctl.m_iAnglelock)
-			{
-				// 开关喷枪
-				if (g_stPV.RunMode == 1)
-				{
-					if (g_stMK32.CH6_SC == -1000)
-					{
-						if (0 == iIsStopOffPaint)
-						{
-							g_stAngleError_Ctl.m_iPaintState = 0;
-							MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
-						}
-					}
-					else
-					{
-						iIsStopOffPaint = 0;
-						g_stAngleError_Ctl.m_iPaintState = 1;
-						MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_PAINTGUN, &g_stAngleError_Ctl.m_iPaintState, sizeof(int));
-					}
-				}
-				else if(g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
-				{
-					if (g_stMK32.CH13_S2 != CH13_S2_Value)
-					{
-						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));
-						}
-						else
-						{
-							//log_w("cannot open paint with 0 move speed !");
-						}
-					}
-				}
-				CH13_S2_Value = g_stMK32.CH13_S2;
-			}
-
-			int aiMotorSpeed[2] = {0}; //0索引表示左，1索引表示右，多轮时到对应motor的MOTOR_CMD_SET_SPEED命令自行处理
-			// 自动巡航
-			if (g_stMK32.CH5_SB == -1000)
-			{
-				if (g_stPV.RunMode == 1)
-				{
-					if (iVehicleSpeed >= 0)
-					{
-						aiMotorSpeed[0] = iVehicleSpeed * 10;
-						aiMotorSpeed[1] = iVehicleSpeed * 10;
-						MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
-					}
-				}
-				else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
-				{
-					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 = iVehicleSpeed
-							};
-						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));						
-					}
-				}
-				break;
-			}
-			else if(g_stMK32.CH5_SB == 1000)
-			{
-				if (g_stPV.RunMode == 1)
-				{
-					if (iVehicleSpeed >= 0)
-					{
-						aiMotorSpeed[0] = -iVehicleSpeed * 10;
-						aiMotorSpeed[1] = -iVehicleSpeed * 10;
-						MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
-					}
-				}
-				else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
-				{
-					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 = iVehicleSpeed
-							};
-						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
-					}
-				}
-				break;
-			}
-
-			// 【注意!!!】摇杆行程死区判断开始，除摇杆以外按键在上边都处理完，不然走不下去
-			if (abs(g_stMK32.CH2_LY_V) <= g_stCV.Joy_Sticker_Value_Allowance && abs(g_stMK32.CH3_LY_H) <= g_stCV.Joy_Sticker_Value_Allowance)
-			{
-				if ((g_stPV.RunMode == 2 || g_stPV.RunMode == 3) && 0 == iPaint)
-				{
-					MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_CMD_STOP_AFTER, NULL, 0);
-				}
-				else
-				{
-					MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
-				}
-				break;
-			}
-			// 【注意!!!】摇杆行程死区判断结束，摇杆处理都放在这以后，否则行程死区可能不生效
-
-			// 摇杆角度死区判断
-			int angle = atan2(g_stMK32.CH2_LY_V, g_stMK32.CH3_LY_H) * 180 / M_PI;
-
-			if (abs(angle - 90) <= g_stCV.Joy_Sticker_Angle_Allowance)
-			{
-				if (g_stPV.RunMode == 1)
-				{
-					if (iVehicleSpeed >= 0)
-					{
-						aiMotorSpeed[0] = iVehicleSpeed * 10;
-						aiMotorSpeed[1] = iVehicleSpeed * 10;
-						MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
-					}
-				}	
-				else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
-				{
-					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 = iVehicleSpeed
-							};
-						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
-					}
-				}
-			}
-			else if (abs(angle - (-90)) <= g_stCV.Joy_Sticker_Angle_Allowance)
-			{
-				if (g_stPV.RunMode == 1)
-				{
-					if (iVehicleSpeed >= 0)
-					{
-						aiMotorSpeed[0] = -iVehicleSpeed * 10;
-						aiMotorSpeed[1] = -iVehicleSpeed * 10;
-						MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
-					}				
-				}
-				else if (g_stPV.RunMode == 2 || g_stPV.RunMode == 3)
-				{
-					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 = iVehicleSpeed
-							};
-						MsgCenter_SendTo(MODULE_NAME_TIMER, TIMER_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd));
-					}
-				}
-			}
-			else if (abs(angle - 0) <= g_stCV.Joy_Sticker_Angle_Allowance)
-			{
-				if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
-				{
-					aiMotorSpeed[0] = g_stCV.LeftTurnSpeed;
-					aiMotorSpeed[1] = -g_stCV.RightTurnSpeed;
-					MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
-				}
-			}
-			else if (abs(angle - 180) <= g_stCV.Joy_Sticker_Angle_Allowance || abs(angle - (-180)) <= g_stCV.Joy_Sticker_Angle_Allowance)
-			{
-				if (g_stCV.LeftTurnSpeed > 0 && g_stCV.RightTurnSpeed > 0)
-				{
-					aiMotorSpeed[0] = -g_stCV.LeftTurnSpeed;
-					aiMotorSpeed[1] = g_stCV.RightTurnSpeed;
-					MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed));
-				}			
-			}
-			else
-			{
-				MsgCenter_SendTo(MODULE_NAME_MOTOR, CMD_STOP_ALL, NULL, 0);
-			}
+			MK32_Key_Func(&g_stMK32, g_stPV.RunMode);
 			break;
 		}
 		case CMD_SHOW_INFO:
@@ -641,7 +654,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 			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",
-					iPaint, g_stAngleError_Ctl.m_iPaintState, g_stAngleError_Ctl.m_iAnglelock, g_stAngleError_Ctl.m_ipaintOffCount, g_Is_All_Button_Reset);
+					g_iPaint, g_stAngleError_Ctl.m_iPaintState, g_stAngleError_Ctl.m_iAnglelock, g_stAngleError_Ctl.m_ipaintOffCount, g_Is_All_Button_Reset);
 			break;
 		}
         default: