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
0a01347d1e
commit
710185b678
2 changed files with 89 additions and 11 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. 2
      usrc/intelligent_winding_robot_ctrl.hpp

98
usrc/intelligent_winding_robot_ctrl.cpp
View File

@ -38,8 +38,9 @@ class WidthDetector {
IntelligentWindingRobotCtrl::config_t* cfg;
APPDM* m_dm;
int32_t m_bullet_distance = 33; // 理论值33,这里取保守值20
int32_t m_bullet_full_distance = 127;
int32_t m_bullet_distance = 33; // 理论值33,这里取保守值20
int32_t m_bullet_full_distance = 154;
int32_t lastDistanceChangeTicket = 0;
public:
void init(IntelligentWindingRobotCtrl::config_t* cfg, APPDM* dm) {
@ -155,7 +156,8 @@ class WidthDetector {
if (g_isrunning && getDetectGPIOState() == 0) {
stop_run();
m_dm->motor_read_pos(14, &g_nowpos);
g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos;
g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos;
lastDistanceChangeTicket = zos_get_tick();
ZLOGI(TAG, "--------------now pos %d bullet width %d", g_nowpos, g_nowdpos);
} else if (!g_isrunning && getDetectGPIOState() != 0) {
start_run_back();
@ -167,7 +169,8 @@ class WidthDetector {
if (g_isrunning && getDetectGPIOState() != 0) {
stop_run();
m_dm->motor_read_pos(14, &g_nowpos);
g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos;
g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos;
lastDistanceChangeTicket = zos_get_tick();
ZLOGI(TAG, "--------------now pos %d bullet width %d", g_nowpos, g_nowdpos);
} else if (!g_isrunning && getDetectGPIOState() == 0) {
start_run_forward_slow();
@ -185,9 +188,16 @@ class WidthDetector {
wait_module_idle(14, 0);
}
bool isFull() { return g_nowdpos > m_bullet_full_distance - 20; }
bool isFull() { return g_nowdpos > m_bullet_full_distance; }
bool isHasBullet() { return g_nowdpos > 5; }
bool isRemoveLineEnd() { return g_nowdpos < m_bullet_distance + 10; }
bool distanceIsStopChange() {
if (zos_haspassedms(lastDistanceChangeTicket) > 10 * 1000) {
return true;
}
return false;
}
};
WidthDetector g_widthDetector;
@ -231,14 +241,16 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() {
cfg.m15_paifei_press_direction = 1;
cfg.m15_paifei_press_torque = 330;
cfg.m16_xianlajin_reset_pos = 2047;
cfg.m16_xianlajin_reset_pos = 2000;
cfg.m16_xianlajin_cook_line_end_ready_pos = 1900;
cfg.m16_xianlajin_tight_line_pos = 1966;
cfg.m16_xianlajin_winding_low_pos = 1885;
cfg.m16_xianlajin_winding_high_pos = 1815;
cfg.m16_xianlajin_line_entry_pos = 1800;
cfg.m16_xianlajin_cook_line_end_low_pos = 1820;
cfg.m16_xianlajin_cook_line_end_high_pos = 1741;
// cfg.m16_xianlajin_winding_low_pos = 1922;
// cfg.m16_xianlajin_winding_high_pos = 1863;
cfg.m16_xianlajin_line_entry_pos = 1800;
cfg.m16_xianlajin_cook_line_end_low_pos = 1820;
cfg.m16_xianlajin_cook_line_end_high_pos = 1741;
cfg.m21_arm_hook_claws_full_pos = 2558;
cfg.m21_arm_hook_claws_half_pos = 2194;
@ -285,6 +297,7 @@ void IntelligentWindingRobotCtrl::regcb() {
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("start_remove_line", "()", 0, [this](PARAM) { return start_remove_line(); });
// m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); });
@ -689,8 +702,12 @@ int32_t IntelligentWindingRobotCtrl::step_remove_line() {
if (i == overtime - 1) {
ZLOGI(TAG, "detect remove line end timeout");
}
if (g_widthDetector.distanceIsStopChange()) {
ZLOGI(TAG, "distanceIsStopChange......");
break;
}
}
osDelay(5000);
osDelay(10 * 1000);
m_dm->module_stop(2);
// m15_paifei_moveto_reset();
m15_paifei_moveto_reset();
@ -749,8 +766,13 @@ int32_t IntelligentWindingRobotCtrl::step_winding_prepare() {
int32_t IntelligentWindingRobotCtrl::step_winding() {
start_probe_bullet_pos();
m_dm->motor_rotate_acctime(2, 1, 1000, 10000);
osDelay(1000);
osDelay(5000);
m23_laxian_motor_move_to_reset_pos();
m13_yaxian_move_to_reset_backward();
osDelay(800);
m13_yaxian_move_to_reset_forward();
osDelay(300);
for (size_t i = 0; i < 80; i++) { // TODO:60这里是个常量可能会造成绕线提前释放。
m16_xianlajin_move_to_winding_low_pos();
WAIT_MODULES_IDLE(16);
@ -759,6 +781,11 @@ int32_t IntelligentWindingRobotCtrl::step_winding() {
if (g_widthDetector.isFull()) {
break;
}
if (g_widthDetector.distanceIsStopChange()) {
ZLOGI(TAG, "distanceIsStopChange......");
break;
}
}
m_dm->module_stop(2);
stop_probe_bullet_pos();
@ -1034,6 +1061,55 @@ int32_t IntelligentWindingRobotCtrl::start_winding() { //
});
return 0;
}
int32_t IntelligentWindingRobotCtrl::start_remove_line() {
//
m_work_thread.start([this]() {
try {
ZLOGI(TAG, "start_winding");
m_iswinding = true;
m_nowwinding_index = 0;
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;
step_take_bullet(i);
step_prepare_remove_line(i, hasbullet);
if (!hasbullet) {
continue;
}
step_remove_line();
stop_probe_bullet_pos();
substep_zaxis_do_bullet_action(kCookPos, kTakeBullet, kKeepLine, NULL);
xymove_to_bullet_pos(i);
substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBackBullet, kReleaseLine, NULL);
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 : 完成一个无异常版本
}
m_iswinding = false;
});
return 0;
}
int32_t IntelligentWindingRobotCtrl::stop_winding() {
m_work_thread.stop();
m_iswinding = false;

2
usrc/intelligent_winding_robot_ctrl.hpp
View File

@ -243,6 +243,8 @@ class IntelligentWindingRobotCtrl {
int32_t start_winding();
int32_t stop_winding();
int32_t start_remove_line();
int32_t dumpcfg();
int32_t do_winding(int32_t index);

Write Preview
Loading…
Cancel
Save