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

stable v2

master
zhaohe 2 years ago
parent
0bab1e9168
commit
0a01347d1e
2 changed files with 72 additions and 37 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. 98
      usrc/intelligent_winding_robot_ctrl.cpp
  2. 11
      usrc/intelligent_winding_robot_ctrl.hpp

98
usrc/intelligent_winding_robot_ctrl.cpp
View File

@ -61,20 +61,28 @@ class WidthDetector {
return m_lastState;
}
void wait_module_idle(int32_t moduleid) {
void wait_module_idle(int32_t moduleid, int32_t timeout_ms) {
zos_delay(100);
int i = 0;
int i = 0;
int32_t enterticket = zos_get_tick();
while (true) {
int32_t status = 0;
int32_t ecode = m_dm->module_get_status(moduleid, &status);
if (ecode != 0) {
break;
};
if (timeout_ms != 0 && zos_haspassedms(enterticket) > timeout_ms) {
ZLOGE(TAG, "wait_module_idle timeout");
throw (int32_t)err::kmotor_run_overtime;
}
if (status == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
break;
}
if (status == 2) {
throw (int32_t)err::kcatch_exception;
break;
};
if (i % 30 == 0) {
@ -106,7 +114,7 @@ class WidthDetector {
void startDetect(bool forward = false) {
m_detect_thread.stop();
m14_raoxiantance_move_to_reset();
wait_module_idle(14);
wait_module_idle(14, 0);
int32_t enterticket = zos_get_tick();
@ -174,7 +182,7 @@ class WidthDetector {
m_detect_thread.stop();
m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0);
wait_module_idle(14);
wait_module_idle(14, 0);
}
bool isFull() { return g_nowdpos > m_bullet_full_distance - 20; }
@ -275,6 +283,8 @@ void IntelligentWindingRobotCtrl::regcb() {
m_cmdparse->regCMD("step_winding_lineend", "()", 0, [this](PARAM) { return step_winding_lineend(); });
m_cmdparse->regCMD("step_winding_lineend_prepare", "()", 1, [this](PARAM) { return step_winding_lineend_prepare(atoi(paraV[0])); });
m_cmdparse->regCMD("step_winding_take_bullet_from_cooking_to_origin_pos", "()", 1, [this](PARAM) { return step_winding_take_bullet_from_cooking_to_origin_pos(atoi(paraV[0])); });
m_cmdparse->regCMD("start_winding", "()", 0, [this](PARAM) { return start_winding(); });
m_cmdparse->regCMD("stop_winding", "()", 0, [this](PARAM) { return stop_winding(); });
// m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); });
@ -309,29 +319,35 @@ void IntelligentWindingRobotCtrl::regcb() {
m_cmdparse->regCMD("start_probe_bullet_pos_forward", "()", 0, [this](PARAM) { return start_probe_bullet_pos_forward(); });
}
void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) {
void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid, int32_t timeout_ms) {
zos_delay(100);
int i = 0;
int i = 0;
int32_t enterticket = zos_get_tick();
while (true) {
int32_t status = 0;
int32_t ecode = m_dm->module_get_status(moduleid, &status);
if (ecode != 0) {
processError(ecode);
break;
};
if (timeout_ms != 0 && zos_haspassedms(enterticket) > timeout_ms) {
ZLOGE(TAG, "wait_module_idle timeout");
throw (int32_t)err::kmotor_run_overtime;
}
if (status == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
break;
}
if (status == 2) {
processError(err::kfail);
throw (int32_t)err::kcatch_exception;
break;
};
if (i % 30 == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
}
i++;
zos_delay(100);
zos_delay(10);
}
}
@ -346,6 +362,21 @@ void IntelligentWindingRobotCtrl::wait_modules_idle(void* mark, ...) {
}
va_end(args);
}
int32_t IntelligentWindingRobotCtrl::stop_all_module() {
m_dm->module_stop(2);
m_dm->module_stop(3);
m_dm->module_stop(4);
m_dm->module_stop(11);
m_dm->module_stop(12);
m_dm->module_stop(13);
m_dm->module_stop(14);
m_dm->module_stop(15);
m_dm->module_stop(16);
m_dm->module_stop(21);
m_dm->module_stop(22);
m_dm->module_stop(23);
stop_probe_bullet_pos();
}
int32_t IntelligentWindingRobotCtrl::disable_all_module() {
m_dm->motor_enable(2, 0);
@ -362,20 +393,7 @@ int32_t IntelligentWindingRobotCtrl::disable_all_module() {
m_dm->motor_enable(22, 0);
m_dm->motor_enable(23, 0);
m_dm->module_stop(2);
m_dm->module_stop(3);
m_dm->module_stop(4);
m_dm->module_stop(11);
m_dm->module_stop(12);
m_dm->module_stop(13);
m_dm->module_stop(14);
m_dm->module_stop(15);
m_dm->module_stop(16);
m_dm->module_stop(21);
m_dm->module_stop(22);
m_dm->module_stop(23);
stop_probe_bullet_pos();
stop_all_module();
return 0;
}
@ -428,7 +446,7 @@ int32_t IntelligentWindingRobotCtrl::device_reset() {
* @brief 优化复位逻辑，超时复位失败，报警
*/
m13_yaxian_move_to_reset_backward();
wait_module_idle(13);
wait_module_idle(13, 5000);
m_dm->xymotor_move_to_zero(XYRobot_ID);
wait_module_idle(XYRobot_ID);
@ -536,11 +554,11 @@ int32_t IntelligentWindingRobotCtrl::substep_zaxis_do_bullet_action(take_bullet_
}
int32_t IntelligentWindingRobotCtrl::step_take_bullet(int32_t bulletindex) {
m4_zreset();
WAIT_MODULES_IDLE(4);
// m4_zreset();
// WAIT_MODULES_IDLE(4);
xy_reset();
WAIT_MODULES_IDLE(3);
// xy_reset();
// WAIT_MODULES_IDLE(3);
m21_arm_hook_claws_reset();
m11_arm_jiaxian_move_to_reset_pos();
@ -964,7 +982,6 @@ int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalu
int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; }
int32_t IntelligentWindingRobotCtrl::start_winding() { //
m_work_thread.start([this]() {
try {
ZLOGI(TAG, "start_winding");
@ -973,27 +990,46 @@ int32_t IntelligentWindingRobotCtrl::start_winding() { //
device_reset();
bool hasbullet = false;
for (size_t i = 0; i < 5 * 12; i++) {
if (m_work_thread.getExitFlag()) {
break;
}
if (!(i / 5 == 0 || i / 5 == 6 || i / 5 == 11)) {
continue;
}
m_nowwinding_index = i + 1;
xymove_to(0, 0);
step_take_bullet(i);
step_prepare_remove_line(i, hasbullet);
if (!hasbullet) {
continue;
}
step_remove_line();
if (m_work_thread.getExitFlag()) break;
step_winding_prepare();
if (m_work_thread.getExitFlag()) break;
step_winding();
if (m_work_thread.getExitFlag()) break;
step_winding_lineend_prepare(i);
if (m_work_thread.getExitFlag()) break;
step_winding_lineend();
if (m_work_thread.getExitFlag()) break;
step_winding_take_bullet_from_cooking_to_origin_pos(i);
if (m_work_thread.getExitFlag()) break;
}
m4_zmove_to(0);
wait_module_idle(4);
xymove_to(0, 0);
wait_module_idle(3);
stop_all_module();
} catch (int32_t ecode) {
ZLOGE(TAG, "work thread catch exception %d", ecode);
disable_all_module(); // todo : 完成一个无异常版本
} catch (...) {
ZLOGE(TAG, "work thread catch unkown exception");
disable_all_module(); // todo : 完成一个无异常版本
}
disable_all_module(); // todo : 完成一个无异常版本
m_iswinding = false;
});
return 0;

11
usrc/intelligent_winding_robot_ctrl.hpp
View File

@ -171,7 +171,7 @@ class IntelligentWindingRobotCtrl {
bool is_hasbullet();
public:
void wait_module_idle(int32_t moduleid);
void wait_module_idle(int32_t moduleid, int32_t timeout_ms = 0);
void wait_modules_idle(void* mark, ...);
int32_t m11_arm_jiaxian_move_to_reset_pos();
@ -194,7 +194,6 @@ class IntelligentWindingRobotCtrl {
int32_t m16_xianlajin_move_to_cook_lineend_low_pos();
int32_t m16_xianlajin_move_to_cook_lineend_ready_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();
@ -208,6 +207,7 @@ class IntelligentWindingRobotCtrl {
int32_t device_reset();
int32_t disable_all_module();
int32_t stop_all_module();
int32_t enable_all_module();
int32_t substep_zaxis_do_bullet_action(take_bullet_pos_type_t zpos, //
take_bullet_acktion_t take_bullet_acktion, //
@ -223,10 +223,9 @@ class IntelligentWindingRobotCtrl {
int32_t step_winding_prepare();
int32_t step_winding();
// int32_t step_load_the_bullet_case();
int32_t step_winding_lineend_prepare(int bulletindex); // 绕线头准备
int32_t step_winding_lineend(); // 绕线头
int32_t step_winding_take_bullet_from_cooking_to_origin_pos(int bulletindex); // 绕线头
int32_t step_winding_lineend_prepare(int bulletindex); // 绕线头准备
int32_t step_winding_lineend(); // 绕线头
int32_t step_winding_take_bullet_from_cooking_to_origin_pos(int bulletindex); // 绕线头
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); // 取弹夹

Write Preview
Loading…
Cancel
Save