diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c index efa3128..1e87ad3 100644 --- a/Core/Src/freertos.c +++ b/Core/Src/freertos.c @@ -102,7 +102,7 @@ void MX_FREERTOS_Init(void) { /* Create the thread(s) */ /* definition and creation of defaultTask */ - osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 512); + osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 1024); defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL); /* USER CODE BEGIN RTOS_THREADS */ diff --git a/sdk b/sdk index 92d3a87..74b48f0 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit 92d3a87b81b8e5ccf0e7d7c9cdf25987f8993ce0 +Subproject commit 74b48f099c773a5cd6d4f402a671746792e57851 diff --git a/usrc/intelligent_winding_robot_ctrl.cpp b/usrc/intelligent_winding_robot_ctrl.cpp index 7637780..0c037c2 100644 --- a/usrc/intelligent_winding_robot_ctrl.cpp +++ b/usrc/intelligent_winding_robot_ctrl.cpp @@ -18,6 +18,7 @@ using namespace iflytop; void IntelligentWindingRobotCtrl::processError(int32_t err) { ZLOGE(TAG, "processError: %d", err); } void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) { + zos_delay(100); while (true) { int32_t status = 0; int32_t ecode = m_dm->module_get_status(moduleid, &status); @@ -25,12 +26,13 @@ void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) { processError(ecode); break; }; + ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status); if (status == 0) break; if (status == 2) { processError(err::kfail); break; }; - zos_delay(100); + zos_delay(1000); } } @@ -149,14 +151,14 @@ int32_t IntelligentWindingRobotCtrl::scissors_cut() { * @brief 机械臂夹线舵机 */ 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); + ZLOGI(TAG, "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::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)); + ZLOGI(TAG, "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; } @@ -203,16 +205,13 @@ int32_t IntelligentWindingRobotCtrl::xy_platform_reset() { return 0; } int32_t IntelligentWindingRobotCtrl::z_axis_reset() { return 0; } int32_t IntelligentWindingRobotCtrl::z_axis_move_to(int32_t pos) { return 0; } -int32_t IntelligentWindingRobotCtrl::xy_reset() { return 0; } // 复位 int32_t IntelligentWindingRobotCtrl::xy_move_to_zero() { return 0; } // 移动到零位 int32_t IntelligentWindingRobotCtrl::xy_take_clip(int32_t index) { return 0; } // 取弹夹 int32_t IntelligentWindingRobotCtrl::xy_take_line() { return 0; } // 取线 int32_t IntelligentWindingRobotCtrl::xy_take_back_clip() { return 0; } // 放弹夹 int32_t IntelligentWindingRobotCtrl::xy_remove_line() { return 0; } // 移除线 -int32_t IntelligentWindingRobotCtrl::do_reset_device() { - -} +int32_t IntelligentWindingRobotCtrl::do_reset_device() {} int32_t IntelligentWindingRobotCtrl::do_winding(int32_t index) {} /** * @brief @@ -220,7 +219,109 @@ int32_t IntelligentWindingRobotCtrl::do_winding(int32_t index) {} 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::device_reset() { + // Z轴复位 + zreset(); + arm_hook_claws_reset(); + arm_jiaxian_duoji_move_to_reset_pos(); + xy_reset(); + ZLOGI(TAG, "device_reset finished...."); +} + +int32_t IntelligentWindingRobotCtrl::disable_all_motor() { + m_dm->motor_enable(2, 0); + m_dm->xymotor_enable(XYRobot_ID, 0); + m_dm->motor_enable(11, 0); + m_dm->motor_enable(12, 0); + m_dm->motor_enable(13, 0); + m_dm->motor_enable(14, 0); + m_dm->motor_enable(15, 0); + m_dm->motor_enable(16, 0); + + m_dm->motor_enable(21, 0); + m_dm->motor_enable(22, 0); + m_dm->motor_enable(23, 0); + return 0; +} + +int32_t IntelligentWindingRobotCtrl::xy_get_point(int32_t clip_index, int32_t& x, int32_t& y) { + // x = 0; + // y = 0; + + int clip_x_off = clip_index / cfg.clip_each_line_num; + int clip_y_off = clip_index % cfg.clip_each_line_num; + + float eachx = (cfg.xy_platform_takeline_clipXX_pos_x - cfg.xy_platform_takeline_clip00_pos_x) / (cfg.clip_line - 1); + float eachy = (cfg.xy_platform_takeline_clipXX_pos_y - cfg.xy_platform_takeline_clip00_pos_y) / (cfg.clip_each_line_num - 1); + + x = cfg.xy_platform_takeline_clip00_pos_x + eachx * clip_x_off; + y = cfg.xy_platform_takeline_clip00_pos_y + eachy * clip_y_off; + + return 0; +} +int32_t IntelligentWindingRobotCtrl::zmove_to(int32_t pos) { + ZLOGI(TAG, "zmove_to %d", pos); + int32_t nowpos = 0; + m_dm->motor_read_pos(ZMOTOR_ID, &nowpos); + if (pos >= nowpos) { + m_dm->motor_move_to(ZMOTOR_ID, pos, 450, 600); + } else { + m_dm->motor_move_to(ZMOTOR_ID, pos, 1000, 600); + } + if(pos == 0){ + zreset(); + } + wait_module_idle(ZMOTOR_ID); + return 0; +} + +int32_t IntelligentWindingRobotCtrl::zreset() { + ZLOGI(TAG, "zreset"); + m_dm->motor_enable(ZMOTOR_ID, 1); + m_dm->motor_move_to_zero_backward(4, 450, 300, 2000, 0); + wait_module_idle(ZMOTOR_ID); + return 0; +} + +int32_t IntelligentWindingRobotCtrl::xymove_to(int32_t x, int32_t y) { + ZLOGI(TAG, "xymove_to %d %d", x, y); + m_dm->xymotor_move_to(XYRobot_ID, x, y, 0); + wait_module_idle(XYRobot_ID); + return 0; +} +int32_t IntelligentWindingRobotCtrl::xy_reset() { + ZLOGI(TAG, "xy_reset"); + m_dm->xymotor_move_to_zero(XYRobot_ID); + wait_module_idle(XYRobot_ID); + return 0; +} + +int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index) { + ZLOGI(TAG, "xy_run_to_clip_pos_test %d", clip_index); + if (clip_index >= cfg.clip_line * cfg.clip_each_line_num) { + ZLOGE(TAG, "clip_index %d out of range", clip_index); + return err::kparam_out_of_range; + } + zreset(); + arm_jiaxian_duoji_move_to_reset_pos(); + arm_hook_claws_reset(); + + int32_t x = 0; + int32_t y = 0; + xy_get_point(clip_index, x, y); + + xymove_to(x, y); + zmove_to(cfg.z_axis_take_clip_pos); + arm_hook_claws_move_to_full_pos(); + zmove_to(0); + zmove_to(cfg.z_axis_take_clip_pos); + arm_hook_claws_reset(); + zmove_to(0); +} + 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) @@ -268,8 +369,11 @@ int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalu else if (strcmp(cfgname, "arm_hook_claws_full_pos") == 0) cfg.arm_hook_claws_full_pos = cfgvalue; return 0; +#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); @@ -293,55 +397,84 @@ int32_t IntelligentWindingRobotCtrl::dumpcfg() { 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); +#endif return 0; } + #if 1 int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse) { m_dm = dm; m_cmdparse = cmdparse; - cmdparse->regCMD("app_paifei_duoji_moveto_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_reset(); }); - cmdparse->regCMD("app_paifei_duoji_moveto_press", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_press(); }); - cmdparse->regCMD("app_raoxiantance_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return raoxiantance_duoji_move_to_reset(); }); - cmdparse->regCMD("app_raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { + 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.arm_hook_claws_full_pos = 2458; + cfg.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_paifei_duoji_moveto_reset", "()", 0, [this](PARAM) { return paifei_duoji_moveto_reset(); }); + m_cmdparse->regCMD("app_paifei_duoji_moveto_press", "()", 0, [this](PARAM) { return paifei_duoji_moveto_press(); }); + m_cmdparse->regCMD("app_raoxiantance_duoji_move_to_reset", "()", 0, [this](PARAM) { return raoxiantance_duoji_move_to_reset(); }); + m_cmdparse->regCMD("app_raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](PARAM) { 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("app_yaxian_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_reset(); }); - cmdparse->regCMD("app_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("app_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(); }); - cmdparse->regCMD("app_xianlajin_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_reset(); }); - cmdparse->regCMD("app_xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_line_entry_pos(); }); - cmdparse->regCMD("app_xianlajin_duoji_move_to_tight_line_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_tight_line_pos(); }); - cmdparse->regCMD("app_xianlajin_duoji_move_to_loose_line_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_loose_line_pos(); }); - cmdparse->regCMD("app_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return jiaxian_duoji_move_to_reset_pos(); }); - cmdparse->regCMD("app_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return jiaxian_duoji_move_to_clamp_pos(); }); - cmdparse->regCMD("app_scissors_move_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return scissors_move_reset_pos(); }); - cmdparse->regCMD("app_scissors_cut", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return scissors_cut(); }); - cmdparse->regCMD("app_arm_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_jiaxian_duoji_move_to_reset_pos(); }); - cmdparse->regCMD("app_arm_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_jiaxian_duoji_move_to_clamp_pos(); }); - cmdparse->regCMD("app_arm_hook_claws_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_reset(); }); - cmdparse->regCMD("app_arm_hook_claws_move_to_half_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_move_to_half_pos(); }); - cmdparse->regCMD("app_arm_hook_claws_move_to_full_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_move_to_full_pos(); }); - cmdparse->regCMD("app_main_shaft_run", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return main_shaft_run(); }); - cmdparse->regCMD("app_main_shaft_stop", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return main_shaft_stop(); }); - cmdparse->regCMD("app_xy_platform_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_platform_reset(); }); - cmdparse->regCMD("app_z_axis_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return z_axis_reset(); }); - cmdparse->regCMD("app_z_axis_move_to", "(int32_t pos)", 1, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return z_axis_move_to(atoi(paraV[0])); }); - cmdparse->regCMD("app_xy_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_reset(); }); - cmdparse->regCMD("app_xy_move_to_zero", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_move_to_zero(); }); - cmdparse->regCMD("app_xy_take_clip", "(int32_t index)", 1, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_clip(atoi(paraV[0])); }); - cmdparse->regCMD("app_xy_take_line", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_line(); }); - cmdparse->regCMD("app_xy_take_back_clip", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_back_clip(); }); - cmdparse->regCMD("app_xy_remove_line", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_remove_line(); }); - cmdparse->regCMD("app_start_winding", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return start_winding(); }); - cmdparse->regCMD("app_stop_winding", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return stop_winding(); }); - cmdparse->regCMD("app_reset_and_check_device", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return reset_and_check_device(); }); - cmdparse->regCMD("app_setcfg", "(const char* cfgname, int32_t cfgvalue)", 2, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return setcfg(paraV[0], atoi(paraV[1])); }); - cmdparse->regCMD("app_dumpcfg", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return dumpcfg(); }); - - return 0; + m_cmdparse->regCMD("app_yaxian_duoji_move_to_reset", "()", 0, [this](PARAM) { return yaxian_duoji_move_to_reset(); }); + m_cmdparse->regCMD("app_yaxian_duoji_move_to_press_pos", "()", 0, [this](PARAM) { return yaxian_duoji_move_to_press_pos(); }); + m_cmdparse->regCMD("app_yaxian_duoji_move_to_wait_for_press_pos", "()", 0, [this](PARAM) { return yaxian_duoji_move_to_wait_for_press_pos(); }); + m_cmdparse->regCMD("app_xianlajin_duoji_move_to_reset", "()", 0, [this](PARAM) { return xianlajin_duoji_move_to_reset(); }); + m_cmdparse->regCMD("app_xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](PARAM) { return xianlajin_duoji_move_to_line_entry_pos(); }); + m_cmdparse->regCMD("app_xianlajin_duoji_move_to_tight_line_pos", "()", 0, [this](PARAM) { return xianlajin_duoji_move_to_tight_line_pos(); }); + m_cmdparse->regCMD("app_xianlajin_duoji_move_to_loose_line_pos", "()", 0, [this](PARAM) { return xianlajin_duoji_move_to_loose_line_pos(); }); + m_cmdparse->regCMD("app_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](PARAM) { return jiaxian_duoji_move_to_reset_pos(); }); + m_cmdparse->regCMD("app_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](PARAM) { return jiaxian_duoji_move_to_clamp_pos(); }); + m_cmdparse->regCMD("app_scissors_move_reset_pos", "()", 0, [this](PARAM) { return scissors_move_reset_pos(); }); + m_cmdparse->regCMD("app_scissors_cut", "()", 0, [this](PARAM) { return scissors_cut(); }); + m_cmdparse->regCMD("app_arm_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](PARAM) { return arm_jiaxian_duoji_move_to_reset_pos(); }); + m_cmdparse->regCMD("app_arm_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](PARAM) { return arm_jiaxian_duoji_move_to_clamp_pos(); }); + m_cmdparse->regCMD("app_arm_hook_claws_reset", "()", 0, [this](PARAM) { return arm_hook_claws_reset(); }); + m_cmdparse->regCMD("app_arm_hook_claws_move_to_half_pos", "()", 0, [this](PARAM) { return arm_hook_claws_move_to_half_pos(); }); + m_cmdparse->regCMD("app_arm_hook_claws_move_to_full_pos", "()", 0, [this](PARAM) { return 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 } -#endif \ No newline at end of file diff --git a/usrc/intelligent_winding_robot_ctrl.hpp b/usrc/intelligent_winding_robot_ctrl.hpp index 4129588..3733e27 100644 --- a/usrc/intelligent_winding_robot_ctrl.hpp +++ b/usrc/intelligent_winding_robot_ctrl.hpp @@ -3,6 +3,10 @@ #include "sdk\components\zprotocols\zcancmder_v2\api\api.hpp" #include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp" +#define ZMOTOR_ID 4 +#define XYRobot_ID 3 +#define XYRobot_HOOK_ID 21 + namespace iflytop { using namespace std; class IntelligentWindingRobotCtrl { @@ -48,7 +52,7 @@ class IntelligentWindingRobotCtrl { * @brief 机械臂夹线舵机 */ int32_t arm_jiaxian_duoji_reset_pos; - int32_t arm_jiaxian_duoji_clamp_pos; + int32_t arm_jiaxian_duoji_clamp_direction; int32_t arm_jiaxian_duoji_clamp_torque; /** @@ -80,14 +84,46 @@ class IntelligentWindingRobotCtrl { int32_t xy_platform_takeline_pos_x; int32_t xy_platform_takeline_pos_y; + /** + * @brief + * + * + * 00 + * + * + * + * XX + */ + + int32_t xy_platform_takeline_clip00_pos_x; // + int32_t xy_platform_takeline_clip00_pos_y; + + int32_t xy_platform_takeline_clipXX_pos_x; + int32_t xy_platform_takeline_clipXX_pos_y; + int32_t clip_line; + int32_t clip_each_line_num; + } config_t; ZModuleDeviceManager* m_dm; ICmdParser* m_cmdparse; config_t cfg; + private: + 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_run_to_clip_pos_test(int32_t clip_index); + int32_t xy_reset(); + + int32_t zmove_to(int32_t pos); + 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(); // 排废舵机 int32_t paifei_duoji_moveto_reset(); @@ -96,7 +132,7 @@ class IntelligentWindingRobotCtrl { * @brief 绕线探测舵机 */ int32_t raoxiantance_duoji_move_to_reset(); - int32_t raoxiantance_duoji_move_to_get_thickness(int32_t *thickness); + int32_t raoxiantance_duoji_move_to_get_thickness(int32_t* thickness); /** * @brief 压线舵机 */ @@ -146,7 +182,6 @@ class IntelligentWindingRobotCtrl { int32_t z_axis_reset(); int32_t z_axis_move_to(int32_t pos); - int32_t xy_reset(); // 复位 int32_t xy_move_to_zero(); // 移动到零位 int32_t xy_take_clip(int32_t index); // 取弹夹 int32_t xy_take_line(); // 取线 @@ -163,7 +198,6 @@ class IntelligentWindingRobotCtrl { int32_t setcfg(const char* cfgname, int32_t cfgvalue); int32_t dumpcfg(); - int32_t do_reset_device(); int32_t do_winding(int32_t index); diff --git a/usrc/main.cpp b/usrc/main.cpp index adea406..b991240 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -211,13 +211,6 @@ void Main::run() { *******************************************************************************/ g_main_servo_motor.init(2, &g_modbusblockhost, 1); g_xyrobotctrlmodule.initialize(3, &m_zcanCommnaderMaster); - g_xyrobotctrlmodule.module_set_param(kcfg_motor_x_one_circle_pulse, 7344); - g_xyrobotctrlmodule.module_set_param(kcfg_motor_y_one_circle_pulse, 7344); - g_xyrobotctrlmodule.module_set_param(kcfg_motor_run_to_zero_speed, 50); - g_xyrobotctrlmodule.module_set_param(kcfg_motor_run_to_zero_dec, 1600); - g_xyrobotctrlmodule.module_set_param(kcfg_motor_look_zero_edge_speed, 10); - g_xyrobotctrlmodule.module_set_param(kcfg_motor_look_zero_edge_dec, 1600); - g_xyrobotctrlmodule.module_active_cfg(); g_z_step_motor.initialize(4, &m_zcanCommnaderMaster); g_mini_servo[0].initialize(11, &g_feiteservomotor_bus, 1); @@ -248,6 +241,26 @@ void Main::run() { g_intelligent_winding_robot_ctrl.initialize(&g_zmodule_device_manager, &g_cmdScheduler); /******************************************************************************* + * g_xyrobotctrlmodule * + *******************************************************************************/ + g_xyrobotctrlmodule.module_set_param(kcfg_motor_x_one_circle_pulse, 7344); + g_xyrobotctrlmodule.module_set_param(kcfg_motor_y_one_circle_pulse, 7344); + g_xyrobotctrlmodule.module_set_param(kcfg_motor_run_to_zero_speed, 50); + g_xyrobotctrlmodule.module_set_param(kcfg_motor_run_to_zero_dec, 1600); + g_xyrobotctrlmodule.module_set_param(kcfg_motor_look_zero_edge_speed, 10); + g_xyrobotctrlmodule.module_set_param(kcfg_motor_look_zero_edge_dec, 1600); + g_xyrobotctrlmodule.module_set_param(kcfg_motor_default_velocity, 600); + g_xyrobotctrlmodule.module_set_param(kcfg_motor_default_acc, 1000); + g_xyrobotctrlmodule.module_set_param(kcfg_motor_default_dec, 1000); + g_xyrobotctrlmodule.module_set_param(k_cfg_stepmotor_irun, 2); + g_xyrobotctrlmodule.module_active_cfg(); + + g_z_step_motor.module_set_param(kcfg_motor_x_shift, 0); + g_z_step_motor.module_set_param(kcfg_motor_x_shaft, 0); + g_z_step_motor.module_set_param(kcfg_motor_x_one_circle_pulse, 800); + g_z_step_motor.module_active_cfg(); + + /******************************************************************************* * 串口字符串指令总线 * *******************************************************************************/ g_cmdScheduler.initialize(&DEBUG_UART, 1000); //