update

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
+stepmotor45_moveBy 4 25600
+
+step_motor_rotate 1 0 
+step_motor_rotate 2 0

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);
   }
 }