From b6a81bd5fc505cea769e3d7a334874d2011de814 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sat, 30 Sep 2023 19:38:11 +0800 Subject: [PATCH] update --- .../mini_servo_motor_ctrl_module.cpp | 47 ++++++++++++++++------ .../mini_servo_motor_ctrl_module.hpp | 5 +-- .../step_motor_ctrl_module.cpp | 2 +- .../step_motor_ctrl_module.hpp | 2 +- .../xy_robot_ctrl_module/xy_robot_ctrl_module.cpp | 2 +- .../xy_robot_ctrl_module/xy_robot_ctrl_module.hpp | 2 +- .../zcan_step_motor_ctrl_module.cpp | 2 +- .../zcancmder_module/zcan_xy_robot_module.cpp | 2 +- 8 files changed, 43 insertions(+), 21 deletions(-) diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp index 2e6101f..bd20c86 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp +++ b/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 status_cb) { - /** - * @brief - * 1:切换成电机模式 - * 2:写入速度 - * 3:启动线程,线程退后,设置速度为0 - */ +int32_t MiniRobotCtrlModule::rotate(s32 speed, s32 torque, s32 run_time, function 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 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() { ; } diff --git a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp b/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp index f67f4bc..5fb3a8f 100644 --- a/components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp +++ b/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 status_cb) override; + virtual int32_t rotate(s32 speed, s32 torque, s32 run_time, function status_cb) override; virtual int32_t move_to(s32 pos, s32 speed, s32 torque, function status_cb) override; virtual int32_t move_by(s32 pos, s32 speed, s32 torque, function status_cb) override; virtual int32_t run_with_torque(s32 torque, s32 run_time, function 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 \ No newline at end of file diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index ae8bb11..7045893 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/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; diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index a3f16b5..699175d 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/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; diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp index 00b9492..15fab0f 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp +++ b/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; diff --git a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp b/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp index 302a158..8988009 100644 --- a/components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp +++ b/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; diff --git a/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp b/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp index e5bdb28..0f3977b 100644 --- a/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp +++ b/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(); diff --git a/components/zcancmder_module/zcan_xy_robot_module.cpp b/components/zcancmder_module/zcan_xy_robot_module.cpp index a4d5094..9b8be46 100644 --- a/components/zcancmder_module/zcan_xy_robot_module.cpp +++ b/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();