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
a80187eba7
commit
c462bfb9cc
2 changed files with 156 additions and 159 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. 283
      usrc/intelligent_winding_robot_ctrl.cpp
  2. 32
      usrc/intelligent_winding_robot_ctrl.hpp

283
usrc/intelligent_winding_robot_ctrl.cpp
View File

@ -26,12 +26,146 @@ class myexception : public exception {
virtual const char* what() const throw() { return "My exception happened"; }
};
class WidthDetector {
ZGPIO g_detectGpio;
int32_t g_nowpos = 0;
int32_t g_nowdpos = 0;
bool g_isrunning = false;
ZThread m_detect_thread;
int32_t m_lastState = false;
IntelligentWindingRobotCtrl::config_t* cfg;
APPDM* m_dm;
public:
void init(IntelligentWindingRobotCtrl::config_t* cfg, APPDM* dm) {
this->cfg = cfg;
m_dm = dm;
g_detectGpio.initAsInput(PB13, ZGPIO::kMode_pullup, ZGPIO::kIRQ_noIrq, true);
m_detect_thread.init("detect", 512);
getDetectGPIOState();
}
int32_t getDetectGPIOState() {
int32_t nowstate = g_detectGpio.getState();
for (size_t i = 0; i < 5; i++) {
if (g_detectGpio.getState() != nowstate) {
return m_lastState;
}
osDelay(3);
}
m_lastState = nowstate;
return m_lastState;
}
void 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);
if (ecode != 0) {
break;
};
if (status == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
break;
}
if (status == 2) {
break;
};
if (i % 30 == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
}
i++;
zos_delay(10);
}
}
void m14_raoxiantance_move_to_reset() { m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0); }
void start_run_back() {
m_dm->motor_move_to(14, cfg->m14_raoxiantance_reset_pos, 30, 0);
g_isrunning = true;
}
void start_run_forward() {
m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_tance_zero_pos+100, 330, 0);
g_isrunning = true;
}
void stop_run() {
m_dm->module_stop(14);
g_isrunning = false;
}
void startDetect() {
m_detect_thread.stop();
m14_raoxiantance_move_to_reset();
wait_module_idle(14);
int32_t enterticket = zos_get_tick();
start_run_forward();
while (true) {
if (g_isrunning && getDetectGPIOState() != 0) {
stop_run();
break;
}
if (zos_haspassedms(enterticket) > 3000) {
ZLOGE(TAG, "start_probe_bullet_pos timeout");
/**
* @brief TODO add exception here
*
*/
stop_run();
break;
}
zos_delay(10);
}
start_run_back();
while (true) {
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;
ZLOGI(TAG, "--------------now pos %d bullet width %d", g_nowpos, g_nowdpos);
break;
}
zos_delay(3);
}
m_detect_thread.start([this]() {
while (!m_detect_thread.getExitFlag()) {
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;
ZLOGI(TAG, "--------------now pos %d bullet width %d",g_nowpos, g_nowdpos);
} else if (!g_isrunning && getDetectGPIOState() != 0) {
start_run_back();
}
zos_delay(3);
}
});
}
void stopDetect() {
m_detect_thread.stop();
m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0);
wait_module_idle(14);
}
};
WidthDetector g_widthDetector;
int32_t IntelligentWindingRobotCtrl::initialize(APPDM* dm, ICmdParser* cmdparse) {
m_dm = dm;
m_cmdparse = cmdparse;
m_work_thread.init("work", 512);
// m_detect_thread.init("detect", 512);
initialize_device();
regcb();
g_widthDetector.init(&cfg, dm);
return 0;
}
@ -57,7 +191,7 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() {
cfg.m13_jiaxian_clamp_torque = 500;
cfg.m14_raoxiantance_reset_pos = 2047;
cfg.m14_raoxiantance_tance_zero_pos = 2517;
cfg.m14_raoxiantance_tance_zero_pos = 2829;
cfg.m15_paifei_reset_pos = 2046;
cfg.m15_paifei_press_direction = 1;
@ -137,6 +271,7 @@ void IntelligentWindingRobotCtrl::regcb() {
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(); });
m_cmdparse->regCMD("start_probe_bullet_pos", "()", 0, [this](PARAM) { return start_probe_bullet_pos(); });
}
void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) {
@ -256,14 +391,8 @@ int32_t IntelligentWindingRobotCtrl::device_reset() {
bool IntelligentWindingRobotCtrl::is_hasbullet() { return true; }
void IntelligentWindingRobotCtrl::start_probe_bullet_pos() {
m14_raoxiantance_move_to_reset();
WAIT_MODULES_IDLE(14);
}
void IntelligentWindingRobotCtrl::stop_probe_bullet_pos() {
m14_raoxiantance_move_to_reset();
WAIT_MODULES_IDLE(14);
}
void IntelligentWindingRobotCtrl::start_probe_bullet_pos() { g_widthDetector.startDetect(); }
void IntelligentWindingRobotCtrl::stop_probe_bullet_pos() { g_widthDetector.stopDetect(); }
int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to_torque(11, cfg.m11_arm_jiaxian_reset_pos, 330, 0); }
int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_clamp_pos() { return m_dm->motor_rotate_with_torque(11, cfg.m11_arm_jiaxian_clamp_direction, cfg.m11_arm_jiaxian_clamp_torque); }
@ -657,33 +786,9 @@ int32_t IntelligentWindingRobotCtrl::step_winding_take_bullet_from_cooking_to_or
substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBackBullet, kReleaseLine);
}
int32_t IntelligentWindingRobotCtrl::main_shaft_run() {
ZLOGI(TAG, "main_shaft_run");
DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000));
return 0;
}
int32_t IntelligentWindingRobotCtrl::main_shaft_stop() {
ZLOGI(TAG, "main_shaft_stop");
DO(m_dm->module_stop(2));
return 0;
}
/**
* @brief XYƽ̨
*/
int32_t IntelligentWindingRobotCtrl::xy_platform_reset() { return 0; }
/**
* @brief ZÖá
*/
int32_t IntelligentWindingRobotCtrl::do_reset_device() {}
int32_t IntelligentWindingRobotCtrl::do_winding(int32_t index) {}
/**
* @brief
*/
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::xy_get_point(int32_t clip_index, int32_t& x, int32_t& y) {
int clip_x_off = clip_index / cfg.clip_each_line_num;
@ -698,31 +803,6 @@ int32_t IntelligentWindingRobotCtrl::xy_get_point(int32_t clip_index, int32_t& x
return 0;
}
#if 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);
wait_module_idle(ZMOTOR_ID);
return 0;
}
#endif
int32_t IntelligentWindingRobotCtrl::xymove_to_bullet_pos(int32_t bulletindex) {
int32_t x = 0;
int32_t y = 0;
@ -758,86 +838,11 @@ int32_t IntelligentWindingRobotCtrl::xy_reset() {
m_dm->xymotor_move_to_zero(XYRobot_ID);
return 0;
}
int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) { return 0; }
int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; }
int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index) {
#if 0
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();
m11_arm_jiaxian_move_to_reset_pos();
m21_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);
m21_arm_hook_claws_move_to_full_pos();
zmove_to(0);
zmove_to(cfg.z_axis_take_clip_pos);
m21_arm_hook_claws_reset();
zmove_to(0);
#endif
}
int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) {
#if 0
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)
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
int32_t IntelligentWindingRobotCtrl::start_winding() { //
return 0;
}
int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; }
#if 1
#endif
int32_t IntelligentWindingRobotCtrl::stop_winding() { return 0; }
int32_t IntelligentWindingRobotCtrl::reset_and_check_device() { return 0; }

32
usrc/intelligent_winding_robot_ctrl.hpp
View File

@ -129,6 +129,9 @@ class IntelligentWindingRobotCtrl {
ICmdParser* m_cmdparse;
config_t cfg;
ZThread m_work_thread;
ZThread m_detect_thread;
typedef enum {
kBulletBulletHolderPos = 0,
kCookPos,
@ -191,7 +194,6 @@ class IntelligentWindingRobotCtrl {
int32_t device_reset();
int32_t disable_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, //
take_bullet_line_acktion_t take_bullet_line_acktion, //
@ -201,46 +203,36 @@ class IntelligentWindingRobotCtrl {
int32_t step_take_back_bullet(int32_t bulletindex);
int32_t step_take_bullet_case(int32_t bulletindex);
int32_t step_take_back_bullet_case(int32_t bulletindex);
int32_t step_prepare_remove_line(int32_t bulletindex);
int32_t step_remove_line();
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); // 取弹夹
int32_t xy_run_to_clip_pos_test(int32_t clip_index);
int32_t xy_reset();
int32_t xymove_to(int32_t x, int32_t y);
int32_t xymove_to_bullet_pos(int32_t bulletindex);
void regcb();
int32_t initialize_device();
int32_t main_shaft_run();
int32_t main_shaft_stop();
/**
* @brief XY平台
*/
int32_t xy_platform_reset();
/**
* @brief
*/
int32_t setcfg(const char* cfgname, int32_t cfgvalue);
public:
int32_t start_winding();
int32_t stop_winding();
int32_t reset_and_check_device();
int32_t setcfg(const char* cfgname, int32_t cfgvalue);
int32_t dumpcfg();
int32_t do_reset_device();
int32_t do_winding(int32_t index);
void processError(int32_t err);

Write Preview
Loading…
Cancel
Save