diff --git a/Core/Src/main.c b/Core/Src/main.c
index 45f8541..32e46d4 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -100,7 +100,10 @@ int main(void)
 
   /* USER CODE END 2 */
 
-  /* Call init function for freertos objects (in freertos.c) */
+  /* Call init function for freertos objects (in freertos      g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity);
+      g_eq20servomotor.writePn(1, 614, INT32_MIN);
+      g_eq20servomotor.writePn(1, 1501, 0);
+      g_eq20servomotor.writePn(1, 1501, 1);.c) */
   MX_FREERTOS_Init();
 
   /* Start scheduler */
diff --git a/usrc/main.cpp b/usrc/main.cpp
index 51cc61d..134b936 100644
--- a/usrc/main.cpp
+++ b/usrc/main.cpp
@@ -6,6 +6,7 @@
 //
 #include "sdk/os/zos.hpp"
 #include "sdk\components\cmdscheduler\cmd_scheduler.hpp"
+#include "sdk\components\eq_20_asb_motor\eq20_servomotor.hpp"
 #include "sdk\components\iflytop_can_slave_module_master_end\stepmotor.hpp"
 #include "sdk\components\iflytop_can_slave_v1\iflytop_can_master.hpp"
 #include "sdk\components\step_motor_45\step_motor_45.hpp"
@@ -132,6 +133,9 @@ StepMotor           g_step_motor[10];
 IflytopCanMaster    m_IflytopCanMaster;
 static CmdScheduler cmdScheduler;
 
+ModbusBlockHost g_modbusblockhost;
+Eq20ServoMotor  g_eq20servomotor;
+
 bool distance_mm_to_step(int motorid, float distance_mm, int32_t* distance) {
   if (screw_lead.find(motorid) == screw_lead.end()) {
     return false;
@@ -331,6 +335,44 @@ void regfn() {
     motor->moveTo(step);
   });
 
+  static int main_servo_motor_velocity = 400;
+
+  cmdScheduler.registerCmd("main_servo_motor_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
+    // main_servo_motor_rotate direction
+    CHECK_ARGC(1);
+    int direction = atoi(argv[1]);
+    if (direction == 0) {
+      // 用正转位置0来停止设备
+      g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity);
+      g_eq20servomotor.writePn(1, 614, 0);
+      g_eq20servomotor.writePn(1, 1501, 0);
+      g_eq20servomotor.writePn(1, 1501, 1);
+    } else if (direction > 0) {
+      g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity);
+      g_eq20servomotor.writePn(1, 614, 9999999);
+      g_eq20servomotor.writePn(1, 1501, 0);
+      g_eq20servomotor.writePn(1, 1501, 1);
+    } else if (direction < 0) {
+      g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity);
+      g_eq20servomotor.writePn(1, 614, -9999999);
+      g_eq20servomotor.writePn(1, 1501, 0);
+      g_eq20servomotor.writePn(1, 1501, 1);
+    }
+  });
+  cmdScheduler.registerCmd("main_servo_motor_set_velocity", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
+    CHECK_ARGC(1);
+    main_servo_motor_velocity = atoi(argv[1]);
+    return;
+  });
+  cmdScheduler.registerCmd("main_servo_motor_stop", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
+    // main_servo_motor_move_to v
+    CHECK_ARGC(0);
+    g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity);
+    g_eq20servomotor.writePn(1, 614, 0);
+    g_eq20servomotor.writePn(1, 1501, 0);
+    g_eq20servomotor.writePn(1, 1501, 1);
+  });
+
   cmdScheduler.registerCmd("sleep_ms", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
     CHECK_ARGC(1);
     HAL_Delay(atoi(argv[1]));
@@ -379,6 +421,10 @@ void Main::run() {
   step_motor45_scheduler.start();
 
   cmdScheduler.initialize(&DEBUG_UART, 1000);
+
+  g_modbusblockhost.initialize(&huart2);
+  g_eq20servomotor.init(&g_modbusblockhost);
+
   regfn();
   while (true) {
     OSDefaultSchduler::getInstance()->loop();