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
301a470e75
commit
1996dd30d1
7 changed files with 138 additions and 41 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. 19
      README.md
  2. 2
      sdk
  3. 6
      usrc/app_zmodule_device_manager.cpp
  4. 2
      usrc/app_zmodule_device_manager.hpp
  5. 133
      usrc/intelligent_winding_robot_ctrl.cpp
  6. 11
      usrc/intelligent_winding_robot_ctrl.hpp
  7. 6
      usrc/main.cpp

19
README.md
View File

@ -124,4 +124,23 @@ xy_robot_ctrl_read_status (id)
xy_robot_ctrl_read_version (id)
xy_robot_ctrl_set_base_param (id,paramName,val)
xy_robot_ctrl_stop (id,stop_type)
```
```
×Óµ¯:
bullet
µ¯¿Ç
Bullet_case
µ¯Ð¾
Bullet_core
µ¯¼Ð
bullet_holder
```
```
È¡µ¯¼Ð
```

2
sdk

@ -1 +1 @@
Subproject commit d4c0048415ea76a2baf4cf731b7ae7d97298b22d
Subproject commit 3b6bcbe155a48abf6a7859d19f47fbe9c772fb7f

6
usrc/app_zmodule_device_manager.cpp
View File

@ -140,9 +140,9 @@ int32_t APPDM::motor_move_to_zero_backward(uint16_t id, int32_t findzerospeed, i
ZLOGI(TAG, "motor_move_to_zero_backward %d %d %d %d %d", id, findzerospeed, findzeroedge_speed, acc, overtime);
DOCMD(DM::motor_move_to_zero_backward(id, findzerospeed, findzeroedge_speed, acc, overtime));
}
int32_t APPDM::motor_move_to_with_torque(uint16_t id, int32_t direction, int32_t torque) { //
ZLOGI(TAG, "motor_move_to_with_torque %d %d %d", id, direction, torque);
DOCMD(DM::motor_move_to_with_torque(id, direction, torque));
int32_t APPDM::motor_rotate_with_torque(uint16_t id, int32_t direction, int32_t torque) { //
ZLOGI(TAG, "motor_rotate_with_torque %d %d %d", id, direction, torque);
DOCMD(DM::motor_rotate_with_torque(id, direction, torque));
}
int32_t APPDM::motor_read_pos(uint16_t id, int32_t *pos) { //
DOCMD(DM::motor_read_pos(id, pos));

2
usrc/app_zmodule_device_manager.hpp
View File

@ -54,7 +54,7 @@ class APPDM : public ZModuleDeviceManager {
virtual int32_t motor_move_to_acctime(uint16_t id, int32_t position, int32_t motor_velocity, int32_t acctime) override;
virtual int32_t motor_move_to_zero_forward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
virtual int32_t motor_move_to_zero_backward(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;
virtual int32_t motor_move_to_with_torque(uint16_t id, int32_t direction, int32_t torque) override;
virtual int32_t motor_rotate_with_torque(uint16_t id, int32_t direction, int32_t torque) override;
virtual int32_t motor_read_pos(uint16_t id, int32_t *pos) override;
virtual int32_t motor_set_current_pos_by_change_shift(uint16_t id, int32_t pos) override;
virtual int32_t motor_move_to_zero_forward_and_calculated_shift(uint16_t id, int32_t findzerospeed, int32_t findzeroedge_speed, int32_t acc, int32_t overtime) override;

133
usrc/intelligent_winding_robot_ctrl.cpp
View File

@ -80,8 +80,13 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() {
void IntelligentWindingRobotCtrl::regcb() {
// device_reset
m_cmdparse->regCMD("device_reset", "()", 0, [this](PARAM) { device_reset(); });
m_cmdparse->regCMD("enable_all_module", "()", 0, [this](PARAM) { enable_all_module(); });
m_cmdparse->regCMD("disable_all_module", "()", 0, [this](PARAM) { disable_all_module(); });
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(); });
// m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); });
m_cmdparse->regCMD("app_m11_arm_jiaxian_move_to_reset_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_reset_pos(); });
m_cmdparse->regCMD("app_m11_arm_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_clamp_pos(); });
@ -136,36 +141,101 @@ void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) {
}
}
void IntelligentWindingRobotCtrl::wait_modules_idle(...) {
void IntelligentWindingRobotCtrl::wait_modules_idle(void* mark, ...) {
int32_t moduleid = 0;
va_list args;
va_start(args, moduleid);
while (moduleid != -1) {
va_start(args, mark);
moduleid = va_arg(args, int32_t);
while (moduleid > 0) {
wait_module_idle(moduleid);
moduleid = va_arg(args, int32_t);
}
va_end(args);
}
int32_t IntelligentWindingRobotCtrl::disable_all_module() {
m_dm->motor_enable(2, 0);
m_dm->xymotor_enable(3, 0);
m_dm->motor_enable(4, 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::enable_all_module() {
m_dm->motor_enable(2, 1);
m_dm->xymotor_enable(3, 1);
m_dm->motor_enable(4, 1);
m_dm->motor_enable(11, 1);
m_dm->motor_enable(12, 1);
m_dm->motor_enable(13, 1);
m_dm->motor_enable(14, 1);
m_dm->motor_enable(15, 1);
m_dm->motor_enable(16, 1);
m_dm->motor_enable(21, 1);
m_dm->motor_enable(22, 1);
m_dm->motor_enable(23, 1);
return 0;
}
#define WAIT_MODULES_IDLE(...) \
{ wait_modules_idle(NULL, __VA_ARGS__, NULL); }
int32_t IntelligentWindingRobotCtrl::device_reset() {
// Z轴复位
zreset();
m21_arm_hook_claws_reset();
/**
* @
*
* 1. Z轴及其组件复位
* 2. 除了M13外，其他电机复位
* 3. M13复位,等待一定时间，查看M13是否复位成功
* 4. 机械臂复位
*/
enable_all_module();
m4_zreset();
m11_arm_jiaxian_move_to_reset_pos();
xy_reset();
m21_arm_hook_claws_reset();
WAIT_MODULES_IDLE(4, 11, 21);
m12_jiaxian_move_to_reset_pos();
m14_raoxiantance_move_to_reset();
m15_paifei_moveto_reset();
m16_xianlajin_move_to_reset();
m22_scissors_move_reset_pos();
m23_laxian_motor_move_to_reset_pos();
WAIT_MODULES_IDLE(12, 14, 15, 16, 22, 23);
/**
* @brief 优化复位逻辑，超时复位失败，报警
*/
m13_yaxian_move_to_reset_backward();
wait_module_idle(13);
m_dm->xymotor_move_to_zero(XYRobot_ID);
wait_module_idle(XYRobot_ID);
ZLOGI(TAG, "device_reset finished....");
}
int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to(11, cfg.m11_arm_jiaxian_reset_pos, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m11_arm_jiaxian_move_to_clamp_pos() { return m_dm->motor_move_to_with_torque(11, cfg.m11_arm_jiaxian_clamp_direction, cfg.m11_arm_jiaxian_clamp_torque); }
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); }
int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_reset_pos() { return m_dm->motor_move_to(12, cfg.m12_jiaxian_reset_pos, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_clamp_pos() { return m_dm->motor_move_to_with_torque(12, cfg.m12_jiaxian_clamp_direction, cfg.m12_jiaxian_clamp_torque); }
int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_clamp_pos() { return m_dm->motor_rotate_with_torque(12, cfg.m12_jiaxian_clamp_direction, cfg.m12_jiaxian_clamp_torque); }
int32_t IntelligentWindingRobotCtrl::m13_yaxian_move_to_reset_forward() { return m_dm->motor_move_to(13, cfg.m13_yaxian_forward_reset_pos, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m13_yaxian_move_to_reset_backward() { return m_dm->motor_move_to(13, cfg.m13_yaxian_backward_reset_pos, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m13_yaxian_press_clip() { return m_dm->motor_move_to_with_torque(13, cfg.m13_jiaxian_clamp_direction, cfg.m13_jiaxian_clamp_torque); }
int32_t IntelligentWindingRobotCtrl::m13_yaxian_press_clip() { return m_dm->motor_rotate_with_torque(13, cfg.m13_jiaxian_clamp_direction, cfg.m13_jiaxian_clamp_torque); }
int32_t IntelligentWindingRobotCtrl::m14_raoxiantance_move_to_reset() { return m_dm->motor_move_to(14, cfg.m14_raoxiantance_reset_pos, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_reset() { return m_dm->motor_move_to(15, cfg.m15_paifei_reset_pos, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_press() { return m_dm->motor_move_to_with_torque(15, cfg.m15_paifei_press_direction, cfg.m15_paifei_press_torque); }
int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_press() { return m_dm->motor_rotate_with_torque(15, cfg.m15_paifei_press_direction, cfg.m15_paifei_press_torque); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_reset() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_reset_pos, 300, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_tight_line_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_tight_line_pos, 100, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_winding_low_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_winding_low_pos, 100, 0); }
@ -175,9 +245,23 @@ int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_reset() { return m_dm->m
int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_half_pos() { return m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_half_pos, 0, 0); }
int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_move_to_full_pos() { return m_dm->motor_move_to(21, cfg.m21_arm_hook_claws_full_pos, 0, 0); }
int32_t IntelligentWindingRobotCtrl::m22_scissors_move_reset_pos() { return 0; }
int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { return m_dm->motor_move_to_with_torque(22, 1, 4095); }
int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { return m_dm->motor_rotate_with_torque(22, 1, 4095); }
int32_t IntelligentWindingRobotCtrl::m23_laxian_motor_move_to_reset_pos() { return m_dm->motor_move_to_zero_backward(23, 0, 0, 0, 0); }
int32_t IntelligentWindingRobotCtrl::m23_laxian_motor_move_to_tight_line_pos() { return m_dm->motor_move_to(23, 500, 0, 0); }
int32_t IntelligentWindingRobotCtrl::m4_zreset() { m_dm->motor_move_to_zero_backward(4, 450, 300, 2000, 0); }
int32_t IntelligentWindingRobotCtrl::m4_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) {
m4_zreset();
}
}
int32_t IntelligentWindingRobotCtrl::main_shaft_run() {
ZLOGI(TAG, "main_shaft_run");
@ -214,26 +298,7 @@ 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::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;
@ -245,6 +310,8 @@ 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;
@ -264,10 +331,10 @@ int32_t IntelligentWindingRobotCtrl::zmove_to(int32_t pos) {
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;
}
#endif
int32_t IntelligentWindingRobotCtrl::xymove_to(int32_t x, int32_t y) {
ZLOGI(TAG, "xymove_to %d %d", x, y);
@ -283,6 +350,7 @@ int32_t IntelligentWindingRobotCtrl::xy_reset() {
}
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);
@ -303,6 +371,7 @@ int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index)
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) {

11
usrc/intelligent_winding_robot_ctrl.hpp
View File

@ -117,7 +117,7 @@ class IntelligentWindingRobotCtrl {
public:
void wait_module_idle(int32_t moduleid);
void wait_modules_idle(...);
void wait_modules_idle(void* mark, ...);
int32_t m11_arm_jiaxian_move_to_reset_pos();
int32_t m11_arm_jiaxian_move_to_clamp_pos();
int32_t m12_jiaxian_move_to_reset_pos();
@ -141,15 +141,18 @@ class IntelligentWindingRobotCtrl {
int32_t m23_laxian_motor_move_to_reset_pos(); // block
int32_t m23_laxian_motor_move_to_tight_line_pos(); // block
int32_t m4_zreset();
int32_t m4_zmove_to(int32_t pos);
int32_t device_reset();
int32_t disable_all_motor();
int32_t disable_all_module();
int32_t enable_all_module();
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 zmove_to(int32_t pos);
int32_t zreset();
int32_t xymove_to(int32_t x, int32_t y);
void regcb();

6
usrc/main.cpp
View File

@ -240,6 +240,12 @@ void Main::run() {
g_zmodule_device_manager.registerModule(&g_step_motor45[1]);
g_zmodule_device_manager.registerModule(&g_step_motor45[2]);
int32_t status;
g_zmodule_device_manager.module_get_status(21, &status);
// status *= 2;
// status
g_intelligent_winding_robot_ctrl.initialize(&g_zmodule_device_manager, &g_cmdScheduler);
/*******************************************************************************

Write Preview
Loading…
Cancel
Save