diff --git a/.cproject b/.cproject index b6f58c3..586c96c 100644 --- a/.cproject +++ b/.cproject @@ -27,6 +27,7 @@ - diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index 08c9439..ef63981 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + diff --git a/sdk b/sdk index 74b48f0..d4c0048 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit 74b48f099c773a5cd6d4f402a671746792e57851 +Subproject commit d4c0048415ea76a2baf4cf731b7ae7d97298b22d diff --git a/usrc/app_zmodule_device_manager.cpp b/usrc/app_zmodule_device_manager.cpp new file mode 100644 index 0000000..633d760 --- /dev/null +++ b/usrc/app_zmodule_device_manager.cpp @@ -0,0 +1,193 @@ +#include "app_zmodule_device_manager.hpp" + +#include "sdk/os/zos.hpp" + +#define TAG "APPDM" + +#define DM ZModuleDeviceManager + +using namespace iflytop; +void APPDM::initialize(IZcanCmderMaster *m_cancmder) { // + DM::initialize(m_cancmder); +} + +void APPDM::registerModule(ZIModule *module) { DM::registerModule(module); } +/******************************************************************************* + * ZIModule * + *******************************************************************************/ + +#define DOCMD(exptr) \ + { \ + int32_t errcode = exptr; \ + if (errcode != 0) { \ + ZLOGE(TAG, "do %s fail,errcode %s(%d)", #exptr, err::error2str(errcode), errcode); \ + } \ + return errcode; \ + } + +int32_t APPDM::module_stop(uint16_t id) { // + ZLOGI(TAG, "module_stop %d", id); + DOCMD(DM::module_stop(id)); +} +int32_t APPDM::module_break(uint16_t id) { // + + ZLOGI(TAG, "module_break %d", id); + DOCMD(DM::module_break(id)); +} + +int32_t APPDM::module_get_last_exec_status(uint16_t id, int32_t *status) { // + DOCMD(DM::module_get_last_exec_status(id, status)); +} +int32_t APPDM::module_get_status(uint16_t id, int32_t *status) { // + DOCMD(DM::module_get_status(id, status)); +} + +int32_t APPDM::module_set_param(uint16_t id, int32_t param_id, int32_t param_value) { // + ZLOGI(TAG, "module_set_param %d %d", id, param_value); + DOCMD(DM::module_set_param(id, param_id, param_value)); +} +int32_t APPDM::module_get_param(uint16_t id, int32_t param_id, int32_t *param_value) { // + DOCMD(DM::module_get_param(id, param_id, param_value)); +} + +int32_t APPDM::module_readio(uint16_t id, int32_t *io) { // + DOCMD(DM::module_readio(id, io)); +} +int32_t APPDM::module_writeio(uint16_t id, int32_t io) { // + + ZLOGI(TAG, "module_writeio %d %d", id, io); + DOCMD(DM::module_writeio(id, io)); +} + +int32_t APPDM::module_read_adc(uint16_t id, int32_t adcindex, int32_t *adc) { // + DOCMD(DM::module_read_adc(id, adcindex, adc)); +} + +int32_t APPDM::module_get_error(uint16_t id, int32_t *iserror) { // + DOCMD(DM::module_get_error(id, iserror)); +} +int32_t APPDM::module_clear_error(uint16_t id) { // + ZLOGI(TAG, "module_clear_error %d", id); + DOCMD(DM::module_clear_error(id)); +} + +int32_t APPDM::module_set_inited_flag(uint16_t id, int32_t flag) { // + ZLOGI(TAG, "module_set_inited_flag %d %d", id, flag); + DOCMD(DM::module_set_inited_flag(id, flag)); +} +int32_t APPDM::module_get_inited_flag(uint16_t id, int32_t *flag) { // + DOCMD(DM::module_get_inited_flag(id, flag)); +} + +int32_t APPDM::module_factory_reset(uint16_t id) { // + ZLOGI(TAG, "module_factory_reset %d", id); + DOCMD(DM::module_factory_reset(id)); +} +int32_t APPDM::module_flush_cfg(uint16_t id) { // + ZLOGI(TAG, "module_flush_cfg %d", id); + DOCMD(DM::module_flush_cfg(id)); +} +int32_t APPDM::module_active_cfg(uint16_t id) { // + ZLOGI(TAG, "module_active_cfg %d", id); + DOCMD(DM::module_active_cfg(id)); +} + +int32_t APPDM::module_set_state(uint16_t id, int32_t state_id, int32_t state_value) { // + ZLOGI(TAG, "module_set_state %d %d %d", id, state_id, state_value); + DOCMD(DM::module_set_state(id, state_id, state_value)); +} +int32_t APPDM::module_get_state(uint16_t id, int32_t state_id, int32_t *state_value) { // + DOCMD(DM::module_get_state(id, state_id, state_value)); +} + +/******************************************************************************* + * ZIMotor * + *******************************************************************************/ + +int32_t APPDM::motor_enable(uint16_t id, int32_t enable) { // + ZLOGI(TAG, "motor_enable %d %d", id, enable); + DOCMD(DM::motor_enable(id, enable)); +} +int32_t APPDM::motor_rotate(uint16_t id, int32_t direction, int32_t motor_velocity, int32_t acc) { // + ZLOGI(TAG, "motor_rotate %d %d %d %d", id, direction, motor_velocity, acc); + DOCMD(DM::motor_rotate(id, direction, motor_velocity, acc)); +} +int32_t APPDM::motor_move_by(uint16_t id, int32_t distance, int32_t motor_velocity, int32_t acc) { // + ZLOGI(TAG, "motor_move_by %d %d %d %d", id, distance, motor_velocity, acc); + DOCMD(DM::motor_move_by(id, distance, motor_velocity, acc)); +} +int32_t APPDM::motor_move_to(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acc) { // + ZLOGI(TAG, "motor_move_to %d %d %d %d", id, position, motor_velocity, acc); + DOCMD(DM::motor_move_to(id, position, motor_velocity, acc)); +} +int32_t APPDM::motor_rotate_acctime(uint16_t id, int32_t direction, int32_t motor_velocity, int32_t acctime) { // + ZLOGI(TAG, "motor_rotate_acctime %d %d %d %d", id, direction, motor_velocity, acctime); + DOCMD(DM::motor_rotate_acctime(id, direction, motor_velocity, acctime)); +} +int32_t APPDM::motor_move_by_acctime(uint16_t id, int32_t distance, int32_t motor_velocity, int32_t acctime) { // + ZLOGI(TAG, "motor_move_by_acctime %d %d %d %d", id, distance, motor_velocity, acctime); + DOCMD(DM::motor_move_by_acctime(id, distance, motor_velocity, acctime)); +} +int32_t APPDM::motor_move_to_acctime(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acctime) { // + ZLOGI(TAG, "motor_move_to_acctime %d %d %d %d", id, position, motor_velocity, acctime); + DOCMD(DM::motor_move_to_acctime(id, position, motor_velocity, acctime)); +} +int32_t APPDM::motor_move_to_zero_forward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { // + ZLOGI(TAG, "motor_move_to_zero_forward %d %d %d %d %d", id, findzerospeed, findzeroedge_speed, acc, overtime); + DOCMD(DM::motor_move_to_zero_forward(id, findzerospeed, findzeroedge_speed, acc, overtime)); +} +int32_t APPDM::motor_move_to_zero_backward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { // + ZLOGI(TAG, "motor_move_to_zero_backward %d %d %d %d %d", id, findzerospeed, findzeroedge_speed, acc, overtime); + DOCMD(DM::motor_move_to_zero_backward(id, findzerospeed, findzeroedge_speed, acc, overtime)); +} +int32_t APPDM::motor_move_to_with_torque(uint16_t id, int32_t direction, int32_t torque) { // + ZLOGI(TAG, "motor_move_to_with_torque %d %d %d", id, direction, torque); + DOCMD(DM::motor_move_to_with_torque(id, direction, torque)); +} +int32_t APPDM::motor_read_pos(uint16_t id, int32_t *pos) { // + DOCMD(DM::motor_read_pos(id, pos)); +} +int32_t APPDM::motor_set_current_pos_by_change_shift(uint16_t id, int32_t pos) { // + ZLOGI(TAG, "motor_set_current_pos_by_change_shift %d %d", id, pos); + DOCMD(DM::motor_set_current_pos_by_change_shift(id, pos)); +} +int32_t APPDM::motor_move_to_zero_forward_and_calculated_shift(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { // + ZLOGI(TAG, "motor_move_to_zero_forward_and_calculated_shift %d %d %d %d %d", id, findzerospeed, findzeroedge_speed, acc, overtime); + DOCMD(DM::motor_move_to_zero_forward_and_calculated_shift(id, findzerospeed, findzeroedge_speed, acc, overtime)); +} +int32_t APPDM::motor_move_to_zero_backward_and_calculated_shift(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) { // + ZLOGI(TAG, "motor_move_to_zero_backward_and_calculated_shift %d %d %d %d %d", id, findzerospeed, findzeroedge_speed, acc, overtime); + DOCMD(DM::motor_move_to_zero_backward_and_calculated_shift(id, findzerospeed, findzeroedge_speed, acc, overtime)); +} + +/******************************************************************************* + * ZIXYMotor * + *******************************************************************************/ + +int32_t APPDM::xymotor_enable(uint16_t id, int32_t enable) { // + ZLOGI(TAG, "xymotor_enable %d %d", id, enable); + DOCMD(DM::xymotor_enable(id, enable)); +} +int32_t APPDM::xymotor_move_by(uint16_t id, int32_t dx, int32_t dy, int32_t motor_velocity) { // + ZLOGI(TAG, "xymotor_move_by %d %d %d %d", id, dx, dy, motor_velocity); + DOCMD(DM::xymotor_move_by(id, dx, dy, motor_velocity)); +} +int32_t APPDM::xymotor_move_to(uint16_t id, int32_t x, int32_t y, int32_t motor_velocity) { // + ZLOGI(TAG, "xymotor_move_to %d %d %d %d", id, x, y, motor_velocity); + DOCMD(DM::xymotor_move_to(id, x, y, motor_velocity)); +} +int32_t APPDM::xymotor_move_to_zero(uint16_t id) { // + ZLOGI(TAG, "xymotor_move_to_zero %d", id); + DOCMD(DM::xymotor_move_to_zero(id)); +} +int32_t APPDM::xymotor_move_to_zero_and_calculated_shift(uint16_t id) { // + ZLOGI(TAG, "xymotor_move_to_zero_and_calculated_shift %d", id); + DOCMD(DM::xymotor_move_to_zero_and_calculated_shift(id)); +} +int32_t APPDM::xymotor_read_pos(uint16_t id, int32_t *x, int32_t *y) { // + DOCMD(DM::xymotor_read_pos(id, x, y)); +} +int32_t APPDM::xymotor_calculated_pos_by_move_to_zero(uint16_t id) { // + ZLOGI(TAG, "xymotor_calculated_pos_by_move_to_zero %d", id); + DOCMD(DM::xymotor_calculated_pos_by_move_to_zero(id)); +} \ No newline at end of file diff --git a/usrc/app_zmodule_device_manager.hpp b/usrc/app_zmodule_device_manager.hpp new file mode 100644 index 0000000..2286651 --- /dev/null +++ b/usrc/app_zmodule_device_manager.hpp @@ -0,0 +1,78 @@ +#pragma once +#include + +#include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp" + +namespace iflytop { +class APPDM : public ZModuleDeviceManager { + private: + + public: + void initialize(IZcanCmderMaster *m_cancmder); + void registerModule(ZIModule *module); + + /******************************************************************************* + * ZIModule * + *******************************************************************************/ + virtual int32_t module_stop(uint16_t id) override; + virtual int32_t module_break(uint16_t id) override; + + virtual int32_t module_get_last_exec_status(uint16_t id, int32_t *status) override; + virtual int32_t module_get_status(uint16_t id, int32_t *status) override; + + virtual int32_t module_set_param(uint16_t id, int32_t param_id, int32_t param_value) override; + virtual int32_t module_get_param(uint16_t id, int32_t param_id, int32_t *param_value) override; + + virtual int32_t module_readio(uint16_t id, int32_t *io) override; + virtual int32_t module_writeio(uint16_t id, int32_t io) override; + + virtual int32_t module_read_adc(uint16_t id, int32_t adcindex, int32_t *adc) override; + + virtual int32_t module_get_error(uint16_t id, int32_t *iserror) override; + virtual int32_t module_clear_error(uint16_t id) override; + + virtual int32_t module_set_inited_flag(uint16_t id, int32_t flag) override; + virtual int32_t module_get_inited_flag(uint16_t id, int32_t *flag) override; + + virtual int32_t module_factory_reset(uint16_t id) override; + virtual int32_t module_flush_cfg(uint16_t id) override; + virtual int32_t module_active_cfg(uint16_t id) override; + + virtual int32_t module_set_state(uint16_t id, int32_t state_id, int32_t state_value) override; + virtual int32_t module_get_state(uint16_t id, int32_t state_id, int32_t *state_value) override; + + /******************************************************************************* + * ZIMotor * + *******************************************************************************/ + + virtual int32_t motor_enable(uint16_t id, int32_t enable) override; + virtual int32_t motor_rotate(uint16_t id, int32_t direction, int32_t motor_velocity, int32_t acc) override; + virtual int32_t motor_move_by(uint16_t id, int32_t distance, int32_t motor_velocity, int32_t acc) override; + virtual int32_t motor_move_to(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acc) override; + virtual int32_t motor_rotate_acctime(uint16_t id, int32_t direction, int32_t motor_velocity, int32_t acctime) override; + virtual int32_t motor_move_by_acctime(uint16_t id, int32_t distance, int32_t motor_velocity, int32_t acctime) override; + virtual int32_t motor_move_to_acctime(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acctime) override; + virtual int32_t motor_move_to_zero_forward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; + virtual int32_t motor_move_to_zero_backward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; + virtual int32_t motor_move_to_with_torque(uint16_t id, int32_t direction, int32_t torque) override; + virtual int32_t motor_read_pos(uint16_t id, int32_t *pos) override; + virtual int32_t motor_set_current_pos_by_change_shift(uint16_t id, int32_t pos) override; + virtual int32_t motor_move_to_zero_forward_and_calculated_shift(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; + virtual int32_t motor_move_to_zero_backward_and_calculated_shift(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override; + + /******************************************************************************* + * ZIXYMotor * + *******************************************************************************/ + + virtual int32_t xymotor_enable(uint16_t id, int32_t enable) override; + virtual int32_t xymotor_move_by(uint16_t id, int32_t dx, int32_t dy, int32_t motor_velocity) override; + virtual int32_t xymotor_move_to(uint16_t id, int32_t x, int32_t y, int32_t motor_velocity) override; + virtual int32_t xymotor_move_to_zero(uint16_t id) override; + virtual int32_t xymotor_move_to_zero_and_calculated_shift(uint16_t id) override; + virtual int32_t xymotor_read_pos(uint16_t id, int32_t *x, int32_t *y) override; + virtual int32_t xymotor_calculated_pos_by_move_to_zero(uint16_t id) override; + + private: +}; + +} // namespace iflytop \ No newline at end of file diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index 7ef9367..4a8e2ed 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -1,11 +1,14 @@ #include "intelligent_winding_robot_ctrl.hpp" +#include #include #include + +#include using namespace std; using namespace iflytop; -#define TAG "IntelligentWindingRobotCtrl" +#define TAG "APPDM" #define DO(exptr) \ { \ @@ -15,8 +18,97 @@ using namespace iflytop; } \ } +#define PARAM int32_t paramN, const char **paraV, ICmdParserACK *ack + void IntelligentWindingRobotCtrl::processError(int32_t err) { ZLOGE(TAG, "processError: %d", err); } +class myexception : public exception { + virtual const char* what() const throw() { return "My exception happened"; } +}; + +int32_t IntelligentWindingRobotCtrl::initialize(APPDM* dm, ICmdParser* cmdparse) { + m_dm = dm; + m_cmdparse = cmdparse; + + initialize_device(); + regcb(); + return 0; +} + +int32_t IntelligentWindingRobotCtrl::initialize_device() { + cfg.xy_platform_takeline_clip00_pos_x = 1736; + cfg.xy_platform_takeline_clip00_pos_y = 16853; + cfg.xy_platform_takeline_clipXX_pos_x = 52307; + cfg.xy_platform_takeline_clipXX_pos_y = 31993; + cfg.clip_line = 12; + cfg.clip_each_line_num = 5; + + cfg.m11_arm_jiaxian_reset_pos = 2619; + cfg.m11_arm_jiaxian_clamp_torque = 330; + cfg.m11_arm_jiaxian_clamp_direction = -1; + + cfg.m12_jiaxian_reset_pos = 2600; + cfg.m12_jiaxian_clamp_direction = -1; + cfg.m12_jiaxian_clamp_torque = 330; + + cfg.m13_yaxian_forward_reset_pos = 1015; + cfg.m13_yaxian_backward_reset_pos = 2885; + cfg.m13_jiaxian_clamp_direction = -1; + cfg.m13_jiaxian_clamp_torque = 200; + + cfg.m14_raoxiantance_reset_pos = 2047; + cfg.m14_raoxiantance_tance_zero_pos = 2517; + + cfg.m15_paifei_reset_pos = 2046; + cfg.m15_paifei_press_direction = 1; + cfg.m15_paifei_press_torque = 330; + + cfg.m16_xianlajin_reset_pos = 2047; + cfg.m16_xianlajin_tight_line_pos = 1966; + cfg.m16_xianlajin_winding_low_pos = 1900; + cfg.m16_xianlajin_winding_up_pos = 1865; + cfg.m16_xianlajin_line_entry_pos = 1833; + + cfg.m21_arm_hook_claws_full_pos = 2558; + cfg.m21_arm_hook_claws_half_pos = 2294; + + cfg.z_axis_take_clip_pos = 6924; + + return 0; +} + +void IntelligentWindingRobotCtrl::regcb() { + // device_reset + m_cmdparse->regCMD("device_reset", "()", 0, [this](PARAM) { device_reset(); }); + m_cmdparse->regCMD("xy_run_to_clip_pos_test", "()", 1, [this](PARAM) { return xy_run_to_clip_pos_test(atoi(paraV[0])); }); + m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); }); + + m_cmdparse->regCMD("app_m11_arm_jiaxian_move_to_reset_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_reset_pos(); }); + m_cmdparse->regCMD("app_m11_arm_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_clamp_pos(); }); + m_cmdparse->regCMD("app_m12_jiaxian_move_to_reset_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_reset_pos(); }); + m_cmdparse->regCMD("app_m12_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_clamp_pos(); }); + m_cmdparse->regCMD("app_m13_yaxian_move_to_reset_forward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_forward(); }); + m_cmdparse->regCMD("app_m13_yaxian_move_to_reset_backward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_backward(); }); + m_cmdparse->regCMD("app_m13_yaxian_press_clip", "()", 0, [this](PARAM) { return m13_yaxian_press_clip(); }); + m_cmdparse->regCMD("app_m14_raoxiantance_move_to_reset", "()", 0, [this](PARAM) { return m14_raoxiantance_move_to_reset(); }); + m_cmdparse->regCMD("app_m15_paifei_moveto_reset", "()", 0, [this](PARAM) { return m15_paifei_moveto_reset(); }); + m_cmdparse->regCMD("app_m15_paifei_moveto_press", "()", 0, [this](PARAM) { return m15_paifei_moveto_press(); }); + m_cmdparse->regCMD("app_m16_xianlajin_move_to_reset", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_reset(); }); + m_cmdparse->regCMD("app_m16_xianlajin_move_to_tight_line_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_tight_line_pos(); }); + m_cmdparse->regCMD("app_m16_xianlajin_move_to_winding_low_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_winding_low_pos(); }); + m_cmdparse->regCMD("app_m16_xianlajin_move_to_winding_up_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_winding_up_pos(); }); + m_cmdparse->regCMD("app_m16_xianlajin_move_to_line_entry_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_line_entry_pos(); }); + + m_cmdparse->regCMD("app_m21_arm_hook_claws_reset", "()", 0, [this](PARAM) { return m21_arm_hook_claws_reset(); }); + m_cmdparse->regCMD("app_m21_arm_hook_claws_move_to_half_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_half_pos(); }); + m_cmdparse->regCMD("app_m21_arm_hook_claws_move_to_full_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_full_pos(); }); + + m_cmdparse->regCMD("app_m22_scissors_move_reset_pos", "()", 0, [this](PARAM) { return m22_scissors_move_reset_pos(); }); + m_cmdparse->regCMD("app_m22_scissors_cut", "()", 0, [this](PARAM) { return m22_scissors_cut(); }); + m_cmdparse->regCMD("app_m23_laxian_motor_move_to_reset_pos", "()", 0, [this](PARAM) { return m23_laxian_motor_move_to_reset_pos(); }); + m_cmdparse->regCMD("app_m23_laxian_motor_move_to_tight_line_pos", "()", 0, [this](PARAM) { return m23_laxian_motor_move_to_tight_line_pos(); }); +} + void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) { zos_delay(100); int i = 0; @@ -59,160 +151,33 @@ int32_t IntelligentWindingRobotCtrl::device_reset() { // Z轴复位 zreset(); m21_arm_hook_claws_reset(); - m11_arm_jiaxian_duoji_move_to_reset_pos(); - + m11_arm_jiaxian_move_to_reset_pos(); xy_reset(); ZLOGI(TAG, "device_reset finished...."); } -int32_t IntelligentWindingRobotCtrl::initialize_device() { return 0; } -// 排废舵机 -int32_t IntelligentWindingRobotCtrl::m15_paifei_duoji_moveto_reset() { - ZLOGI(TAG, "m15_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::m15_paifei_duoji_moveto_press() { - ZLOGI(TAG, "m15_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::m14_raoxiantance_duoji_move_to_reset() { - ZLOGI(TAG, "m14_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::m14_raoxiantance_duoji_move_to_get_thickness(int32_t* thickness) { - ZLOGI(TAG, "m14_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(m14_raoxiantance_duoji_move_to_reset()); - return 0; -} -/** - * @brief 压线舵机 - */ -int32_t IntelligentWindingRobotCtrl::m13_yaxian_duoji_move_to_reset() { - ZLOGI(TAG, "m13_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::m13_yaxian_duoji_move_to_press_pos() { - ZLOGI(TAG, "m13_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::m13_yaxian_duoji_move_to_wait_for_press_pos() { - ZLOGI(TAG, "m13_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::m16_xianlajin_duoji_move_to_reset() { - ZLOGI(TAG, "m16_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::m16_xianlajin_duoji_move_to_line_entry_pos() { - ZLOGI(TAG, "m16_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::m16_xianlajin_duoji_move_to_tight_line_pos() { - ZLOGI(TAG, "m16_xianlajin_duoji_move_to_line_entry_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::m16_xianlajin_duoji_move_to_loose_line_pos() { - ZLOGI(TAG, "m16_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 夹线舵机12 - */ -int32_t IntelligentWindingRobotCtrl::m12_jiaxian_duoji_move_to_reset_pos() { - ZLOGI(TAG, "m12_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::m12_jiaxian_duoji_move_to_clamp_pos() { - ZLOGI(TAG, "m12_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::m22_scissors_move_reset_pos() { - // ZLOGI(TAG, "m22_scissors_move_reset_pos %d", 11); - ZLOGI(TAG, "m22_scissors_move_reset_pos"); - return 0; -} // block -int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { - ZLOGI(TAG, "m22_scissors_cut %d", 22); - DO(m_dm->motor_move_by(22, 4095, 0, 0)); - wait_module_idle(22); - return 0; -} // block -/** - * @brief 机械臂夹线舵机 - */ -int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_duoji_move_to_reset_pos() { - ZLOGI(TAG, "m11_arm_jiaxian_duoji_move_to_reset_pos"); - DO(m_dm->motor_move_to(11, cfg.arm_jiaxian_duoji_reset_pos, 2000, 0)); - wait_module_idle(11); - return 0; -} -int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_duoji_move_to_clamp_pos() { - ZLOGI(TAG, "m11_arm_jiaxian_duoji_move_to_clamp_pos"); - DO(m_dm->motor_move_to_with_torque(11, cfg.arm_jiaxian_duoji_clamp_direction, cfg.arm_jiaxian_duoji_clamp_torque)); - wait_module_idle(11); - return 0; -} - -/** - * @brief 机械臂钩爪 - */ -int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_reset() { - ZLOGI(TAG, "m21_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::m21_arm_hook_claws_move_to_half_pos() { - ZLOGI(TAG, "m21_arm_hook_claws_move_to_half_pos"); - DO(m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_half_pos, 0, 0)); - wait_module_idle(21); - return 0; -} -int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_full_pos() { - ZLOGI(TAG, "m21_arm_hook_claws_move_to_full_pos"); - DO(m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_full_pos, 0, 0)); - wait_module_idle(21); - return 0; -} +int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to(11, cfg.m11_arm_jiaxian_reset_pos, 2000, 0); } +int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_clamp_pos() { return m_dm->motor_move_to_with_torque(11, cfg.m11_arm_jiaxian_clamp_direction, cfg.m11_arm_jiaxian_clamp_torque); } +int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to(12, cfg.m12_jiaxian_reset_pos, 2000, 0); } +int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_clamp_pos() { return m_dm->motor_move_to_with_torque(12, cfg.m12_jiaxian_clamp_direction, cfg.m12_jiaxian_clamp_torque); } +int32_t IntelligentWindingRobotCtrl::m13_yaxian_move_to_reset_forward() { return m_dm->motor_move_to(13, cfg.m13_yaxian_forward_reset_pos, 2000, 0); } +int32_t IntelligentWindingRobotCtrl::m13_yaxian_move_to_reset_backward() { return m_dm->motor_move_to(13, cfg.m13_yaxian_backward_reset_pos, 2000, 0); } +int32_t IntelligentWindingRobotCtrl::m13_yaxian_press_clip() { return m_dm->motor_move_to_with_torque(13, cfg.m13_jiaxian_clamp_direction, cfg.m13_jiaxian_clamp_torque); } +int32_t IntelligentWindingRobotCtrl::m14_raoxiantance_move_to_reset() { return m_dm->motor_move_to(14, cfg.m14_raoxiantance_reset_pos, 2000, 0); } +int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_reset() { return m_dm->motor_move_to(15, cfg.m15_paifei_reset_pos, 2000, 0); } +int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_press() { return m_dm->motor_move_to_with_torque(15, cfg.m15_paifei_press_direction, cfg.m15_paifei_press_torque); } +int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_reset() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_reset_pos, 300, 0); } +int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_tight_line_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_tight_line_pos, 100, 0); } +int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_winding_low_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_winding_low_pos, 100, 0); } +int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_winding_up_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_winding_up_pos, 100, 0); } +int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_line_entry_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_line_entry_pos, 100, 0); } +int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_reset() { return m_dm->motor_move_to_zero_backward(21, 0, 0, 0, 0); } +int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_half_pos() { return m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_half_pos, 0, 0); } +int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_full_pos() { return m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_full_pos, 0, 0); } +int32_t IntelligentWindingRobotCtrl::m22_scissors_move_reset_pos() { return 0; } +int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { return m_dm->motor_move_to_with_torque(22, 1, 4095); } +int32_t IntelligentWindingRobotCtrl::m23_laxian_motor_move_to_reset_pos() { return m_dm->motor_move_to_zero_backward(23, 0, 0, 0, 0); } +int32_t IntelligentWindingRobotCtrl::m23_laxian_motor_move_to_tight_line_pos() { return m_dm->motor_move_to(23, 500, 0, 0); } int32_t IntelligentWindingRobotCtrl::main_shaft_run() { ZLOGI(TAG, "main_shaft_run"); @@ -324,7 +289,7 @@ int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index) return err::kparam_out_of_range; } zreset(); - m11_arm_jiaxian_duoji_move_to_reset_pos(); + m11_arm_jiaxian_move_to_reset_pos(); m21_arm_hook_claws_reset(); int32_t x = 0; @@ -342,44 +307,44 @@ int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index) int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) { #if 0 - 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; + if (strcmp(cfgname, "paifei_reset_pos") == 0) + cfg.paifei_reset_pos = cfgvalue; + else if (strcmp(cfgname, "paifei_press_pos") == 0) + cfg.paifei_press_pos = cfgvalue; + else if (strcmp(cfgname, "paifei_press_torque") == 0) + cfg.paifei_press_torque = cfgvalue; + else if (strcmp(cfgname, "raoxiantance_reset_pos") == 0) + cfg.raoxiantance_reset_pos = cfgvalue; + else if (strcmp(cfgname, "raoxiantance_tance_zero_pos") == 0) + cfg.raoxiantance_tance_zero_pos = cfgvalue; + else if (strcmp(cfgname, "yaxian_reset_pos") == 0) + cfg.yaxian_reset_pos = cfgvalue; + else if (strcmp(cfgname, "yaxian_press_pos") == 0) + cfg.yaxian_press_pos = cfgvalue; + else if (strcmp(cfgname, "yaxian_press_torque") == 0) + cfg.yaxian_press_torque = cfgvalue; + else if (strcmp(cfgname, "yaxian_wait_for_press_pos") == 0) + cfg.yaxian_wait_for_press_pos = cfgvalue; + else if (strcmp(cfgname, "xianlajin_reset_pos") == 0) + cfg.xianlajin_reset_pos = cfgvalue; + else if (strcmp(cfgname, "xianlajin_line_entry_pos") == 0) + cfg.xianlajin_line_entry_pos = cfgvalue; + else if (strcmp(cfgname, "xianlajin_tight_line_pos") == 0) + cfg.xianlajin_tight_line_pos = cfgvalue; + else if (strcmp(cfgname, "xianlajin_loose_line_pos") == 0) + cfg.xianlajin_loose_line_pos = cfgvalue; + else if (strcmp(cfgname, "jiaxian_reset_pos") == 0) + cfg.jiaxian_reset_pos = cfgvalue; + else if (strcmp(cfgname, "jiaxian_clamp_pos") == 0) + cfg.jiaxian_clamp_pos = cfgvalue; + else if (strcmp(cfgname, "jiaxian_clamp_torque") == 0) + cfg.jiaxian_clamp_torque = cfgvalue; + else if (strcmp(cfgname, "arm_jiaxian_reset_pos") == 0) + cfg.arm_jiaxian_reset_pos = cfgvalue; + else if (strcmp(cfgname, "arm_jiaxian_clamp_pos") == 0) + cfg.arm_jiaxian_clamp_pos = cfgvalue; + else if (strcmp(cfgname, "arm_jiaxian_clamp_torque") == 0) + cfg.arm_jiaxian_clamp_torque = cfgvalue; else if (strcmp(cfgname, "scissors_reset_pos") == 0) cfg.scissors_reset_pos = cfgvalue; else if (strcmp(cfgname, "m22_scissors_cut_pos") == 0) @@ -392,109 +357,8 @@ int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalu #endif return 0; } -int32_t IntelligentWindingRobotCtrl::dumpcfg() { -#if 0 - 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, "m22_scissors_cut_pos %d", cfg.m22_scissors_cut_pos); - ZLOGI(TAG, "m21_arm_hook_claws_half_pos %d", cfg.m21_arm_hook_claws_half_pos); - ZLOGI(TAG, "m21_arm_hook_claws_full_pos %d", cfg.m21_arm_hook_claws_full_pos); -#endif - return 0; -} +int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; } #if 1 -int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse) { - m_dm = dm; - m_cmdparse = cmdparse; - - cfg.xy_platform_takeline_clip00_pos_x = 1736; - cfg.xy_platform_takeline_clip00_pos_y = 16853; - cfg.xy_platform_takeline_clipXX_pos_x = 52307; - cfg.xy_platform_takeline_clipXX_pos_y = 31993; - cfg.clip_line = 12; - cfg.clip_each_line_num = 5; - - cfg.arm_jiaxian_duoji_reset_pos = 2619; - cfg.arm_jiaxian_duoji_clamp_torque = 330; - cfg.arm_jiaxian_duoji_clamp_direction = -1; - - cfg.z_axis_take_clip_pos = 6924; - cfg.m21_arm_hook_claws_full_pos = 2458; - cfg.m21_arm_hook_claws_half_pos = 2294; - - regcb(); - return 0; -} -#endif - -#define PARAM int32_t paramN, const char **paraV, ICmdParserACK *ack - -void IntelligentWindingRobotCtrl::regcb() { - // device_reset - m_cmdparse->regCMD("device_reset", "()", 0, [this](PARAM) { return device_reset(); }); - m_cmdparse->regCMD("xy_run_to_clip_pos_test", "()", 1, [this](PARAM) { return xy_run_to_clip_pos_test(atoi(paraV[0])); }); - m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); }); -#if 0 - m_cmdparse->regCMD("app_m15_paifei_duoji_moveto_reset", "()", 0, [this](PARAM) { return m15_paifei_duoji_moveto_reset(); }); - m_cmdparse->regCMD("app_m15_paifei_duoji_moveto_press", "()", 0, [this](PARAM) { return m15_paifei_duoji_moveto_press(); }); - m_cmdparse->regCMD("app_m14_raoxiantance_duoji_move_to_reset", "()", 0, [this](PARAM) { return m14_raoxiantance_duoji_move_to_reset(); }); - m_cmdparse->regCMD("app_m14_raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](PARAM) { - int32_t thickness = 0; - int32_t err = m14_raoxiantance_duoji_move_to_get_thickness(ack->getAck(1)); - ack->acktype = ICmdParserACK::kAckType_int32; - ack->rawlen = sizeof(int32_t); - }); - m_cmdparse->regCMD("app_m13_yaxian_duoji_move_to_reset", "()", 0, [this](PARAM) { return m13_yaxian_duoji_move_to_reset(); }); - m_cmdparse->regCMD("app_m13_yaxian_duoji_move_to_press_pos", "()", 0, [this](PARAM) { return m13_yaxian_duoji_move_to_press_pos(); }); - m_cmdparse->regCMD("app_m13_yaxian_duoji_move_to_wait_for_press_pos", "()", 0, [this](PARAM) { return m13_yaxian_duoji_move_to_wait_for_press_pos(); }); - m_cmdparse->regCMD("app_m16_xianlajin_duoji_move_to_reset", "()", 0, [this](PARAM) { return m16_xianlajin_duoji_move_to_reset(); }); - m_cmdparse->regCMD("app_m16_xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](PARAM) { return m16_xianlajin_duoji_move_to_line_entry_pos(); }); - m_cmdparse->regCMD("app_m16_xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](PARAM) { return m16_xianlajin_duoji_move_to_line_entry_pos(); }); - m_cmdparse->regCMD("app_m16_xianlajin_duoji_move_to_loose_line_pos", "()", 0, [this](PARAM) { return m16_xianlajin_duoji_move_to_loose_line_pos(); }); - m_cmdparse->regCMD("app_m12_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](PARAM) { return m12_jiaxian_duoji_move_to_reset_pos(); }); - m_cmdparse->regCMD("app_m12_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](PARAM) { return m12_jiaxian_duoji_move_to_clamp_pos(); }); - m_cmdparse->regCMD("app_m22_scissors_move_reset_pos", "()", 0, [this](PARAM) { return m22_scissors_move_reset_pos(); }); - m_cmdparse->regCMD("app_m22_scissors_cut", "()", 0, [this](PARAM) { return m22_scissors_cut(); }); - m_cmdparse->regCMD("app_m11_arm_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_duoji_move_to_reset_pos(); }); - m_cmdparse->regCMD("app_m11_arm_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_duoji_move_to_clamp_pos(); }); - m_cmdparse->regCMD("app_m21_arm_hook_claws_reset", "()", 0, [this](PARAM) { return m21_arm_hook_claws_reset(); }); - m_cmdparse->regCMD("app_m21_arm_hook_claws_move_to_half_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_half_pos(); }); - m_cmdparse->regCMD("app_m21_arm_hook_claws_move_to_full_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_full_pos(); }); - m_cmdparse->regCMD("app_main_shaft_run", "()", 0, [this](PARAM) { return main_shaft_run(); }); - m_cmdparse->regCMD("app_main_shaft_stop", "()", 0, [this](PARAM) { return main_shaft_stop(); }); - m_cmdparse->regCMD("app_xy_platform_reset", "()", 0, [this](PARAM) { return xy_platform_reset(); }); - m_cmdparse->regCMD("app_z_axis_reset", "()", 0, [this](PARAM) { return z_axis_reset(); }); - m_cmdparse->regCMD("app_z_axis_move_to", "(int32_t pos)", 1, [this](PARAM) { return z_axis_move_to(atoi(paraV[0])); }); - m_cmdparse->regCMD("app_xy_reset", "()", 0, [this](PARAM) { return xy_reset(); }); - m_cmdparse->regCMD("app_xy_move_to_zero", "()", 0, [this](PARAM) { return xy_move_to_zero(); }); - m_cmdparse->regCMD("app_xy_take_clip", "(int32_t index)", 1, [this](PARAM) { return xy_take_clip(atoi(paraV[0])); }); - m_cmdparse->regCMD("app_xy_take_line", "()", 0, [this](PARAM) { return xy_take_line(); }); - m_cmdparse->regCMD("app_xy_take_back_clip", "()", 0, [this](PARAM) { return xy_take_back_clip(); }); - m_cmdparse->regCMD("app_xy_remove_line", "()", 0, [this](PARAM) { return xy_remove_line(); }); - m_cmdparse->regCMD("app_start_winding", "()", 0, [this](PARAM) { return start_winding(); }); - m_cmdparse->regCMD("app_stop_winding", "()", 0, [this](PARAM) { return stop_winding(); }); - m_cmdparse->regCMD("app_reset_and_check_device", "()", 0, [this](PARAM) { return reset_and_check_device(); }); - m_cmdparse->regCMD("app_setcfg", "(const char* cfgname, int32_t cfgvalue)", 2, [this](PARAM) { return setcfg(paraV[0], atoi(paraV[1])); }); - m_cmdparse->regCMD("app_dumpcfg", "()", 0, [this](PARAM) { return dumpcfg(); }); #endif -} diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index 2237c4d..02cf5cc 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -3,6 +3,9 @@ #include "sdk\components\zprotocols\zcancmder_v2\api\api.hpp" #include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp" +// +#include "app_zmodule_device_manager.hpp" + #define ZMOTOR_ID 4 #define XYRobot_ID 3 #define XYRobot_HOOK_ID 21 @@ -14,59 +17,69 @@ class IntelligentWindingRobotCtrl { public: typedef struct { /** - * @brief 排废舵机 + * @brief 机械臂夹线舵机 */ - int32_t paifei_duoji_reset_pos; - int32_t paifei_duoji_press_pos; - int32_t paifei_duoji_press_torque; + int32_t m11_arm_jiaxian_reset_pos; + int32_t m11_arm_jiaxian_clamp_direction; + int32_t m11_arm_jiaxian_clamp_torque; /** - * @brief 绕线探测舵机 + * @brief 线夹紧舵机 */ - int32_t raoxiantance_duoji_reset_pos; - int32_t raoxiantance_duoji_tance_zero_pos; + int32_t m12_jiaxian_reset_pos; + int32_t m12_jiaxian_clamp_direction; + int32_t m12_jiaxian_clamp_torque; /** * @brief 压线舵机 */ - int32_t yaxian_duoji_reset_pos; - int32_t yaxian_duoji_press_pos; - int32_t yaxian_duoji_press_torque; - int32_t yaxian_duoji_wait_for_press_pos; - /** - * @brief 线拉紧舵机 + * 高度: + * 1. 没有弹夹 1015 + * 2. 有弹夹 1055 + * 3. 弹夹有壳 1110 + * */ - int32_t xianlajin_duoji_reset_pos; - int32_t xianlajin_duoji_line_entry_pos; - int32_t xianlajin_duoji_tight_line_pos; - int32_t xianlajin_duoji_loose_line_pos; + int32_t m13_yaxian_forward_reset_pos; // 压死待机位 + int32_t m13_yaxian_backward_reset_pos; // 反向待机位 + int32_t m13_jiaxian_clamp_direction; // 压线方向 + int32_t m13_jiaxian_clamp_torque; // 压线扭矩 /** - * @brief 线夹紧舵机 + * @brief 绕线探测舵机 */ - int32_t jiaxian_duoji_reset_pos; - int32_t jiaxian_duoji_clamp_pos; - int32_t jiaxian_duoji_clamp_torque; + int32_t m14_raoxiantance_reset_pos; + int32_t m14_raoxiantance_tance_zero_pos; /** - * @brief 机械臂夹线舵机 + * @brief 排废舵机 */ - int32_t arm_jiaxian_duoji_reset_pos; - int32_t arm_jiaxian_duoji_clamp_direction; - int32_t arm_jiaxian_duoji_clamp_torque; + int32_t m15_paifei_reset_pos; + int32_t m15_paifei_press_direction; + int32_t m15_paifei_press_torque; /** - * @brief 剪刀 + * @brief 线拉紧舵机 */ - int32_t scissors_reset_pos; - int32_t m22_scissors_cut_pos; + int32_t m16_xianlajin_reset_pos; + int32_t m16_xianlajin_tight_line_pos; + int32_t m16_xianlajin_winding_low_pos; + int32_t m16_xianlajin_winding_up_pos; + int32_t m16_xianlajin_line_entry_pos; /** * @brief 钩爪 */ int32_t m21_arm_hook_claws_half_pos; int32_t m21_arm_hook_claws_full_pos; + /** + * @brief 剪刀 + */ + // int32_t m22_scissors_cut_pos; + /** + * @brief 拉线 + */ + // int32_t m23_laxian_move_to_tight_pos; /** * @brief Z轴定位 @@ -95,69 +108,43 @@ class IntelligentWindingRobotCtrl { } config_t; - ZModuleDeviceManager* m_dm; - ICmdParser* m_cmdparse; - config_t cfg; + APPDM* m_dm; + ICmdParser* m_cmdparse; + config_t cfg; - private: - void wait_module_idle(int32_t moduleid); - void wait_modules_idle(...); + public: + int32_t initialize(APPDM* dm, ICmdParser* cmdparse); - /** - * @brief 机械臂夹线舵机 - */ - int32_t m11_arm_jiaxian_duoji_move_to_reset_pos(); - int32_t m11_arm_jiaxian_duoji_move_to_clamp_pos(); - /** - * @brief 夹线舵机 - */ - int32_t m12_jiaxian_duoji_move_to_reset_pos(); - int32_t m12_jiaxian_duoji_move_to_clamp_pos(); - /** - * @brief 压线舵机 - */ - int32_t m13_yaxian_duoji_move_to_reset(); - int32_t m13_yaxian_duoji_move_to_press_pos(); - int32_t m13_yaxian_duoji_move_to_wait_for_press_pos(); - /** - * @brief 绕线探测舵机 - */ - int32_t m14_raoxiantance_duoji_move_to_reset(); - int32_t m14_raoxiantance_duoji_move_to_get_thickness(int32_t* thickness); - // 排废舵机 - int32_t m15_paifei_duoji_moveto_reset(); - int32_t m15_paifei_duoji_moveto_press(); - /** - * @brief 线拉紧舵机 - */ - int32_t m16_xianlajin_duoji_move_to_reset(); // 零位 - int32_t m16_xianlajin_duoji_move_to_line_entry_pos(); // 入线位 - int32_t m16_xianlajin_duoji_move_to_tight_line_pos(); // 拉线位置 - int32_t m16_xianlajin_duoji_move_to_loose_line_pos(); // 放线位置 - /** - * @brief 机械臂钩爪 - */ + public: + void wait_module_idle(int32_t moduleid); + void wait_modules_idle(...); + int32_t m11_arm_jiaxian_move_to_reset_pos(); + int32_t m11_arm_jiaxian_move_to_clamp_pos(); + int32_t m12_jiaxian_move_to_reset_pos(); + int32_t m12_jiaxian_move_to_clamp_pos(); + int32_t m13_yaxian_move_to_reset_forward(); + int32_t m13_yaxian_move_to_reset_backward(); + int32_t m13_yaxian_press_clip(); + int32_t m14_raoxiantance_move_to_reset(); + int32_t m15_paifei_moveto_reset(); + int32_t m15_paifei_moveto_press(); + int32_t m16_xianlajin_move_to_reset(); + int32_t m16_xianlajin_move_to_tight_line_pos(); + int32_t m16_xianlajin_move_to_winding_low_pos(); + int32_t m16_xianlajin_move_to_winding_up_pos(); + int32_t m16_xianlajin_move_to_line_entry_pos(); int32_t m21_arm_hook_claws_reset(); int32_t m21_arm_hook_claws_move_to_half_pos(); int32_t m21_arm_hook_claws_move_to_full_pos(); - /** - * @brief 剪刀 - */ - int32_t m22_scissors_move_reset_pos(); // block - int32_t m22_scissors_cut(); // block - /** - * @brief 拉线电机 - */ + int32_t m22_scissors_move_reset_pos(); // block + int32_t m22_scissors_cut(); // block int32_t m23_laxian_motor_move_to_reset_pos(); // block int32_t m23_laxian_motor_move_to_tight_line_pos(); // block - - - int32_t device_reset(); int32_t disable_all_motor(); - int32_t xy_get_point(int32_t clip_index, int32_t& x, int32_t& y); // 取弹夹 - int32_t xy_run_to(int32_t x, int32_t y, int32_t zpos = 0, bool jiaxian_duoji_reset = true); // 取弹夹 + int32_t xy_get_point(int32_t clip_index, int32_t& x, int32_t& y); // 取弹夹 + int32_t xy_run_to(int32_t x, int32_t y, int32_t zpos = 0, bool jiaxian_reset = true); // 取弹夹 int32_t xy_run_to_clip_pos_test(int32_t clip_index); int32_t xy_reset(); @@ -165,8 +152,6 @@ class IntelligentWindingRobotCtrl { int32_t zreset(); int32_t xymove_to(int32_t x, int32_t y); - public: - int32_t initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse); void regcb(); int32_t initialize_device(); diff --git a/usrc/main.cpp b/usrc/main.cpp index 7b589a5..92158e4 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -32,6 +32,8 @@ #include "sdk\components\zprotocols\zcancmder_v2\protocol_proxy.hpp" #include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp" #include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_script_cmder_paser.hpp" +// +#include "app_zmodule_device_manager.hpp" #define TAG "main" namespace iflytop { Main gmain; @@ -146,7 +148,7 @@ namespace iflytop { ZCanCommnaderMaster m_zcanCommnaderMaster; // can总线 ModbusBlockHost g_modbusblockhost; // modbus总线 FeiTeServoMotor g_feiteservomotor_bus; // 飞特舵机总线 -ZModuleDeviceManager g_zmodule_device_manager; // 用于管理所有的设备 +APPDM g_zmodule_device_manager; // 用于管理所有的设备 StepMotor45Scheduler step_motor45_scheduler; // 45步进电机调度器 CmdSchedulerV2 g_cmdScheduler; // 串口字符串指令总线 @@ -303,4 +305,3 @@ void Main::run() { osDelay(1); } } - diff --git a/usrc/project_configs.h b/usrc/project_configs.h index 637fd8e..4cd8bb3 100644 --- a/usrc/project_configs.h +++ b/usrc/project_configs.h @@ -15,4 +15,5 @@ #define IFLYTOP_ENABLE_OS 1 #define IFLYTOP_PREEMPTPRIORITY_DEFAULT 5 +#define IFLYTOP_ENABLE_EXCEPTION 1