From 2cc69263e40ca80e1af8142a84965dcb3f9814b6 Mon Sep 17 00:00:00 2001
From: zhaohe <h_zhaohe@163.com>
Date: Wed, 6 Sep 2023 08:08:56 +0800
Subject: [PATCH] update

---
 README.md     |  5 ++++-
 usrc/main.cpp | 34 ++++++++++++++++++++++++++--------
 2 files changed, 30 insertions(+), 9 deletions(-)

diff --git a/README.md b/README.md
index d7f06df..aa4564c 100644
--- a/README.md
+++ b/README.md
@@ -33,4 +33,7 @@ stepmotor45_moveBy 4 -25600
 stepmotor45_moveBy 1 25600
 stepmotor45_moveBy 2 25600
 stepmotor45_moveBy 3 25600
-stepmotor45_moveBy 4 25600
\ No newline at end of file
+stepmotor45_moveBy 4 25600
+
+step_motor_rotate 1 0 
+step_motor_rotate 2 0
\ No newline at end of file
diff --git a/usrc/main.cpp b/usrc/main.cpp
index 5ac500c..b525c6a 100644
--- a/usrc/main.cpp
+++ b/usrc/main.cpp
@@ -148,7 +148,8 @@ bool distance_mm_to_step(int motorid, float distance_mm, int32_t* distance) {
 void regfn() {
   cmdScheduler.registerCmd("help",  //
                            [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { ZLOGI(TAG, "do_help"); });
-
+  cmdScheduler.registerCmd("reset_board",  //
+                           [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) { NVIC_SystemReset(); });
   cmdScheduler.registerCmd("stepmotor45_wait_to_reach_pos", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
     // stepmotor45_rotate motorid direction
     CHECK_ARGC(2);
@@ -166,13 +167,23 @@ void regfn() {
       osDelay(100);
     }
   });
+  cmdScheduler.registerCmd("stepmotor45_reset_pos_all", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
+    // stepmotor45_rotate motorid direction
+    CHECK_ARGC(0);
+    g_step_motor45[1].setPos(0);
+    g_step_motor45[2].setPos(0);
+    g_step_motor45[3].setPos(0);
+    g_step_motor45[4].setPos(0);
+    g_step_motor45[5].setPos(0);
+    g_step_motor45[6].setPos(0);
+  });
 
   // stepmotor45
   cmdScheduler.registerCmd("stepmotor45_rotate", [](int argc, char** argv, CmdScheduler::CmdProcessContext* context) {
     // stepmotor45_rotate motorid direction
     CHECK_ARGC(2);
-    int  motorid   = atoi(argv[1]);
-    bool direction = atoi(argv[2]);
+    int motorid   = atoi(argv[1]);
+    int direction = atoi(argv[2]);
 
     if (motorid < 1 || motorid > 6) {
       ZLOGE(TAG, "motorid out of range");
@@ -361,18 +372,25 @@ void regfn() {
     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, 614, 1);
       g_eq20servomotor.writePn(1, 1501, 0);
       g_eq20servomotor.writePn(1, 1501, 1);
+      g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity);
+
     } else if (direction > 0) {
       g_eq20servomotor.writePn(1, 615, main_servo_motor_velocity);
-      g_eq20servomotor.writePn(1, 614, 9999999);
+      g_eq20servomotor.writePn(1, 614, -1);
+      g_eq20servomotor.writePn(1, 1501, 0);
+      g_eq20servomotor.writePn(1, 1501, 1);
+      g_eq20servomotor.writePn(1, 614, 999999);
       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, 614, 1);
+      g_eq20servomotor.writePn(1, 1501, 0);
+      g_eq20servomotor.writePn(1, 1501, 1);
+      g_eq20servomotor.writePn(1, 614, -999999);
       g_eq20servomotor.writePn(1, 1501, 0);
       g_eq20servomotor.writePn(1, 1501, 1);
     }
@@ -449,6 +467,6 @@ void Main::run() {
     cmdScheduler.schedule();
     m_IflytopCanMaster.periodicJob();
     // m_IflytopCanMaster.writeReg(1,1,1,10);
-    osDelay(1);
+    // osDelay(1);
   }
 }