update

components/mini_servo_motor/feite_servo_motor.cpp

@@ -174,6 +174,7 @@ void FeiTeServoMotor::dump_detailed_status(detailed_status_t* detailed_status) {
   ZLOGI(TAG, "= detailed_status->current    :%d", detailed_status->current);
   ZLOGI(TAG, "=");
 }
+bool FeiTeServoMotor::getMoveFlag(uint8_t id, uint8_t& moveflag) { return read_u8(id, kRegServoMoveFlag, moveflag); }
 
 bool FeiTeServoMotor::reCalibration(int id, int16_t pos) {
   if (pos < 0 || pos > 4095) {

components/mini_servo_motor/feite_servo_motor.hpp

@@ -175,6 +175,8 @@ class FeiTeServoMotor {
   bool read_detailed_status(uint8_t id, detailed_status_t* detailed_status);
   void dump_detailed_status(detailed_status_t* detailed_status);
 
+  bool getMoveFlag(uint8_t id, uint8_t& moveflag);
+  
  public:
   bool write_u8(uint8_t id, feite::reg_add_e add, uint8_t regval);
   bool write_u16(uint8_t id, feite::reg_add_e add, uint16_t regval);

components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -18,12 +18,13 @@ int32_t MiniRobotCtrlModule::stop(u8 stop_type) {
    */
   if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
   feite::run_mode_e runmode = m_bus->getmode(m_id);
-  if (runmode != feite::kServoMode) {
-    m_bus->setmode(m_id, feite::kServoMode);
-  } else {
+  if (runmode == feite::kServoMode) {
     m_bus->setmode(m_id, feite::kMotorMode);
   }
-  m_bus->setmode(m_id, runmode);
+  m_bus->setmode(m_id, feite::kServoMode);
+  int16_t nowpos = 0;
+  m_bus->getNowPos(m_id, nowpos);
+  m_bus->setTargetPos(m_id, nowpos);
   return err::ksucc;
 }
 int32_t MiniRobotCtrlModule::position_calibrate(s32 nowpos) {
@@ -39,11 +40,11 @@ int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, functio
   if (torque > 1000) torque = 1000;
   if (torque < 0) torque = 0;
 
-  m_bus->rotate(m_id, speed, torque);
+  if (!m_bus->rotate(m_id, speed, torque)) return err::kce_subdevice_overtime;
 
   m_thread.start([this, speed, torque, run_time, status_cb]() {
     int32_t entertime = zos_get_tick();
-    while (zos_haspassedms(entertime) > run_time) {
+    while (!m_thread.getExitFlag() || zos_haspassedms(entertime) > run_time) {
       m_thread.sleep(10);
     }
     stop(0);
@@ -54,9 +55,83 @@ int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, functio
 
   return 0;
 }
-int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, function<void(move_to_cb_status_t& status)> status_cb) { return 0; }
-int32_t MiniRobotCtrlModule::move_by(s32 pos, s32 speed, s32 torque, function<void(move_by_cb_status_t& status)> status_cb) { return 0; }
-int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, function<void(run_with_torque_cb_status_t& status)> status_cb) { return 0; }
+int32_t MiniRobotCtrlModule::move_to(s32 pos, s32 speed, s32 torque, function<void(move_to_cb_status_t& status)> status_cb) {
+  ZLOGI(TAG, "move_to pos:%d speed:%d torque:%d", pos, speed, torque);
+  m_thread.stop();
+
+  if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
+  if (torque > 1000) torque = 1000;
+  if (torque < 0) torque = 0;
+
+  if (pos > 4095) pos = 4095;
+  if (pos < 0) pos = 0;
+
+  // m_bus->moveTo(m_id, pos, speed, torque);
+  if (!m_bus->moveTo(m_id, pos, speed, torque)) return err::kce_subdevice_overtime;
+
+  m_thread.start([this, pos, speed, torque, status_cb]() {
+    while (!m_thread.getExitFlag()) {
+      uint8_t moveflag = 0;
+      m_bus->getMoveFlag(m_id, moveflag);
+      if (moveflag == 0) break;
+      m_thread.sleep(10);
+    }
+    stop(0);
+    move_to_cb_status_t status;
+    status.pos    = pos;
+    status.status = 0;
+    if (status_cb) status_cb(status);
+  });
+
+  return 0;
+}
+int32_t MiniRobotCtrlModule::move_by(s32 dpos, s32 speed, s32 torque, function<void(move_by_cb_status_t& status)> status_cb) {
+  ZLOGI(TAG, "move_by pos:%d speed:%d torque:%d", dpos, speed, torque);
+  m_thread.stop();
+
+  if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
+  if (torque > 1000) torque = 1000;
+  if (torque < 0) torque = 0;
+
+  int16_t nowpos = 0;
+  m_bus->getNowPos(m_id, nowpos);
+
+  int32_t targetpos = nowpos + dpos;
+  if (targetpos > 4095) targetpos = 4095;
+  if (targetpos < 0) targetpos = 0;
+
+  move_to(targetpos, speed, torque, [this, nowpos, status_cb](move_to_cb_status_t& status) {
+    move_by_cb_status_t moveby_status = {0};
+    moveby_status.dpos                = status.pos - nowpos;
+    moveby_status.status              = status.status;
+    if (status_cb) status_cb(moveby_status);
+  });
+
+  return 0;
+}
+int32_t MiniRobotCtrlModule::run_with_torque(s32 torque, s32 run_time, function<void(run_with_torque_cb_status_t& status)> status_cb) {
+  ZLOGI(TAG, "run_with_torque torque:%d run_time:%d", torque, run_time);
+  m_thread.stop();
+
+  if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
+  if (torque > 1000) torque = 1000;
+  if (torque < 0) torque = 0;
+
+  if (!m_bus->moveWithTorque(m_id, torque)) return err::kce_subdevice_overtime;
+
+  m_thread.start([this, torque, run_time, status_cb]() {
+    int32_t entertime = zos_get_tick();
+    while (!m_thread.getExitFlag() || zos_haspassedms(entertime) > run_time) {
+      m_thread.sleep(10);
+    }
+    stop(0);
+    run_with_torque_cb_status_t status;
+    status.has_run_time = zos_haspassedms(entertime);
+    if (status_cb) status_cb(status);
+  });
+
+  return 0;
+}
 
 int32_t MiniRobotCtrlModule::move_by_nolimit(s32 pos, s32 speed, s32 torque, function<void(move_by_nolimit_cb_status_t& status)> status_cb) { return err::kcommon_error_operation_not_support; }
 

components/zprotocols/zcancmder

@@ -1 +1 @@
-Subproject commit 2393d97dec8c2e235087f1eaf239e5ad4ebb9f2f
+Subproject commit ca80d7fd03f22edaac9927a64e31946323c87dd5