diff --git a/controller/include/msp_MK32.h b/controller/include/msp_MK32.h
index 0e85084..2d6109c 100644
--- a/controller/include/msp_MK32.h
+++ b/controller/include/msp_MK32.h
@@ -76,6 +76,7 @@ typedef enum {
 
 typedef enum {
 	SUB_MODE_NULL = 0,
+	SUB_MODE_DEFAULT,
 	SUB_MODE_MAX
 } E_BHBF_RBSubMode;
 
@@ -160,21 +161,21 @@ static inline void SafeDispatch(const TKeyTree *table, int count, int mode, int
         }
     }
 
-	// for (int i = 0; i < count; i++) 
-	// {
-    //     if (table[i].m_iMode == mode && table[i].m_iSubMode == SUB_MODE_NULL) 
-	// 	{
-    //         if (table[i].m_ptFunc != NULL) 
-	// 		{
-    //             table[i].m_ptFunc();
-    //         }
-    //         return;
-    //     }
-    // }
+	for (int i = 0; i < count; i++)
+	{
+		if (table[i].m_iMode == mode && table[i].m_iSubMode == SUB_MODE_DEFAULT)
+		{
+			if (table[i].m_ptFunc != NULL)
+			{
+			 table[i].m_ptFunc();
+			}
+			return;
+		}
+	}
 
 	for (int i = 0; i < count; i++) 
 	{
-        if (table[i].m_iMode == MODE_DEFAULT && table[i].m_iSubMode == SUB_MODE_NULL) 
+        if (table[i].m_iMode == MODE_DEFAULT && table[i].m_iSubMode == SUB_MODE_DEFAULT) 
 		{
             if (table[i].m_ptFunc != NULL) 
 			{
diff --git a/project/paint_robot_new/paint_robot_new.c b/project/paint_robot_new/paint_robot_new.c
index 9096914..3d5b9b0 100644
--- a/project/paint_robot_new/paint_robot_new.c
+++ b/project/paint_robot_new/paint_robot_new.c
@@ -332,129 +332,129 @@ static void Robot_Move_Right(void)
 const ActionFunc g_ActionTable[KEY_MAX][MODE_MAX][SUB_MODE_MAX] = {
 	[E_STOP] = {
 		[MODE_DEFAULT] = {
-			[SUB_MODE_NULL] = E_Robot_Stop,
+			[SUB_MODE_DEFAULT] = E_Robot_Stop,
 		},
 		[MODE_VERTICAL_LEFT] = {
-			[SUB_MODE_NULL] = E_Robot_Stop,
+			[SUB_MODE_DEFAULT] = E_Robot_Stop,
 		},
 		[MODE_VERTICAL_RIGHT] = {
-			[SUB_MODE_NULL] = E_Robot_Stop,
+			[SUB_MODE_DEFAULT] = E_Robot_Stop,
 		},
 	},
     [KEY_SA_UP] = {
 		[MODE_VERTICAL_LEFT] = {
-			[SUB_MODE_NULL] = Vertical_Lane_Change_From_Right_To_Left_UP_Control,
+			[SUB_MODE_DEFAULT] = Vertical_Lane_Change_From_Right_To_Left_UP_Control,
 		},
 		[MODE_VERTICAL_RIGHT] = {
-			[SUB_MODE_NULL] = Vertical_Lane_Change_From_Left_To_Right_UP_Control,
+			[SUB_MODE_DEFAULT] = 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,
+			[SUB_MODE_DEFAULT] = Vertical_Lane_Change_From_Right_To_Left_Down_Control,
 		},
 		[MODE_VERTICAL_RIGHT] = {
-			[SUB_MODE_NULL] = Vertical_Lane_Change_From_Left_To_Right_Down_Control,
+			[SUB_MODE_DEFAULT] = Vertical_Lane_Change_From_Left_To_Right_Down_Control,
 		},
     },
 	[KEY_SA_DEFAULT] = {
 		[MODE_VERTICAL_LEFT] = {
-			[SUB_MODE_NULL] = Lane_Change_State_Reset,
+			[SUB_MODE_DEFAULT] = Lane_Change_State_Reset,
 		},
 		[MODE_VERTICAL_RIGHT] = {
-			[SUB_MODE_NULL] = Lane_Change_State_Reset,
+			[SUB_MODE_DEFAULT] = Lane_Change_State_Reset,
 		},
     },
 	[KEY_SB_UP] = {
 		[MODE_DEFAULT] = {
-			[SUB_MODE_NULL] = Robot_Move_Forward,
+			[SUB_MODE_DEFAULT] = Robot_Move_Forward,
 		},
 		[MODE_VERTICAL_LEFT] = {
-			[SUB_MODE_NULL] = Robot_Move_Forward_PID,
+			[SUB_MODE_DEFAULT] = Robot_Move_Forward_PID,
 		},
 		[MODE_VERTICAL_RIGHT] = {
-			[SUB_MODE_NULL] = Robot_Move_Forward_PID,
+			[SUB_MODE_DEFAULT] = Robot_Move_Forward_PID,
 		}
     },
 	[KEY_SB_DOWN] = {
 		[MODE_DEFAULT] = {
-			[SUB_MODE_NULL] = Robot_Move_Backward,
+			[SUB_MODE_DEFAULT] = Robot_Move_Backward,
 		},
 		[MODE_VERTICAL_LEFT] = {
-			[SUB_MODE_NULL] = Robot_Move_Backward_PID,
+			[SUB_MODE_DEFAULT] = Robot_Move_Backward_PID,
 		},
 		[MODE_VERTICAL_RIGHT] = {
-			[SUB_MODE_NULL] = Robot_Move_Backward_PID,
+			[SUB_MODE_DEFAULT] = Robot_Move_Backward_PID,
 		}
     },
 	[KEY_SC_DOWN] = {
 		[MODE_DEFAULT] = {
-			[SUB_MODE_NULL] = Robot_Paint_OFF,
+			[SUB_MODE_DEFAULT] = Robot_Paint_OFF,
 		}
     },
 	[KEY_SC_DEFAULT] = {
 		[MODE_DEFAULT] = {
-			[SUB_MODE_NULL] = Robot_Paint_OFF,
+			[SUB_MODE_DEFAULT] = Robot_Paint_OFF,
 		}
     },
 	[KEY_SC_UP] = {
 		[MODE_DEFAULT] = {
-			[SUB_MODE_NULL] = Robot_Paint_ON,
+			[SUB_MODE_DEFAULT] = Robot_Paint_ON,
 		}
     },
 	[JOY_DEFAULT] = {
 		[MODE_DEFAULT] = {
-			[SUB_MODE_NULL] = Robot_Stop,
+			[SUB_MODE_DEFAULT] = Robot_Stop,
 		},
 		[MODE_VERTICAL_LEFT] = {
-			[SUB_MODE_NULL] = Robot_Stop_After,
+			[SUB_MODE_DEFAULT] = Robot_Stop_After,
 		},
 		[MODE_VERTICAL_RIGHT] = {
-			[SUB_MODE_NULL] = Robot_Stop_After,
+			[SUB_MODE_DEFAULT] = Robot_Stop_After,
 		},
     },
 	[JOY_LEFT_UP] = {
 		[MODE_DEFAULT] = {
-			[SUB_MODE_NULL] = Robot_Move_Forward,
+			[SUB_MODE_DEFAULT] = Robot_Move_Forward,
 		},
 		[MODE_VERTICAL_LEFT] = {
-			[SUB_MODE_NULL] = Robot_Move_Forward_PID,
+			[SUB_MODE_DEFAULT] = Robot_Move_Forward_PID,
 		},
 		[MODE_VERTICAL_RIGHT] = {
-			[SUB_MODE_NULL] = Robot_Move_Forward_PID,
+			[SUB_MODE_DEFAULT] = Robot_Move_Forward_PID,
 		},
     },
 	[JOY_LEFT_DWON] = {
 		[MODE_DEFAULT] = {
-			[SUB_MODE_NULL] = Robot_Move_Backward,
+			[SUB_MODE_DEFAULT] = Robot_Move_Backward,
 		},
 		[MODE_VERTICAL_LEFT] = {
-			[SUB_MODE_NULL] = Robot_Move_Backward_PID,
+			[SUB_MODE_DEFAULT] = Robot_Move_Backward_PID,
 		},
 		[MODE_VERTICAL_RIGHT] = {
-			[SUB_MODE_NULL] = Robot_Move_Backward_PID,
+			[SUB_MODE_DEFAULT] = Robot_Move_Backward_PID,
 		},
     },
 	[JOY_LEFT_LEFT] = {
 		[MODE_DEFAULT] = {
-			[SUB_MODE_NULL] = Robot_Move_Left,
+			[SUB_MODE_DEFAULT] = Robot_Move_Left,
 		},
 		[MODE_VERTICAL_LEFT] = {
-			[SUB_MODE_NULL] = Robot_Move_Left,
+			[SUB_MODE_DEFAULT] = Robot_Move_Left,
 		},
 		[MODE_VERTICAL_RIGHT] = {
-			[SUB_MODE_NULL] = Robot_Move_Left,
+			[SUB_MODE_DEFAULT] = Robot_Move_Left,
 		},
     },
 	[JOY_LEFT_RIGHT] = {
 		[MODE_DEFAULT] = {
-			[SUB_MODE_NULL] = Robot_Move_Right,
+			[SUB_MODE_DEFAULT] = Robot_Move_Right,
 		},
 		[MODE_VERTICAL_LEFT] = {
-			[SUB_MODE_NULL] = Robot_Move_Right,
+			[SUB_MODE_DEFAULT] = Robot_Move_Right,
 		},
 		[MODE_VERTICAL_RIGHT] = {
-			[SUB_MODE_NULL] = Robot_Move_Right,
+			[SUB_MODE_DEFAULT] = Robot_Move_Right,
 		},
     }
 };
@@ -462,61 +462,61 @@ const ActionFunc g_ActionTable[KEY_MAX][MODE_MAX][SUB_MODE_MAX] = {
 
 const TKeyForest g_ActionTable = {
 	.E_Stop = {
-		{MODE_DEFAULT, SUB_MODE_NULL, E_Robot_Stop}
+		{MODE_DEFAULT, SUB_MODE_DEFAULT, E_Robot_Stop}
 	},
 	.SA_Up = {
-		{MODE_DEFAULT, SUB_MODE_NULL, NULL},
-		{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}
+		{MODE_DEFAULT, SUB_MODE_DEFAULT, NULL},
+		{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_DEFAULT, SUB_MODE_NULL, NULL},
-		{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}
+		{MODE_DEFAULT, SUB_MODE_DEFAULT, NULL},
+		{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_VERTICAL_LEFT, SUB_MODE_NULL, Lane_Change_State_Reset}
+		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Lane_Change_State_Reset}
 	},
 	.SB_Up = {
-		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Move_Forward},
-		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, Robot_Move_Forward_PID}
+		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Forward},
+		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Forward_PID}
 	},
 	.SB_Down = {
-		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Move_Backward},
-		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, Robot_Move_Backward_PID}
+		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Backward},
+		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Backward_PID}
 	},
 	.SC_Up = {
-		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Paint_ON},
-		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, NULL},
-		{MODE_VERTICAL_RIGHT, SUB_MODE_NULL, NULL}
+		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Paint_ON},
+		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, NULL},
+		{MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, NULL}
 	},
 	.SC_Down = {
-		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Paint_OFF},
-		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, NULL},
-		{MODE_VERTICAL_RIGHT, SUB_MODE_NULL, NULL}
+		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Paint_OFF},
+		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, NULL},
+		{MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, NULL}
 	},
 	.SC_Default = {
-		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Paint_OFF},
-		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, NULL},
-		{MODE_VERTICAL_RIGHT, SUB_MODE_NULL, NULL}
+		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Paint_OFF},
+		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, NULL},
+		{MODE_VERTICAL_RIGHT, SUB_MODE_DEFAULT, NULL}
 	},
 	.Joy_Default = {
-		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Stop},
-		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, Robot_Stop_After}
+		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Stop},
+		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, 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_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Forward},
+		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Forward_PID}
 	},
 	.Joy_Left_Down = {
-		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Move_Backward},
-		{MODE_VERTICAL_LEFT, SUB_MODE_NULL, Robot_Move_Backward_PID}
+		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Backward},
+		{MODE_VERTICAL_LEFT, SUB_MODE_DEFAULT, Robot_Move_Backward_PID}
 	},
 	.Joy_Left_Left = {
-		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Move_Left}
+		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Left}
 	},
 	.Joy_Left_Right = {
-		{MODE_DEFAULT, SUB_MODE_NULL, Robot_Move_Right}
+		{MODE_DEFAULT, SUB_MODE_DEFAULT, Robot_Move_Right}
 	}
 };
 #endif
@@ -961,7 +961,7 @@ static void Custom_ModuleHandler(const Msg_t *pstMsg)
 
 			Move_Halt_AngleError();
 
-			MK32_Key_Func(&g_stMK32, g_stPV.RunMode, SUB_MODE_NULL);
+			MK32_Key_Func(&g_stMK32, g_stPV.RunMode, SUB_MODE_DEFAULT);
 			break;
 		}
 		case CMD_SHOW_INFO: