diff --git a/sdk b/sdk index e06be65..1a752ec 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit e06be65d256688d5ab0018e4d195352654081205 +Subproject commit 1a752ece8b739fe743348afe09463ed8b09565be diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index eca9d05..275bb68 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -1,54 +1,187 @@ #include "intelligent_winding_robot_ctrl.hpp" + #include +#include using namespace std; using namespace iflytop; #define TAG "IntelligentWindingRobotCtrl" +#define DO(exptr) \ + { \ + int32_t ret = exptr; \ + if (ret != 0) { \ + return ret; \ + } \ + } + +void IntelligentWindingRobotCtrl::processError(int32_t err) { ZLOGE(TAG, "processError: %d", err); } + +void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) { + while (true) { + int32_t status = 0; + int32_t ecode = m_dm->module_get_status(moduleid, &status); + if (ecode != 0) { + processError(ecode); + break; + }; + if (status == 0) break; + if (status == 2) { + processError(err::kfail); + break; + }; + zos_delay(100); + } +} + int32_t IntelligentWindingRobotCtrl::initialize_device() { return 0; } // 排废舵机 -int32_t IntelligentWindingRobotCtrl::paifei_duoji_moveto_reset() { return 0; } -int32_t IntelligentWindingRobotCtrl::paifei_duoji_moveto_press() { return 0; } +int32_t IntelligentWindingRobotCtrl::paifei_duoji_moveto_reset() { + ZLOGI(TAG, "paifei_duoji_moveto_reset %d %d %d", 15, cfg.paifei_duoji_reset_pos, 330); + DO(m_dm->motor_move_to(15, cfg.paifei_duoji_reset_pos, 2000, 0)); + wait_module_idle(15); + return 0; +} +int32_t IntelligentWindingRobotCtrl::paifei_duoji_moveto_press() { + ZLOGI(TAG, "paifei_duoji_moveto_press %d %d %d", 15, cfg.paifei_duoji_press_pos, cfg.paifei_duoji_press_torque); + DO(m_dm->motor_move_to_with_torque(15, cfg.paifei_duoji_press_pos, cfg.paifei_duoji_press_torque)); + wait_module_idle(15); +} /** * @brief 绕线探测舵机 */ -int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_reset() { return 0; } -int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_get_thickness() { return 0; } +int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_reset() { + ZLOGI(TAG, "raoxiantance_duoji_move_to_reset %d %d %d", 14, cfg.raoxiantance_duoji_reset_pos, 330); + DO(m_dm->motor_move_to(14, cfg.raoxiantance_duoji_reset_pos, 2000, 0)); + wait_module_idle(14); + return 0; +} +int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_get_thickness(int32_t* thickness) { + ZLOGI(TAG, "raoxiantance_duoji_move_to_get_thickness %d %d", 14, cfg.raoxiantance_duoji_tance_zero_pos); + DO(m_dm->motor_move_to_with_torque(14, cfg.raoxiantance_duoji_tance_zero_pos, 200)); + zos_delay(2000); + + int32_t nowpos = 0; + DO(m_dm->motor_read_pos(14, &nowpos)); + + *thickness = cfg.raoxiantance_duoji_tance_zero_pos - nowpos; + DO(raoxiantance_duoji_move_to_reset()); + return 0; +} /** * @brief 压线舵机 */ -int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_reset() { return 0; } -int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_press_pos() { return 0; } -int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_wait_for_press_pos() { return 0; } +int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_reset() { + ZLOGI(TAG, "yaxian_duoji_move_to_reset %d %d %d", 13, cfg.yaxian_duoji_reset_pos, 330); + DO(m_dm->motor_move_to(13, cfg.yaxian_duoji_reset_pos, 2000, 0)); + wait_module_idle(13); + return 0; +} +int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_press_pos() { + ZLOGI(TAG, "yaxian_duoji_move_to_press_pos %d %d %d", 13, cfg.yaxian_duoji_press_pos, cfg.yaxian_duoji_press_torque); + DO(m_dm->motor_move_to_with_torque(13, cfg.yaxian_duoji_press_pos, cfg.yaxian_duoji_press_torque)); + wait_module_idle(13); + return 0; +} +int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_wait_for_press_pos() { + ZLOGI(TAG, "yaxian_duoji_move_to_wait_for_press_pos %d %d %d", 13, cfg.yaxian_duoji_wait_for_press_pos, cfg.yaxian_duoji_press_torque); + DO(m_dm->motor_move_to_with_torque(13, cfg.yaxian_duoji_wait_for_press_pos, cfg.yaxian_duoji_press_torque)); + wait_module_idle(13); + return 0; +} /** * @brief 线拉紧舵机 */ -int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_reset() { return 0; } // 零位 -int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_line_entry_pos() { return 0; } // 入线位 -int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_tight_line_pos() { return 0; } // 拉线位置 -int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_loose_line_pos() { return 0; } // 放线位置 +int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_reset() { + ZLOGI(TAG, "xianlajin_duoji_move_to_reset %d %d %d", 12, cfg.xianlajin_duoji_reset_pos, 330); + DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_reset_pos, 2000, 0)); + wait_module_idle(16); + return 0; +} // 零位 +int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_line_entry_pos() { + ZLOGI(TAG, "xianlajin_duoji_move_to_line_entry_pos %d %d %d", 16, cfg.xianlajin_duoji_line_entry_pos, 330); + DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_line_entry_pos, 2000, 0)); + wait_module_idle(16); + return 0; +} // 入线位 +int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_tight_line_pos() { + ZLOGI(TAG, "xianlajin_duoji_move_to_tight_line_pos %d %d %d", 16, cfg.xianlajin_duoji_tight_line_pos, 330); + DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_tight_line_pos, 2000, 0)); + wait_module_idle(16); + return 0; +} // 拉线位置 +int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_loose_line_pos() { + ZLOGI(TAG, "xianlajin_duoji_move_to_loose_line_pos %d %d %d", 16, cfg.xianlajin_duoji_loose_line_pos, 330); + DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_loose_line_pos, 2000, 0)); + wait_module_idle(16); + return 0; +} // 放线位置 /** - * @brief 夹线舵机 + * @brief 夹线舵机12 */ -int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_reset_pos() { return 0; } -int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_clamp_pos() { return 0; } +int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_reset_pos() { + ZLOGI(TAG, "jiaxian_duoji_move_to_reset_pos %d %d %d", 12, cfg.jiaxian_duoji_reset_pos, 330); + DO(m_dm->motor_move_to(12, cfg.jiaxian_duoji_reset_pos, 2000, 0)); + wait_module_idle(12); + return 0; +} +int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_clamp_pos() { + ZLOGI(TAG, "jiaxian_duoji_move_to_clamp_pos %d %d %d", 12, cfg.jiaxian_duoji_clamp_pos, cfg.jiaxian_duoji_clamp_torque); + DO(m_dm->motor_move_to_with_torque(12, cfg.jiaxian_duoji_clamp_pos, cfg.jiaxian_duoji_clamp_torque)); + wait_module_idle(12); + return 0; +} /** * @brief 剪刀 */ -int32_t IntelligentWindingRobotCtrl::scissors_move_reset_pos() { return 0; } // block -int32_t IntelligentWindingRobotCtrl::scissors_cut() { return 0; } // block +int32_t IntelligentWindingRobotCtrl::scissors_move_reset_pos() { + // ZLOGI(TAG, "scissors_move_reset_pos %d", 11); + ZLOGI(TAG, "scissors_move_reset_pos"); + return 0; +} // block +int32_t IntelligentWindingRobotCtrl::scissors_cut() { + ZLOGI(TAG, "scissors_cut %d", 24); + DO(m_dm->motor_move_by(24, 4095, 0, 0)); + wait_module_idle(24); + return 0; +} // block /** * @brief 机械臂夹线舵机 */ -int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_reset_pos() { return 0; } -int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_clamp_pos() { return 0; } +int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_reset_pos() { + ZLOGI(TAG, "arm_jiaxian_duoji_move_to_reset_pos %d %d %d", 11, cfg.arm_jiaxian_duoji_reset_pos, 330); + DO(m_dm->motor_move_to(11, cfg.arm_jiaxian_duoji_reset_pos, 2000, 0)); + wait_module_idle(11); + return 0; +} +int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_clamp_pos() { + ZLOGI(TAG, "arm_jiaxian_duoji_move_to_clamp_pos %d %d %d", 11, cfg.arm_jiaxian_duoji_clamp_pos, cfg.arm_jiaxian_duoji_clamp_torque); + DO(m_dm->motor_move_to_with_torque(11, cfg.arm_jiaxian_duoji_clamp_pos, cfg.arm_jiaxian_duoji_clamp_torque)); + wait_module_idle(11); + return 0; +} /** * @brief 机械臂钩爪 */ -int32_t IntelligentWindingRobotCtrl::arm_hook_claws_reset() { return 0; } -int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_half_pos() { return 0; } -int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_full_pos() { return 0; } +int32_t IntelligentWindingRobotCtrl::arm_hook_claws_reset() { + ZLOGI(TAG, "arm_hook_claws_reset"); + DO(m_dm->motor_move_to_zero_backward(21, 0, 0, 0, 0)); + wait_module_idle(21); + return 0; +} +int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_half_pos() { + ZLOGI(TAG, "arm_hook_claws_move_to_half_pos"); + DO(m_dm->motor_move_to(21, cfg.arm_hook_claws_half_pos, 0, 0)); + wait_module_idle(21); + return 0; +} +int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_full_pos() { + ZLOGI(TAG, "arm_hook_claws_move_to_full_pos"); + DO(m_dm->motor_move_to(21, cfg.arm_hook_claws_full_pos, 0, 0)); + wait_module_idle(21); + return 0; +} /** * @brief XY平台 */ @@ -71,10 +204,82 @@ int32_t IntelligentWindingRobotCtrl::xy_remove_line() { return 0; } int32_t IntelligentWindingRobotCtrl::start_winding() { return 0; } int32_t IntelligentWindingRobotCtrl::stop_winding() { return 0; } int32_t IntelligentWindingRobotCtrl::reset_and_check_device() { return 0; } - -int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) { return 0; } -int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; } - +int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) { + if (strcmp(cfgname, "paifei_duoji_reset_pos") == 0) + cfg.paifei_duoji_reset_pos = cfgvalue; + else if (strcmp(cfgname, "paifei_duoji_press_pos") == 0) + cfg.paifei_duoji_press_pos = cfgvalue; + else if (strcmp(cfgname, "paifei_duoji_press_torque") == 0) + cfg.paifei_duoji_press_torque = cfgvalue; + else if (strcmp(cfgname, "raoxiantance_duoji_reset_pos") == 0) + cfg.raoxiantance_duoji_reset_pos = cfgvalue; + else if (strcmp(cfgname, "raoxiantance_duoji_tance_zero_pos") == 0) + cfg.raoxiantance_duoji_tance_zero_pos = cfgvalue; + else if (strcmp(cfgname, "yaxian_duoji_reset_pos") == 0) + cfg.yaxian_duoji_reset_pos = cfgvalue; + else if (strcmp(cfgname, "yaxian_duoji_press_pos") == 0) + cfg.yaxian_duoji_press_pos = cfgvalue; + else if (strcmp(cfgname, "yaxian_duoji_press_torque") == 0) + cfg.yaxian_duoji_press_torque = cfgvalue; + else if (strcmp(cfgname, "yaxian_duoji_wait_for_press_pos") == 0) + cfg.yaxian_duoji_wait_for_press_pos = cfgvalue; + else if (strcmp(cfgname, "xianlajin_duoji_reset_pos") == 0) + cfg.xianlajin_duoji_reset_pos = cfgvalue; + else if (strcmp(cfgname, "xianlajin_duoji_line_entry_pos") == 0) + cfg.xianlajin_duoji_line_entry_pos = cfgvalue; + else if (strcmp(cfgname, "xianlajin_duoji_tight_line_pos") == 0) + cfg.xianlajin_duoji_tight_line_pos = cfgvalue; + else if (strcmp(cfgname, "xianlajin_duoji_loose_line_pos") == 0) + cfg.xianlajin_duoji_loose_line_pos = cfgvalue; + else if (strcmp(cfgname, "jiaxian_duoji_reset_pos") == 0) + cfg.jiaxian_duoji_reset_pos = cfgvalue; + else if (strcmp(cfgname, "jiaxian_duoji_clamp_pos") == 0) + cfg.jiaxian_duoji_clamp_pos = cfgvalue; + else if (strcmp(cfgname, "jiaxian_duoji_clamp_torque") == 0) + cfg.jiaxian_duoji_clamp_torque = cfgvalue; + else if (strcmp(cfgname, "arm_jiaxian_duoji_reset_pos") == 0) + cfg.arm_jiaxian_duoji_reset_pos = cfgvalue; + else if (strcmp(cfgname, "arm_jiaxian_duoji_clamp_pos") == 0) + cfg.arm_jiaxian_duoji_clamp_pos = cfgvalue; + else if (strcmp(cfgname, "arm_jiaxian_duoji_clamp_torque") == 0) + cfg.arm_jiaxian_duoji_clamp_torque = cfgvalue; + else if (strcmp(cfgname, "scissors_reset_pos") == 0) + cfg.scissors_reset_pos = cfgvalue; + else if (strcmp(cfgname, "scissors_cut_pos") == 0) + cfg.scissors_cut_pos = cfgvalue; + else if (strcmp(cfgname, "arm_hook_claws_half_pos") == 0) + cfg.arm_hook_claws_half_pos = cfgvalue; + else if (strcmp(cfgname, "arm_hook_claws_full_pos") == 0) + cfg.arm_hook_claws_full_pos = cfgvalue; + return 0; +} +int32_t IntelligentWindingRobotCtrl::dumpcfg() { + ZLOGI(TAG, "paifei_duoji_reset_pos %d", cfg.paifei_duoji_reset_pos); + ZLOGI(TAG, "paifei_duoji_press_pos %d", cfg.paifei_duoji_press_pos); + ZLOGI(TAG, "paifei_duoji_press_torque %d", cfg.paifei_duoji_press_torque); + ZLOGI(TAG, "raoxiantance_duoji_reset_pos %d", cfg.raoxiantance_duoji_reset_pos); + ZLOGI(TAG, "raoxiantance_duoji_tance_zero_pos %d", cfg.raoxiantance_duoji_tance_zero_pos); + ZLOGI(TAG, "yaxian_duoji_reset_pos %d", cfg.yaxian_duoji_reset_pos); + ZLOGI(TAG, "yaxian_duoji_press_pos %d", cfg.yaxian_duoji_press_pos); + ZLOGI(TAG, "yaxian_duoji_press_torque %d", cfg.yaxian_duoji_press_torque); + ZLOGI(TAG, "yaxian_duoji_wait_for_press_pos %d", cfg.yaxian_duoji_wait_for_press_pos); + ZLOGI(TAG, "xianlajin_duoji_reset_pos %d", cfg.xianlajin_duoji_reset_pos); + ZLOGI(TAG, "xianlajin_duoji_line_entry_pos %d", cfg.xianlajin_duoji_line_entry_pos); + ZLOGI(TAG, "xianlajin_duoji_tight_line_pos %d", cfg.xianlajin_duoji_tight_line_pos); + ZLOGI(TAG, "xianlajin_duoji_loose_line_pos %d", cfg.xianlajin_duoji_loose_line_pos); + ZLOGI(TAG, "jiaxian_duoji_reset_pos %d", cfg.jiaxian_duoji_reset_pos); + ZLOGI(TAG, "jiaxian_duoji_clamp_pos %d", cfg.jiaxian_duoji_clamp_pos); + ZLOGI(TAG, "jiaxian_duoji_clamp_torque %d", cfg.jiaxian_duoji_clamp_torque); + ZLOGI(TAG, "arm_jiaxian_duoji_reset_pos %d", cfg.arm_jiaxian_duoji_reset_pos); + ZLOGI(TAG, "arm_jiaxian_duoji_clamp_pos %d", cfg.arm_jiaxian_duoji_clamp_pos); + ZLOGI(TAG, "arm_jiaxian_duoji_clamp_torque %d", cfg.arm_jiaxian_duoji_clamp_torque); + ZLOGI(TAG, "scissors_reset_pos %d", cfg.scissors_reset_pos); + ZLOGI(TAG, "scissors_cut_pos %d", cfg.scissors_cut_pos); + ZLOGI(TAG, "arm_hook_claws_half_pos %d", cfg.arm_hook_claws_half_pos); + ZLOGI(TAG, "arm_hook_claws_full_pos %d", cfg.arm_hook_claws_full_pos); + return 0; +} +#if 1 int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse) { m_dm = dm; m_cmdparse = cmdparse; @@ -82,7 +287,12 @@ int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdPa cmdparse->regCMD("paifei_duoji_moveto_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_reset(); }); cmdparse->regCMD("paifei_duoji_moveto_press", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_press(); }); cmdparse->regCMD("raoxiantance_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return raoxiantance_duoji_move_to_reset(); }); - cmdparse->regCMD("raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return raoxiantance_duoji_move_to_get_thickness(); }); + cmdparse->regCMD("raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + int32_t thickness = 0; + int32_t err = raoxiantance_duoji_move_to_get_thickness(ack->getAck(1)); + ack->acktype = ICmdParserACK::kAckType_int32; + ack->rawlen = sizeof(int32_t); + }); cmdparse->regCMD("yaxian_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_reset(); }); cmdparse->regCMD("yaxian_duoji_move_to_press_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_press_pos(); }); cmdparse->regCMD("yaxian_duoji_move_to_wait_for_press_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_wait_for_press_pos(); }); @@ -116,3 +326,4 @@ int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdPa return 0; } +#endif \ No newline at end of file diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index 20aa28b..f8818c7 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -84,6 +84,7 @@ class IntelligentWindingRobotCtrl { ZModuleDeviceManager* m_dm; ICmdParser* m_cmdparse; + config_t cfg; public: int32_t initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse); @@ -95,7 +96,7 @@ class IntelligentWindingRobotCtrl { * @brief 绕线探测舵机 */ int32_t raoxiantance_duoji_move_to_reset(); - int32_t raoxiantance_duoji_move_to_get_thickness(); + int32_t raoxiantance_duoji_move_to_get_thickness(int32_t *thickness); /** * @brief 压线舵机 */ @@ -156,6 +157,10 @@ class IntelligentWindingRobotCtrl { int32_t setcfg(const char* cfgname, int32_t cfgvalue); int32_t dumpcfg(); + + void processError(int32_t err); + + void wait_module_idle(int32_t moduleid); }; } // namespace iflytop