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
575186bae4
commit
521a488da3
3 changed files with 113 additions and 36 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 114
      usrc/intelligent_winding_robot_ctrl.cpp
  2. 30
      usrc/intelligent_winding_robot_ctrl.hpp
  3. 5
      usrc/main.cpp

114
usrc/intelligent_winding_robot_ctrl.cpp
View File

@ -72,7 +72,16 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() {
cfg.m21_arm_hook_claws_full_pos = 2558; cfg.m21_arm_hook_claws_full_pos = 2558;
cfg.m21_arm_hook_claws_half_pos = 2294; cfg.m21_arm_hook_claws_half_pos = 2294;
cfg.z_axis_take_clip_pos = 6924;
cfg.xy_platform_cook_bullet_pos_x = 21691;
cfg.xy_platform_cook_bullet_pos_y = 6989;
// 6989 - 4266 2723
cfg.xy_platform_remove_line_pos_x = 648;
cfg.xy_platform_remove_line_pos_y = 6092;
//
//
cfg.z_axis_cook_bullet_pos = 3377;
cfg.z_axis_take_clip_pos = 6924;
cfg.z_axis_process_line_high = 3148;
return 0; return 0;
} }
@ -84,6 +93,7 @@ void IntelligentWindingRobotCtrl::regcb() {
m_cmdparse->regCMD("disable_all_module", "()", 0, [this](PARAM) { disable_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("xy_run_to_clip_pos_test", "()", 1, [this](PARAM) { return xy_run_to_clip_pos_test(atoi(paraV[0])); });
m_cmdparse->regCMD("step_take_bullet", "()", 1, [this](PARAM) { return step_take_bullet(atoi(paraV[0])); }); m_cmdparse->regCMD("step_take_bullet", "()", 1, [this](PARAM) { return step_take_bullet(atoi(paraV[0])); });
m_cmdparse->regCMD("step_prepare_remove_line", "()", 0, [this](PARAM) { return step_prepare_remove_line(); });
// 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(); });
@ -227,6 +237,17 @@ int32_t IntelligentWindingRobotCtrl::device_reset() {
return 0; return 0;
} }
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);
}
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_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); } 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_open_pos() { return m_dm->motor_move_to_torque(12, cfg.m12_jiaxian_reset_pos, 330, 0); } int32_t IntelligentWindingRobotCtrl::m12_jiaxian_move_to_open_pos() { return m_dm->motor_move_to_torque(12, cfg.m12_jiaxian_reset_pos, 330, 0); }
@ -266,19 +287,12 @@ int32_t IntelligentWindingRobotCtrl::m4_zmove_to(int32_t pos) {
} }
int32_t IntelligentWindingRobotCtrl::step_take_bullet(int32_t bulletindex) { int32_t IntelligentWindingRobotCtrl::step_take_bullet(int32_t bulletindex) {
#if 0
0. XY复位
1. Z轴复位
2. 钩子复位
3. 移动到指定位置
m4_zreset();
WAIT_MODULES_IDLE(4);
3. 取子弹
4. 夹线
5. Z轴移动到零点
#endif
xy_reset(); xy_reset();
m4_zreset();
WAIT_MODULES_IDLE(3);
m21_arm_hook_claws_reset(); m21_arm_hook_claws_reset();
m11_arm_jiaxian_move_to_reset_pos(); m11_arm_jiaxian_move_to_reset_pos();
@ -305,6 +319,67 @@ int32_t IntelligentWindingRobotCtrl::step_take_bullet(int32_t bulletindex) {
WAIT_MODULES_IDLE(4); WAIT_MODULES_IDLE(4);
} }
int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line() {
m15_paifei_moveto_reset();
m13_yaxian_move_to_reset_backward();
stop_probe_bullet_pos();
WAIT_MODULES_IDLE(13, 15);
// 移动到COOK位置
xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y);
WAIT_MODULES_IDLE(3);
// Z移动到放置子弹位置
m4_zmove_to(cfg.z_axis_cook_bullet_pos);
WAIT_MODULES_IDLE(4);
m21_arm_hook_claws_move_to_half_pos();
WAIT_MODULES_IDLE(21);
m4_zmove_to(0); // Z轴归零
WAIT_MODULES_IDLE(4);
start_probe_bullet_pos();
/**
* @brief
*
* <------------------point1()
* removeLinePos ^
* |2723
* |
* COOK()
*
*
*/
if (is_hasbullet()) {
xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y - 2723);
WAIT_MODULES_IDLE(3);
xymove_to(cfg.xy_platform_remove_line_pos_x, cfg.xy_platform_remove_line_pos_y);
WAIT_MODULES_IDLE(3);
m4_zmove_to(cfg.z_axis_process_line_high);
WAIT_MODULES_IDLE(4);
// 排线舵机
m15_paifei_moveto_press();
// m4_zmove_to(cfg.z_axis_take_clip_pos);
// WAIT_MODULES_IDLE(4);
m11_arm_jiaxian_move_to_reset_pos();
WAIT_MODULES_IDLE(11);
m4_zmove_to(0);
WAIT_MODULES_IDLE(4);
xymove_to(0, 0);
WAIT_MODULES_IDLE(3);
/**
* @brief TODO:如果需要在这里放回子弹壳
*/
}
// XY移动
// xymove_to();
// is_hasbullet
}
int32_t IntelligentWindingRobotCtrl::main_shaft_run() { int32_t IntelligentWindingRobotCtrl::main_shaft_run() {
ZLOGI(TAG, "main_shaft_run"); ZLOGI(TAG, "main_shaft_run");
DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000)); DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000));
@ -322,14 +397,6 @@ int32_t IntelligentWindingRobotCtrl::xy_platform_reset() { return 0; }
/** /**
* @brief Z轴 * @brief Z轴
*/ */
int32_t IntelligentWindingRobotCtrl::z_axis_reset() { return 0; }
int32_t IntelligentWindingRobotCtrl::z_axis_move_to(int32_t pos) { return 0; }
int32_t IntelligentWindingRobotCtrl::xy_move_to_zero() { return 0; } // 移动到零位
int32_t IntelligentWindingRobotCtrl::xy_take_clip(int32_t index) { return 0; } // 取弹夹
int32_t IntelligentWindingRobotCtrl::xy_take_line() { return 0; } // 取线
int32_t IntelligentWindingRobotCtrl::xy_take_back_clip() { return 0; } // 放弹夹
int32_t IntelligentWindingRobotCtrl::xy_remove_line() { return 0; } // 移除线
int32_t IntelligentWindingRobotCtrl::do_reset_device() {} int32_t IntelligentWindingRobotCtrl::do_reset_device() {}
int32_t IntelligentWindingRobotCtrl::do_winding(int32_t index) {} int32_t IntelligentWindingRobotCtrl::do_winding(int32_t index) {}
@ -380,6 +447,13 @@ int32_t IntelligentWindingRobotCtrl::zreset() {
int32_t IntelligentWindingRobotCtrl::xymove_to(int32_t x, int32_t y) { int32_t IntelligentWindingRobotCtrl::xymove_to(int32_t x, int32_t y) {
ZLOGI(TAG, "xymove_to %d %d", x, y); ZLOGI(TAG, "xymove_to %d %d", x, y);
int32_t nowx;
int32_t nowy;
m_dm->xymotor_read_pos(XYRobot_ID, &nowx, &nowy);
if (nowx == 0 && nowy == 0) {
xy_reset();
}
WAIT_MODULES_IDLE(3);
m_dm->xymotor_move_to(XYRobot_ID, x, y, 0); m_dm->xymotor_move_to(XYRobot_ID, x, y, 0);
return 0; return 0;
} }

30
usrc/intelligent_winding_robot_ctrl.hpp
View File

@ -84,16 +84,22 @@ class IntelligentWindingRobotCtrl {
/** /**
* @brief Z轴定位 * @brief Z轴定位
*/ */
int32_t z_axis_take_line_pos;
int32_t z_axis_process_line_high;
int32_t z_axis_take_clip_pos; int32_t z_axis_take_clip_pos;
int32_t z_axis_winding_hight; int32_t z_axis_winding_hight;
// 3377
int32_t z_axis_cook_bullet_pos;
/** /**
* @brief XY平台 * @brief XY平台
*/ */
int32_t xy_platform_cook_bullet_pos_x;
int32_t xy_platform_cook_bullet_pos_y;
int32_t xy_platform_winding_pos_x;
int32_t xy_platform_winding_pos_y;
int32_t xy_platform_remove_line_pos_x;
int32_t xy_platform_remove_line_pos_y;
int32_t xy_platform_takeline_pos_x; int32_t xy_platform_takeline_pos_x;
int32_t xy_platform_takeline_pos_y; int32_t xy_platform_takeline_pos_y;
@ -115,6 +121,12 @@ class IntelligentWindingRobotCtrl {
public: public:
int32_t initialize(APPDM* dm, ICmdParser* cmdparse); int32_t initialize(APPDM* dm, ICmdParser* cmdparse);
private:
void start_probe_bullet_pos();
void stop_probe_bullet_pos();
int32_t get_probe_bullet_pos();
bool is_hasbullet();
public: public:
void wait_module_idle(int32_t moduleid); void wait_module_idle(int32_t moduleid);
void wait_modules_idle(void* mark, ...); void wait_modules_idle(void* mark, ...);
@ -150,6 +162,7 @@ class IntelligentWindingRobotCtrl {
int32_t enable_all_module(); int32_t enable_all_module();
int32_t step_take_bullet(int32_t bulletindex); int32_t step_take_bullet(int32_t bulletindex);
int32_t step_prepare_remove_line();
int32_t xy_get_point(int32_t clip_index, int32_t& x, int32_t& y); // 取弹夹 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(int32_t x, int32_t y, int32_t zpos = 0, bool jiaxian_reset = true); // 取弹夹
@ -168,17 +181,6 @@ class IntelligentWindingRobotCtrl {
* @brief XY平台 * @brief XY平台
*/ */
int32_t xy_platform_reset(); int32_t xy_platform_reset();
/**
* @brief Z轴
*/
int32_t z_axis_reset();
int32_t z_axis_move_to(int32_t pos);
int32_t xy_move_to_zero(); // 移动到零位
int32_t xy_take_clip(int32_t index); // 取弹夹
int32_t xy_take_line(); // 取线
int32_t xy_take_back_clip(); // 放弹夹
int32_t xy_remove_line(); // 移除线
/** /**
* @brief * @brief

5
usrc/main.cpp
View File

@ -242,7 +242,7 @@ void Main::run() {
int32_t status; int32_t status;
g_zmodule_device_manager.module_get_status(21, &status); g_zmodule_device_manager.module_get_status(21, &status);
// status *= 2; // status *= 2;
// status // status
@ -251,6 +251,8 @@ void Main::run() {
/******************************************************************************* /*******************************************************************************
* g_xyrobotctrlmodule * * g_xyrobotctrlmodule *
*******************************************************************************/ *******************************************************************************/
g_xyrobotctrlmodule.module_set_param(kcfg_motor_x_shift, 0);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_y_shift, 0);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_x_one_circle_pulse, 7344); g_xyrobotctrlmodule.module_set_param(kcfg_motor_x_one_circle_pulse, 7344);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_y_one_circle_pulse, 7344); g_xyrobotctrlmodule.module_set_param(kcfg_motor_y_one_circle_pulse, 7344);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_run_to_zero_speed, 50); g_xyrobotctrlmodule.module_set_param(kcfg_motor_run_to_zero_speed, 50);
@ -264,7 +266,6 @@ void Main::run() {
g_xyrobotctrlmodule.module_active_cfg(); g_xyrobotctrlmodule.module_active_cfg();
g_z_step_motor.module_set_param(kcfg_motor_x_shift, 0); g_z_step_motor.module_set_param(kcfg_motor_x_shift, 0);
g_z_step_motor.module_set_param(kcfg_motor_x_shaft, 0);
g_z_step_motor.module_set_param(kcfg_motor_x_one_circle_pulse, 800); g_z_step_motor.module_set_param(kcfg_motor_x_one_circle_pulse, 800);
g_z_step_motor.module_active_cfg(); g_z_step_motor.module_active_cfg();

Write Preview
Loading…
Cancel
Save