update

components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -3,6 +3,7 @@
 #include "sdk\components\errorcode\errorcode.hpp"
 using namespace iflytop;
 using namespace std;
+#define TAG "MiniRobotCtrlModule"
 
 void MiniRobotCtrlModule::initialize(FeiTeServoMotor* bus, uint8_t id) { m_bus->write_u8(m_id, feite::kRegServoRunMode, feite::kServoMode); }
 
@@ -31,15 +32,26 @@ int32_t MiniRobotCtrlModule::position_calibrate(s32 nowpos) {
   if (m_bus->reCalibration(m_id, nowpos)) return err::kce_subdevice_overtime;
   return 0;
 }
-int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 run_time, function<void(rotate_cb_status_t& status)> status_cb) {
-  /**
-   * @brief
-   *  1:切换成电机模式
-   *  2:写入速度
-   *  3:启动线程，线程退后，设置速度为0
-   */
+int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, function<void(rotate_cb_status_t& status)> status_cb) {
+  ZLOGI(TAG, "rotate speed:%d torque:%d run_time:%d", speed, 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;
+
+  m_bus->rotate(m_id, speed, torque);
+
+  m_thread.start([this, speed, torque, run_time, status_cb]() {
+    int32_t entertime = zos_get_tick();
+    while (zos_haspassedms(entertime) > run_time) {
+      m_thread.sleep(10);
+    }
+    stop(0);
+    rotate_cb_status_t status;
+    status.has_run_time = zos_haspassedms(entertime);
+    if (status_cb) status_cb(status);
+  });
+
   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; }
@@ -70,12 +82,24 @@ int32_t MiniRobotCtrlModule::read_status(status_t& status) {
   }
   return 0;
 }
-int32_t MiniRobotCtrlModule::read_debug_info(detailed_status_t& debug_info) {
+int32_t MiniRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) {
   if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
-  FeiTeServoMotor::status_t feitestatus;
-  if (!m_bus->read_status(m_id, &feitestatus)) return err::kce_subdevice_overtime;
-
+  FeiTeServoMotor::detailed_status_t feitestatus;
+  if (!m_bus->read_detailed_status(m_id, &feitestatus)) return err::kce_subdevice_overtime;
 
+  if (feitestatus.state != 0) {
+    debug_info.status = 3;
+  } else {
+    debug_info.status = m_thread.isworking();
+  }
+  debug_info.io_state    = 0;
+  debug_info.torque      = feitestatus.torque;
+  debug_info.speed       = feitestatus.vel;
+  debug_info.pos         = feitestatus.pos;
+  debug_info.voltage     = feitestatus.voltage;
+  debug_info.current     = feitestatus.current;
+  debug_info.temperature = feitestatus.temperature;
+  debug_info.error_flag  = feitestatus.state;
   return 0;
 }
 
@@ -86,4 +110,3 @@ int32_t MiniRobotCtrlModule::set_warning_limit_param(warning_limit_param_t& para
 int32_t MiniRobotCtrlModule::get_run_param(run_param_t& param) { return 0; }
 int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t& param) { return 0; }
 int32_t MiniRobotCtrlModule::get_warning_limit_param(warning_limit_param_t& param) { return 0; }
-uint8_t MiniRobotCtrlModule::get_module_status() { ; }

components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp

@@ -19,7 +19,7 @@ class MiniRobotCtrlModule : public I_MiniServoModule {
   virtual int32_t stop(u8 stop_type) override;
   virtual int32_t position_calibrate(s32 nowpos) override;
 
-  virtual int32_t rotate(s32 speed, s32 run_time, function<void(rotate_cb_status_t& status)> status_cb) override;
+  virtual int32_t rotate(s32 speed, s32 torque, s32 run_time, function<void(rotate_cb_status_t& status)> status_cb) override;
   virtual int32_t move_to(s32 pos, s32 speed, s32 torque, function<void(move_to_cb_status_t& status)> status_cb) override;
   virtual int32_t move_by(s32 pos, s32 speed, s32 torque, function<void(move_by_cb_status_t& status)> status_cb) override;
   virtual int32_t run_with_torque(s32 torque, s32 run_time, function<void(run_with_torque_cb_status_t& status)> status_cb) override;
@@ -27,7 +27,7 @@ class MiniRobotCtrlModule : public I_MiniServoModule {
 
   virtual int32_t read_version(version_t& version) override;
   virtual int32_t read_status(status_t& status) override;
-  virtual int32_t read_debug_info(detailed_status_t& debug_info) override;
+  virtual int32_t read_detailed_status(detailed_status_t& debug_info) override;
 
   virtual int32_t set_run_param(run_param_t& param) override;
   virtual int32_t set_basic_param(basic_param_t& param) override;
@@ -38,6 +38,5 @@ class MiniRobotCtrlModule : public I_MiniServoModule {
   virtual int32_t get_warning_limit_param(warning_limit_param_t& param) override;
 
  private:
-  uint8_t get_module_status();
 };
 }  // namespace iflytop

components/step_motor_ctrl_module/step_motor_ctrl_module.cpp

@@ -208,7 +208,7 @@ int32_t StepMotorCtrlModule::read_status(status_t& status) {
   getnowpos(status.x);
   return 0;
 }
-int32_t StepMotorCtrlModule::read_debug_info(detailed_status_t& debug_info) {
+int32_t StepMotorCtrlModule::read_detailed_status(detailed_status_t& debug_info) {
   debug_info.status = m_status;
   if (m_Xgpio) debug_info.io_state |= m_Xgpio->getState() ? 0x01 : 0x00;
   if (m_end_gpio) debug_info.io_state |= m_end_gpio->getState() ? 0x02 : 0x00;

components/step_motor_ctrl_module/step_motor_ctrl_module.hpp

@@ -58,7 +58,7 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule {
 
   virtual int32_t read_version(version_t& version) override;
   virtual int32_t read_status(status_t& status) override;
-  virtual int32_t read_debug_info(detailed_status_t& debug_info) override;
+  virtual int32_t read_detailed_status(detailed_status_t& debug_info) override;
 
   virtual int32_t set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) override;
   virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override;

components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

@@ -199,7 +199,7 @@ int32_t XYRobotCtrlModule::read_status(status_t& status) {
   getnowpos(status.x, status.y);
   return 0;
 }
-int32_t XYRobotCtrlModule::read_debug_info(detailed_status_t& debug_info) {
+int32_t XYRobotCtrlModule::read_detailed_status(detailed_status_t& debug_info) {
   zlock_guard lock(m_lock);
   debug_info = {0};
   if (m_Xgpio) debug_info.iostate |= m_Xgpio->getState() ? 0x01 : 0x00;

components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp

@@ -70,7 +70,7 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule {
 
   virtual int32_t read_version(version_t& version) override;
   virtual int32_t read_status(status_t& status) override;
-  virtual int32_t read_debug_info(detailed_status_t& debug_info) override;
+  virtual int32_t read_detailed_status(detailed_status_t& debug_info) override;
 
   virtual int32_t set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) override;
   virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override;

components/zcancmder_module/zcan_step_motor_ctrl_module.cpp

@@ -96,7 +96,7 @@ void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
   END_PP();
 
   PROCESS_PACKET(kcmd_step_motor_ctrl_read_detailed_status, m_id) {  //
-    errorcode = m_module->read_debug_info(ack->ack);
+    errorcode = m_module->read_detailed_status(ack->ack);
   }
   END_PP();
 

components/zcancmder_module/zcan_xy_robot_module.cpp

@@ -87,7 +87,7 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
   END_PP();
 
   PROCESS_PACKET(kcmd_xy_robot_ctrl_read_detailed_status, m_id) {  //
-    errorcode = m_xyRobotCtrlModule->read_debug_info(ack->ack);
+    errorcode = m_xyRobotCtrlModule->read_detailed_status(ack->ack);
   }
   END_PP();