fix some bug

components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -46,7 +46,7 @@ int32_t MiniRobotCtrlModule::position_calibrate(s32 nowpos) {
   return 0;
 }
 int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, action_cb_status_t status_cb) {
-  ZLOGI(TAG, "rotate speed:%d torque:%d run_time:%d", speed, torque, run_time);
+  ZLOGI(TAG, "%d rotate speed:%d torque:%d run_time:%d", m_module_id, speed, torque, run_time);
   m_thread.stop();
   if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
 
@@ -68,13 +68,13 @@ int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, action_
       }
       m_thread.sleep(10);
     }
-    ZLOGI(TAG, "rotate stop");
+    ZLOGI(TAG, "%d rotate stop", m_module_id);
   });
 
   return 0;
 }
 int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, action_cb_status_t status_cb) {
-  ZLOGI(TAG, "move_to pos:%d speed:%d torque:%d", pos, speed, torque);
+  ZLOGI(TAG, "%d move_to pos:%d speed:%d torque:%d", m_module_id, pos, speed, torque);
   m_thread.stop();
 
   if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
@@ -98,14 +98,14 @@ int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, action_cb_s
       m_thread.sleep(10);
     }
     stop(0);
-    // ZLOGI(TAG, "move_to stop");
+    // ZLOGI(TAG, "%d move_to stop")m_module_id,;
   });
 
   return 0;
 }
 
 int32_t MiniRobotCtrlModule::move_forward(s32 torque) {
-  ZLOGI(TAG, "move_forward torque:%d", torque);
+  ZLOGI(TAG, "%d move_forward torque:%d", m_module_id, torque);
 
   if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
   limit_input(torque, 0, 1000);
@@ -118,13 +118,13 @@ int32_t MiniRobotCtrlModule::move_forward(s32 torque) {
       m_thread.sleep(10);
     }
     stop(0);
-    ZLOGI(TAG, "move_forward stop");
+    ZLOGI(TAG, "%d move_forward stop", m_module_id);
   });
 
   return 0;
 }
 int32_t MiniRobotCtrlModule::move_backward(s32 torque) {
-  ZLOGI(TAG, "move_backward torque:%d", torque);
+  ZLOGI(TAG, "%d move_backward torque:%d", m_module_id, torque);
 
   if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
   limit_input(torque, 0, 1000);
@@ -137,13 +137,13 @@ int32_t MiniRobotCtrlModule::move_backward(s32 torque) {
       m_thread.sleep(10);
     }
     stop(0);
-    ZLOGI(TAG, "move_forward stop");
+    ZLOGI(TAG, "%d move_forward stop", m_module_id);
   });
 
   return 0;
 }
 int32_t MiniRobotCtrlModule::move_by(s32 dpos, s32 speed, s32 torque, action_cb_status_t status_cb) {
-  ZLOGI(TAG, "move_by pos:%d speed:%d torque:%d", dpos, speed, torque);
+  ZLOGI(TAG, "%d move_by pos:%d speed:%d torque:%d", m_module_id, dpos, speed, torque);
   m_thread.stop();
 
   if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
@@ -164,7 +164,7 @@ int32_t MiniRobotCtrlModule::move_by(s32 dpos, s32 speed, s32 torque, action_cb_
 int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, action_cb_status_t status_cb) {
   return err::koperation_not_support;
 #if 0
-  ZLOGI(TAG, "run_with_torque torque:%d run_time:%d", torque, run_time);
+  ZLOGI(TAG, "%d run_with_torque torque:%d run_time:%d",m_module_id, torque, run_time);
   m_thread.stop();
 
   if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
@@ -248,7 +248,7 @@ int32_t MiniRobotCtrlModule::read_detailed_status(detailed_status_t &debug_info)
 
 int32_t MiniRobotCtrlModule::set_basic_param(basic_param_t &param) { return err::koperation_not_support; }
 int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t &param) {
-  ZLOGI(TAG, "get_basic_param");
+  // ZLOGI(TAG, "%d get_basic_param", m_module_id);
   if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
   m_bus->read_u16(m_id, feite::kRegServoCalibration, param.posCalibrate);
   m_bus->read_u16(m_id, feite::kRegServoMinAngle, param.minlimit);
@@ -325,14 +325,23 @@ int32_t MiniRobotCtrlModule::motor_rotate_with_torque(int32_t direction, int32_t
 }
 void MiniRobotCtrlModule::set_errorcode(int32_t errorcode) { m_errorcode = errorcode; }
 
+int32_t MiniRobotCtrlModule::getdpos(int32_t targetpos) {
+  int32_t nowpos = 0;
+  m_bus->getNowPos(m_id, nowpos);
+
+  int32_t dpos = targetpos - nowpos;
+  return abs(dpos);
+}
+
 int32_t MiniRobotCtrlModule::motor_move_to_torque(int32_t targetpos, int32_t torque, int32_t overtime) {
-  ZLOGI(TAG, "move_backward torque:%d", torque);
+  ZLOGI(TAG, "%d motor_move_to_torque torque:%d", m_module_id, torque);
 
   if (!m_bus->ping(m_id)) return err::ksubdevice_overtime;
   limit_input(torque, 0, 1000);
 
   m_thread.stop();
   if (!m_bus->moveTo(m_id, targetpos, 0, torque)) return err::ksubdevice_overtime;
+
   m_thread.start([this, torque, overtime, targetpos]() {
     int32_t entertime = zos_get_tick();
 
@@ -340,20 +349,18 @@ int32_t MiniRobotCtrlModule::motor_move_to_torque(int32_t targetpos, int32_t tor
       if (m_thread.getExitFlag()) break;
 
       if (overtime != 0 && zos_haspassedms(entertime) > overtime) {
-        ZLOGI(TAG, "motor_move_to_torque %d overtime", m_id);
+        ZLOGI(TAG, "%d motor_move_to_torque %d overtime", m_module_id, m_id);
         stop(0);
         set_errorcode(err::kmotor_run_overtime);
         break;
       }
 
-      int32_t nowpos   = 0;
       uint8_t moveFlag = 0;
 
-      m_bus->getNowPos(m_id, nowpos);
       m_bus->getMoveFlag(m_id, moveFlag);
 
-      if (nowpos - targetpos < 10 && moveFlag == 0) {
-        ZLOGI(TAG, "motor_move_to_torque %d succ", m_id);
+      if (getdpos(targetpos) < 10 && moveFlag == 0) {
+        ZLOGI(TAG, "%d motor_move_to_torque %d succ", m_module_id, m_id);
         stop(0);
         break;
       }

components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp

@@ -82,7 +82,8 @@ class MiniRobotCtrlModule : public I_MiniServoModule, public ZIModule, public ZI
   virtual int32_t motor_read_pos(int32_t *pos);
 
  private:
-  void call_status_cb(action_cb_status_t cb, int32_t status);
-  void set_errorcode(int32_t errorcode);
+  void    call_status_cb(action_cb_status_t cb, int32_t status);
+  void    set_errorcode(int32_t errorcode);
+  int32_t getdpos(int32_t targetpos);
 };
 }  // namespace iflytop