diff --git a/Src/command.c b/Src/command.c
index 8ee2752..116ce92 100644
--- a/Src/command.c
+++ b/Src/command.c
@@ -462,7 +462,7 @@ void handle_command(const uint8_t *frame, uint8_t len) {
                         send_response(RESP_TYPE_PARAM_ERR, s_report_status_err, sizeof(s_report_status_err));
                         return;
                     }
-                    COMMAND_DEBUG("M3 Command(PUSH): Actuator Channel=%u, Duty Cycle=%u", p_val, s_val);
+                    SEGGER_RTT_printf(0, "[COMMAND] M3 Command(PUSH): Actuator Channel=%u, Duty Cycle=%u \n", p_val, s_val);
                     actuator_output((actuator_channel_t)p_val, (uint16_t)s_val, DIRECTION_PUSH);
                     send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok));
                 }
@@ -484,12 +484,24 @@ void handle_command(const uint8_t *frame, uint8_t len) {
                         send_response(RESP_TYPE_PARAM_ERR, s_report_status_err, sizeof(s_report_status_err));
                         return;
                     }
-                    COMMAND_DEBUG("M4 Command(PULL): Actuator Channel=%u, Duty Cycle=%u", p_val, s_val);
+                    SEGGER_RTT_printf(0, "[COMMAND] M4 Command(PULL): Actuator Channel=%u, Duty Cycle=%u \n", p_val, s_val);
                     actuator_output((actuator_channel_t)p_val, (uint16_t)s_val, DIRECTION_PULL);
                     send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok));
                 }
                 return;
 
+            case 5u: // M5 stop actuator output
+                uint32_t p_val = 0;
+                bool has_p = find_parameter_value(cmd, cmd_len, 'P', &p_val);
+                if (!has_p || p_val > (uint32_t)ACTUATOR_CH2) {
+                    send_response(RESP_TYPE_PARAM_ERR, s_report_status_err, sizeof(s_report_status_err));
+                    return;
+                }
+            SEGGER_RTT_printf(0, "[COMMAND] M5 Command(STOP): Actuator Channel=%u \n", p_val);
+            actuator_stop((actuator_channel_t)p_val);
+            send_response(RESP_TYPE_OK, s_report_status_ok, sizeof(s_report_status_ok));
+            return;
+
         /* ==========================================
          * M888 软件重启命令
          * ========================================== */
diff --git a/Src/main.c b/Src/main.c
index cd94610..0f10291 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -53,9 +53,6 @@ int main(void)
 {
     led_init();
     mcu_detect_and_config();
-
-    // delay_ms(1000);
-
     systick_config();
 
     uart_ring_buffer_init();
@@ -63,10 +60,6 @@ int main(void)
 
     actuator_init();
 
-    // actuator_output(ACTUATOR_CH1, 800, DIRECTION_PULL); // Set actuator channel 1 to high, channel 2 to low
-
-    // i2c_config();
-
 #if DEBUG_MODE == CFG_ENABLE
     printf("Hello World!\r\n");
 #endif
@@ -82,6 +75,7 @@ int main(void)
 #endif
 
     /* ========== Command Testing ========== */
+    // command_execute("M3P0S999");
   
     /* ========== */