p_Intelligent_winding_robot_v2
/
Intelligent_winding_robot_main_board
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

master
zhaohe 2 years ago
parent
a8aa8a6173
commit
bab30dca57
3 changed files with 171 additions and 151 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 206
      usrc/intelligent_winding_robot_ctrl.cpp
  2. 113
      usrc/intelligent_winding_robot_ctrl.hpp
  3. 3
      usrc/main.cpp

206
usrc/intelligent_winding_robot_ctrl.cpp
View File

@ -19,6 +19,7 @@ void IntelligentWindingRobotCtrl::processError(int32_t err) { ZLOGE(TAG, "proces
void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) {
zos_delay(100);
int i = 0;
while (true) {
int32_t status = 0;
int32_t ecode = m_dm->module_get_status(moduleid, &status);
@ -26,40 +27,68 @@ 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 == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
break;
}
if (status == 2) {
processError(err::kfail);
break;
};
zos_delay(1000);
if (i % 30 == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
break;
}
i++;
zos_delay(100);
}
}
void IntelligentWindingRobotCtrl::wait_modules_idle(...) {
int32_t moduleid = 0;
va_list args;
va_start(args, moduleid);
while (moduleid != -1) {
wait_module_idle(moduleid);
moduleid = va_arg(args, int32_t);
}
va_end(args);
}
int32_t IntelligentWindingRobotCtrl::device_reset() {
// Z轴复位
zreset();
m21_arm_hook_claws_reset();
m11_arm_jiaxian_duoji_move_to_reset_pos();
xy_reset();
ZLOGI(TAG, "device_reset finished....");
}
int32_t IntelligentWindingRobotCtrl::initialize_device() { 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);
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::paifei_duoji_moveto_press() {
ZLOGI(TAG, "paifei_duoji_moveto_press %d %d %d", 15, cfg.paifei_duoji_press_pos, cfg.paifei_duoji_press_torque);
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::raoxiantance_duoji_move_to_reset() {
ZLOGI(TAG, "raoxiantance_duoji_move_to_reset %d %d %d", 14, cfg.raoxiantance_duoji_reset_pos, 330);
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::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);
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);
@ -67,26 +96,26 @@ int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_get_thickness(in
DO(m_dm->motor_read_pos(14, &nowpos));
*thickness = cfg.raoxiantance_duoji_tance_zero_pos - nowpos;
DO(raoxiantance_duoji_move_to_reset());
DO(m14_raoxiantance_duoji_move_to_reset());
return 0;
}
/**
* @brief 压线舵机
*/
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);
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::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);
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::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);
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;
@ -94,26 +123,26 @@ int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_wait_for_press_pos() {
/**
* @brief 线拉紧舵机
*/
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);
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::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);
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::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);
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::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);
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;
@ -121,14 +150,14 @@ int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_loose_line_pos() {
/**
* @brief 夹线舵机12
*/
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);
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::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);
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;
@ -136,13 +165,13 @@ int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_clamp_pos() {
/**
* @brief 剪刀
*/
int32_t IntelligentWindingRobotCtrl::scissors_move_reset_pos() {
// ZLOGI(TAG, "scissors_move_reset_pos %d", 11);
ZLOGI(TAG, "scissors_move_reset_pos");
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::scissors_cut() {
ZLOGI(TAG, "scissors_cut %d", 22);
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;
@ -150,14 +179,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");
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::arm_jiaxian_duoji_move_to_clamp_pos() {
ZLOGI(TAG, "arm_jiaxian_duoji_move_to_clamp_pos");
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;
@ -166,21 +195,21 @@ int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_clamp_pos() {
/**
* @brief 机械臂钩爪
*/
int32_t IntelligentWindingRobotCtrl::arm_hook_claws_reset() {
ZLOGI(TAG, "arm_hook_claws_reset");
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::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));
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::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));
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;
}
@ -220,15 +249,6 @@ 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);
@ -269,7 +289,7 @@ int32_t IntelligentWindingRobotCtrl::zmove_to(int32_t pos) {
} else {
m_dm->motor_move_to(ZMOTOR_ID, pos, 1000, 600);
}
if(pos == 0){
if (pos == 0) {
zreset();
}
wait_module_idle(ZMOTOR_ID);
@ -304,8 +324,8 @@ int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index)
return err::kparam_out_of_range;
}
zreset();
arm_jiaxian_duoji_move_to_reset_pos();
arm_hook_claws_reset();
m11_arm_jiaxian_duoji_move_to_reset_pos();
m21_arm_hook_claws_reset();
int32_t x = 0;
int32_t y = 0;
@ -313,10 +333,10 @@ int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index)
xymove_to(x, y);
zmove_to(cfg.z_axis_take_clip_pos);
arm_hook_claws_move_to_full_pos();
m21_arm_hook_claws_move_to_full_pos();
zmove_to(0);
zmove_to(cfg.z_axis_take_clip_pos);
arm_hook_claws_reset();
m21_arm_hook_claws_reset();
zmove_to(0);
}
@ -362,12 +382,12 @@ int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalu
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;
else if (strcmp(cfgname, "m22_scissors_cut_pos") == 0)
cfg.m22_scissors_cut_pos = cfgvalue;
else if (strcmp(cfgname, "m21_arm_hook_claws_half_pos") == 0)
cfg.m21_arm_hook_claws_half_pos = cfgvalue;
else if (strcmp(cfgname, "m21_arm_hook_claws_full_pos") == 0)
cfg.m21_arm_hook_claws_full_pos = cfgvalue;
return 0;
#endif
return 0;
@ -394,9 +414,9 @@ int32_t IntelligentWindingRobotCtrl::dumpcfg() {
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);
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;
}
@ -418,8 +438,8 @@ int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdPa
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;
cfg.m21_arm_hook_claws_full_pos = 2458;
cfg.m21_arm_hook_claws_half_pos = 2294;
regcb();
return 0;
@ -435,31 +455,31 @@ void IntelligentWindingRobotCtrl::regcb() {
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) {
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 = raoxiantance_duoji_move_to_get_thickness(ack->getAck(1));
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_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_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(); });

113
usrc/intelligent_winding_robot_ctrl.hpp
View File

@ -9,6 +9,7 @@
namespace iflytop {
using namespace std;
class IntelligentWindingRobotCtrl {
public:
typedef struct {
@ -59,13 +60,13 @@ class IntelligentWindingRobotCtrl {
* @brief 剪刀
*/
int32_t scissors_reset_pos;
int32_t scissors_cut_pos;
int32_t m22_scissors_cut_pos;
/**
* @brief 钩爪
*/
int32_t arm_hook_claws_half_pos;
int32_t arm_hook_claws_full_pos;
int32_t m21_arm_hook_claws_half_pos;
int32_t m21_arm_hook_claws_full_pos;
/**
* @brief Z轴定位
@ -84,17 +85,6 @@ 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;
@ -110,64 +100,75 @@ class IntelligentWindingRobotCtrl {
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);
void wait_module_idle(int32_t moduleid);
void wait_modules_idle(...);
public:
int32_t initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse);
void regcb();
int32_t initialize_device();
// 排废舵机
int32_t paifei_duoji_moveto_reset();
int32_t paifei_duoji_moveto_press();
/**
* @brief 绕线探测舵机
* @brief 机械臂夹线舵机
*/
int32_t m11_arm_jiaxian_duoji_move_to_reset_pos();
int32_t m11_arm_jiaxian_duoji_move_to_clamp_pos();
/**
* @brief 夹线舵机
*/
int32_t raoxiantance_duoji_move_to_reset();
int32_t raoxiantance_duoji_move_to_get_thickness(int32_t* thickness);
int32_t m12_jiaxian_duoji_move_to_reset_pos();
int32_t m12_jiaxian_duoji_move_to_clamp_pos();
/**
* @brief 压线舵机
*/
int32_t yaxian_duoji_move_to_reset();
int32_t yaxian_duoji_move_to_press_pos();
int32_t yaxian_duoji_move_to_wait_for_press_pos();
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 xianlajin_duoji_move_to_reset(); // 零位
int32_t xianlajin_duoji_move_to_line_entry_pos(); // 入线位
int32_t xianlajin_duoji_move_to_tight_line_pos(); // 拉线位置
int32_t xianlajin_duoji_move_to_loose_line_pos(); // 放线位置
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 夹线舵机
* @brief 机械臂钩爪
*/
int32_t jiaxian_duoji_move_to_reset_pos();
int32_t jiaxian_duoji_move_to_clamp_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 scissors_move_reset_pos(); // block
int32_t scissors_cut(); // block
int32_t m22_scissors_move_reset_pos(); // block
int32_t m22_scissors_cut(); // block
/**
* @brief 机械臂夹线舵机
* @brief 拉线电机
*/
int32_t arm_jiaxian_duoji_move_to_reset_pos();
int32_t arm_jiaxian_duoji_move_to_clamp_pos();
int32_t m23_laxian_motor_move_to_reset_pos(); // block
int32_t m23_laxian_motor_move_to_tight_line_pos(); // block
/**
* @brief 机械臂钩爪
*/
int32_t arm_hook_claws_reset();
int32_t arm_hook_claws_move_to_half_pos();
int32_t arm_hook_claws_move_to_full_pos();
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 main_shaft_run();
int32_t main_shaft_stop();
@ -202,8 +203,6 @@ class IntelligentWindingRobotCtrl {
int32_t do_winding(int32_t index);
void processError(int32_t err);
void wait_module_idle(int32_t moduleid);
};
} // namespace iflytop

3
usrc/main.cpp
View File

@ -252,7 +252,7 @@ void Main::run() {
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_set_param(k_cfg_stepmotor_irun, 4);
g_xyrobotctrlmodule.module_active_cfg();
g_z_step_motor.module_set_param(kcfg_motor_x_shift, 0);
@ -303,3 +303,4 @@ void Main::run() {
osDelay(1);
}
}
Write Preview
Loading…
Cancel
Save