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
ef501ad2cf
commit
a8aa8a6173
5 changed files with 240 additions and 60 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. 2
      Core/Src/freertos.c
  2. 2
      sdk
  3. 227
      usrc/intelligent_winding_robot_ctrl.cpp
  4. 42
      usrc/intelligent_winding_robot_ctrl.hpp
  5. 27
      usrc/main.cpp

2
Core/Src/freertos.c
View File

@ -102,7 +102,7 @@ void MX_FREERTOS_Init(void) {
/* Create the thread(s) */ /* Create the thread(s) */
/* definition and creation of defaultTask */ /* definition and creation of defaultTask */
osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 512);
osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 1024);
defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL); defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
/* USER CODE BEGIN RTOS_THREADS */ /* USER CODE BEGIN RTOS_THREADS */

2
sdk

@ -1 +1 @@
Subproject commit 92d3a87b81b8e5ccf0e7d7c9cdf25987f8993ce0
Subproject commit 74b48f099c773a5cd6d4f402a671746792e57851

227
usrc/intelligent_winding_robot_ctrl.cpp
View File

@ -18,6 +18,7 @@ using namespace iflytop;
void IntelligentWindingRobotCtrl::processError(int32_t err) { ZLOGE(TAG, "processError: %d", err); } void IntelligentWindingRobotCtrl::processError(int32_t err) { ZLOGE(TAG, "processError: %d", err); }
void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) { void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) {
zos_delay(100);
while (true) { while (true) {
int32_t status = 0; int32_t status = 0;
int32_t ecode = m_dm->module_get_status(moduleid, &status); int32_t ecode = m_dm->module_get_status(moduleid, &status);
@ -25,12 +26,13 @@ void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) {
processError(ecode); processError(ecode);
break; break;
}; };
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
if (status == 0) break; if (status == 0) break;
if (status == 2) { if (status == 2) {
processError(err::kfail); processError(err::kfail);
break; break;
}; };
zos_delay(100);
zos_delay(1000);
} }
} }
@ -149,14 +151,14 @@ int32_t IntelligentWindingRobotCtrl::scissors_cut() {
* @brief 机械臂夹线舵机 * @brief 机械臂夹线舵机
*/ */
int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_reset_pos() { int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_reset_pos() {
ZLOGI(TAG, "arm_jiaxian_duoji_move_to_reset_pos %d %d %d", 11, cfg.arm_jiaxian_duoji_reset_pos, 330);
ZLOGI(TAG, "arm_jiaxian_duoji_move_to_reset_pos");
DO(m_dm->motor_move_to(11, cfg.arm_jiaxian_duoji_reset_pos, 2000, 0)); DO(m_dm->motor_move_to(11, cfg.arm_jiaxian_duoji_reset_pos, 2000, 0));
wait_module_idle(11); wait_module_idle(11);
return 0; return 0;
} }
int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_clamp_pos() { int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_clamp_pos() {
ZLOGI(TAG, "arm_jiaxian_duoji_move_to_clamp_pos %d %d %d", 11, cfg.arm_jiaxian_duoji_clamp_pos, cfg.arm_jiaxian_duoji_clamp_torque);
DO(m_dm->motor_move_to_with_torque(11, cfg.arm_jiaxian_duoji_clamp_pos, cfg.arm_jiaxian_duoji_clamp_torque));
ZLOGI(TAG, "arm_jiaxian_duoji_move_to_clamp_pos");
DO(m_dm->motor_move_to_with_torque(11, cfg.arm_jiaxian_duoji_clamp_direction, cfg.arm_jiaxian_duoji_clamp_torque));
wait_module_idle(11); wait_module_idle(11);
return 0; return 0;
} }
@ -203,16 +205,13 @@ int32_t IntelligentWindingRobotCtrl::xy_platform_reset() { return 0; }
int32_t IntelligentWindingRobotCtrl::z_axis_reset() { return 0; } int32_t IntelligentWindingRobotCtrl::z_axis_reset() { return 0; }
int32_t IntelligentWindingRobotCtrl::z_axis_move_to(int32_t pos) { return 0; } int32_t IntelligentWindingRobotCtrl::z_axis_move_to(int32_t pos) { return 0; }
int32_t IntelligentWindingRobotCtrl::xy_reset() { return 0; } // 复位
int32_t IntelligentWindingRobotCtrl::xy_move_to_zero() { 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_clip(int32_t index) { return 0; } // 取弹夹
int32_t IntelligentWindingRobotCtrl::xy_take_line() { return 0; } // 取线 int32_t IntelligentWindingRobotCtrl::xy_take_line() { return 0; } // 取线
int32_t IntelligentWindingRobotCtrl::xy_take_back_clip() { return 0; } // 放弹夹 int32_t IntelligentWindingRobotCtrl::xy_take_back_clip() { return 0; } // 放弹夹
int32_t IntelligentWindingRobotCtrl::xy_remove_line() { 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) {}
/** /**
* @brief * @brief
@ -220,7 +219,109 @@ int32_t IntelligentWindingRobotCtrl::do_winding(int32_t index) {}
int32_t IntelligentWindingRobotCtrl::start_winding() { return 0; } int32_t IntelligentWindingRobotCtrl::start_winding() { return 0; }
int32_t IntelligentWindingRobotCtrl::stop_winding() { return 0; } int32_t IntelligentWindingRobotCtrl::stop_winding() { return 0; }
int32_t IntelligentWindingRobotCtrl::reset_and_check_device() { return 0; } int32_t IntelligentWindingRobotCtrl::reset_and_check_device() { return 0; }
int32_t IntelligentWindingRobotCtrl::device_reset() {
// Z轴复位
zreset();
arm_hook_claws_reset();
arm_jiaxian_duoji_move_to_reset_pos();
xy_reset();
ZLOGI(TAG, "device_reset finished....");
}
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;
float eachx = (cfg.xy_platform_takeline_clipXX_pos_x - cfg.xy_platform_takeline_clip00_pos_x) / (cfg.clip_line - 1);
float eachy = (cfg.xy_platform_takeline_clipXX_pos_y - cfg.xy_platform_takeline_clip00_pos_y) / (cfg.clip_each_line_num - 1);
x = cfg.xy_platform_takeline_clip00_pos_x + eachx * clip_x_off;
y = cfg.xy_platform_takeline_clip00_pos_y + eachy * clip_y_off;
return 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);
m_dm->motor_move_to_zero_backward(4, 450, 300, 2000, 0);
wait_module_idle(ZMOTOR_ID);
return 0;
}
int32_t IntelligentWindingRobotCtrl::xymove_to(int32_t x, int32_t y) {
ZLOGI(TAG, "xymove_to %d %d", x, y);
m_dm->xymotor_move_to(XYRobot_ID, x, y, 0);
wait_module_idle(XYRobot_ID);
return 0;
}
int32_t IntelligentWindingRobotCtrl::xy_reset() {
ZLOGI(TAG, "xy_reset");
m_dm->xymotor_move_to_zero(XYRobot_ID);
wait_module_idle(XYRobot_ID);
return 0;
}
int32_t IntelligentWindingRobotCtrl::xy_run_to_clip_pos_test(int32_t clip_index) {
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();
arm_jiaxian_duoji_move_to_reset_pos();
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);
arm_hook_claws_move_to_full_pos();
zmove_to(0);
zmove_to(cfg.z_axis_take_clip_pos);
arm_hook_claws_reset();
zmove_to(0);
}
int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) { int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) {
#if 0
if (strcmp(cfgname, "paifei_duoji_reset_pos") == 0) if (strcmp(cfgname, "paifei_duoji_reset_pos") == 0)
cfg.paifei_duoji_reset_pos = cfgvalue; cfg.paifei_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "paifei_duoji_press_pos") == 0) else if (strcmp(cfgname, "paifei_duoji_press_pos") == 0)
@ -268,8 +369,11 @@ int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalu
else if (strcmp(cfgname, "arm_hook_claws_full_pos") == 0) else if (strcmp(cfgname, "arm_hook_claws_full_pos") == 0)
cfg.arm_hook_claws_full_pos = cfgvalue; cfg.arm_hook_claws_full_pos = cfgvalue;
return 0; return 0;
#endif
return 0;
} }
int32_t IntelligentWindingRobotCtrl::dumpcfg() { int32_t IntelligentWindingRobotCtrl::dumpcfg() {
#if 0
ZLOGI(TAG, "paifei_duoji_reset_pos %d", cfg.paifei_duoji_reset_pos); ZLOGI(TAG, "paifei_duoji_reset_pos %d", cfg.paifei_duoji_reset_pos);
ZLOGI(TAG, "paifei_duoji_press_pos %d", cfg.paifei_duoji_press_pos); ZLOGI(TAG, "paifei_duoji_press_pos %d", cfg.paifei_duoji_press_pos);
ZLOGI(TAG, "paifei_duoji_press_torque %d", cfg.paifei_duoji_press_torque); ZLOGI(TAG, "paifei_duoji_press_torque %d", cfg.paifei_duoji_press_torque);
@ -293,55 +397,84 @@ int32_t IntelligentWindingRobotCtrl::dumpcfg() {
ZLOGI(TAG, "scissors_cut_pos %d", cfg.scissors_cut_pos); ZLOGI(TAG, "scissors_cut_pos %d", cfg.scissors_cut_pos);
ZLOGI(TAG, "arm_hook_claws_half_pos %d", cfg.arm_hook_claws_half_pos); ZLOGI(TAG, "arm_hook_claws_half_pos %d", cfg.arm_hook_claws_half_pos);
ZLOGI(TAG, "arm_hook_claws_full_pos %d", cfg.arm_hook_claws_full_pos); ZLOGI(TAG, "arm_hook_claws_full_pos %d", cfg.arm_hook_claws_full_pos);
#endif
return 0; return 0;
} }
#if 1 #if 1
int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse) { int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse) {
m_dm = dm; m_dm = dm;
m_cmdparse = cmdparse; m_cmdparse = cmdparse;
cmdparse->regCMD("app_paifei_duoji_moveto_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_reset(); });
cmdparse->regCMD("app_paifei_duoji_moveto_press", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_press(); });
cmdparse->regCMD("app_raoxiantance_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return raoxiantance_duoji_move_to_reset(); });
cmdparse->regCMD("app_raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
cfg.xy_platform_takeline_clip00_pos_x = 1736;
cfg.xy_platform_takeline_clip00_pos_y = 16853;
cfg.xy_platform_takeline_clipXX_pos_x = 52307;
cfg.xy_platform_takeline_clipXX_pos_y = 31993;
cfg.clip_line = 12;
cfg.clip_each_line_num = 5;
cfg.arm_jiaxian_duoji_reset_pos = 2619;
cfg.arm_jiaxian_duoji_clamp_torque = 330;
cfg.arm_jiaxian_duoji_clamp_direction = -1;
cfg.z_axis_take_clip_pos = 6924;
cfg.arm_hook_claws_full_pos = 2458;
cfg.arm_hook_claws_half_pos = 2294;
regcb();
return 0;
}
#endif
#define PARAM int32_t paramN, const char **paraV, ICmdParserACK *ack
void IntelligentWindingRobotCtrl::regcb() {
// device_reset
m_cmdparse->regCMD("device_reset", "()", 0, [this](PARAM) { return device_reset(); });
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(); });
#if 0
m_cmdparse->regCMD("app_paifei_duoji_moveto_reset", "()", 0, [this](PARAM) { return paifei_duoji_moveto_reset(); });
m_cmdparse->regCMD("app_paifei_duoji_moveto_press", "()", 0, [this](PARAM) { return paifei_duoji_moveto_press(); });
m_cmdparse->regCMD("app_raoxiantance_duoji_move_to_reset", "()", 0, [this](PARAM) { return raoxiantance_duoji_move_to_reset(); });
m_cmdparse->regCMD("app_raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](PARAM) {
int32_t thickness = 0; int32_t thickness = 0;
int32_t err = raoxiantance_duoji_move_to_get_thickness(ack->getAck(1)); int32_t err = raoxiantance_duoji_move_to_get_thickness(ack->getAck(1));
ack->acktype = ICmdParserACK::kAckType_int32; ack->acktype = ICmdParserACK::kAckType_int32;
ack->rawlen = sizeof(int32_t); ack->rawlen = sizeof(int32_t);
}); });
cmdparse->regCMD("app_yaxian_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_reset(); });
cmdparse->regCMD("app_yaxian_duoji_move_to_press_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_press_pos(); });
cmdparse->regCMD("app_yaxian_duoji_move_to_wait_for_press_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_wait_for_press_pos(); });
cmdparse->regCMD("app_xianlajin_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_reset(); });
cmdparse->regCMD("app_xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_line_entry_pos(); });
cmdparse->regCMD("app_xianlajin_duoji_move_to_tight_line_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_tight_line_pos(); });
cmdparse->regCMD("app_xianlajin_duoji_move_to_loose_line_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xianlajin_duoji_move_to_loose_line_pos(); });
cmdparse->regCMD("app_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return jiaxian_duoji_move_to_reset_pos(); });
cmdparse->regCMD("app_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return jiaxian_duoji_move_to_clamp_pos(); });
cmdparse->regCMD("app_scissors_move_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return scissors_move_reset_pos(); });
cmdparse->regCMD("app_scissors_cut", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return scissors_cut(); });
cmdparse->regCMD("app_arm_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_jiaxian_duoji_move_to_reset_pos(); });
cmdparse->regCMD("app_arm_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_jiaxian_duoji_move_to_clamp_pos(); });
cmdparse->regCMD("app_arm_hook_claws_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_reset(); });
cmdparse->regCMD("app_arm_hook_claws_move_to_half_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_move_to_half_pos(); });
cmdparse->regCMD("app_arm_hook_claws_move_to_full_pos", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return arm_hook_claws_move_to_full_pos(); });
cmdparse->regCMD("app_main_shaft_run", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return main_shaft_run(); });
cmdparse->regCMD("app_main_shaft_stop", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return main_shaft_stop(); });
cmdparse->regCMD("app_xy_platform_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_platform_reset(); });
cmdparse->regCMD("app_z_axis_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return z_axis_reset(); });
cmdparse->regCMD("app_z_axis_move_to", "(int32_t pos)", 1, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return z_axis_move_to(atoi(paraV[0])); });
cmdparse->regCMD("app_xy_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_reset(); });
cmdparse->regCMD("app_xy_move_to_zero", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_move_to_zero(); });
cmdparse->regCMD("app_xy_take_clip", "(int32_t index)", 1, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_clip(atoi(paraV[0])); });
cmdparse->regCMD("app_xy_take_line", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_line(); });
cmdparse->regCMD("app_xy_take_back_clip", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_take_back_clip(); });
cmdparse->regCMD("app_xy_remove_line", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return xy_remove_line(); });
cmdparse->regCMD("app_start_winding", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return start_winding(); });
cmdparse->regCMD("app_stop_winding", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return stop_winding(); });
cmdparse->regCMD("app_reset_and_check_device", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return reset_and_check_device(); });
cmdparse->regCMD("app_setcfg", "(const char* cfgname, int32_t cfgvalue)", 2, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return setcfg(paraV[0], atoi(paraV[1])); });
cmdparse->regCMD("app_dumpcfg", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return dumpcfg(); });
return 0;
m_cmdparse->regCMD("app_yaxian_duoji_move_to_reset", "()", 0, [this](PARAM) { return yaxian_duoji_move_to_reset(); });
m_cmdparse->regCMD("app_yaxian_duoji_move_to_press_pos", "()", 0, [this](PARAM) { return yaxian_duoji_move_to_press_pos(); });
m_cmdparse->regCMD("app_yaxian_duoji_move_to_wait_for_press_pos", "()", 0, [this](PARAM) { return yaxian_duoji_move_to_wait_for_press_pos(); });
m_cmdparse->regCMD("app_xianlajin_duoji_move_to_reset", "()", 0, [this](PARAM) { return xianlajin_duoji_move_to_reset(); });
m_cmdparse->regCMD("app_xianlajin_duoji_move_to_line_entry_pos", "()", 0, [this](PARAM) { return xianlajin_duoji_move_to_line_entry_pos(); });
m_cmdparse->regCMD("app_xianlajin_duoji_move_to_tight_line_pos", "()", 0, [this](PARAM) { return xianlajin_duoji_move_to_tight_line_pos(); });
m_cmdparse->regCMD("app_xianlajin_duoji_move_to_loose_line_pos", "()", 0, [this](PARAM) { return xianlajin_duoji_move_to_loose_line_pos(); });
m_cmdparse->regCMD("app_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](PARAM) { return jiaxian_duoji_move_to_reset_pos(); });
m_cmdparse->regCMD("app_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](PARAM) { return jiaxian_duoji_move_to_clamp_pos(); });
m_cmdparse->regCMD("app_scissors_move_reset_pos", "()", 0, [this](PARAM) { return scissors_move_reset_pos(); });
m_cmdparse->regCMD("app_scissors_cut", "()", 0, [this](PARAM) { return scissors_cut(); });
m_cmdparse->regCMD("app_arm_jiaxian_duoji_move_to_reset_pos", "()", 0, [this](PARAM) { return arm_jiaxian_duoji_move_to_reset_pos(); });
m_cmdparse->regCMD("app_arm_jiaxian_duoji_move_to_clamp_pos", "()", 0, [this](PARAM) { return arm_jiaxian_duoji_move_to_clamp_pos(); });
m_cmdparse->regCMD("app_arm_hook_claws_reset", "()", 0, [this](PARAM) { return arm_hook_claws_reset(); });
m_cmdparse->regCMD("app_arm_hook_claws_move_to_half_pos", "()", 0, [this](PARAM) { return arm_hook_claws_move_to_half_pos(); });
m_cmdparse->regCMD("app_arm_hook_claws_move_to_full_pos", "()", 0, [this](PARAM) { return arm_hook_claws_move_to_full_pos(); });
m_cmdparse->regCMD("app_main_shaft_run", "()", 0, [this](PARAM) { return main_shaft_run(); });
m_cmdparse->regCMD("app_main_shaft_stop", "()", 0, [this](PARAM) { return main_shaft_stop(); });
m_cmdparse->regCMD("app_xy_platform_reset", "()", 0, [this](PARAM) { return xy_platform_reset(); });
m_cmdparse->regCMD("app_z_axis_reset", "()", 0, [this](PARAM) { return z_axis_reset(); });
m_cmdparse->regCMD("app_z_axis_move_to", "(int32_t pos)", 1, [this](PARAM) { return z_axis_move_to(atoi(paraV[0])); });
m_cmdparse->regCMD("app_xy_reset", "()", 0, [this](PARAM) { return xy_reset(); });
m_cmdparse->regCMD("app_xy_move_to_zero", "()", 0, [this](PARAM) { return xy_move_to_zero(); });
m_cmdparse->regCMD("app_xy_take_clip", "(int32_t index)", 1, [this](PARAM) { return xy_take_clip(atoi(paraV[0])); });
m_cmdparse->regCMD("app_xy_take_line", "()", 0, [this](PARAM) { return xy_take_line(); });
m_cmdparse->regCMD("app_xy_take_back_clip", "()", 0, [this](PARAM) { return xy_take_back_clip(); });
m_cmdparse->regCMD("app_xy_remove_line", "()", 0, [this](PARAM) { return xy_remove_line(); });
m_cmdparse->regCMD("app_start_winding", "()", 0, [this](PARAM) { return start_winding(); });
m_cmdparse->regCMD("app_stop_winding", "()", 0, [this](PARAM) { return stop_winding(); });
m_cmdparse->regCMD("app_reset_and_check_device", "()", 0, [this](PARAM) { return reset_and_check_device(); });
m_cmdparse->regCMD("app_setcfg", "(const char* cfgname, int32_t cfgvalue)", 2, [this](PARAM) { return setcfg(paraV[0], atoi(paraV[1])); });
m_cmdparse->regCMD("app_dumpcfg", "()", 0, [this](PARAM) { return dumpcfg(); });
#endif
} }
#endif

42
usrc/intelligent_winding_robot_ctrl.hpp
View File

@ -3,6 +3,10 @@
#include "sdk\components\zprotocols\zcancmder_v2\api\api.hpp" #include "sdk\components\zprotocols\zcancmder_v2\api\api.hpp"
#include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp" #include "sdk\components\zprotocols\zcancmder_v2\zmodule_device_manager.hpp"
#define ZMOTOR_ID 4
#define XYRobot_ID 3
#define XYRobot_HOOK_ID 21
namespace iflytop { namespace iflytop {
using namespace std; using namespace std;
class IntelligentWindingRobotCtrl { class IntelligentWindingRobotCtrl {
@ -48,7 +52,7 @@ class IntelligentWindingRobotCtrl {
* @brief 机械臂夹线舵机 * @brief 机械臂夹线舵机
*/ */
int32_t arm_jiaxian_duoji_reset_pos; int32_t arm_jiaxian_duoji_reset_pos;
int32_t arm_jiaxian_duoji_clamp_pos;
int32_t arm_jiaxian_duoji_clamp_direction;
int32_t arm_jiaxian_duoji_clamp_torque; int32_t arm_jiaxian_duoji_clamp_torque;
/** /**
@ -80,14 +84,46 @@ class IntelligentWindingRobotCtrl {
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;
/**
* @brief
*
*
* 00
*
*
*
* XX
*/
int32_t xy_platform_takeline_clip00_pos_x; //
int32_t xy_platform_takeline_clip00_pos_y;
int32_t xy_platform_takeline_clipXX_pos_x;
int32_t xy_platform_takeline_clipXX_pos_y;
int32_t clip_line;
int32_t clip_each_line_num;
} config_t; } config_t;
ZModuleDeviceManager* m_dm; ZModuleDeviceManager* m_dm;
ICmdParser* m_cmdparse; ICmdParser* m_cmdparse;
config_t cfg; config_t cfg;
private:
int32_t device_reset();
int32_t disable_all_motor();
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_duoji_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);
public: public:
int32_t initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse); int32_t initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse);
void regcb();
int32_t initialize_device(); int32_t initialize_device();
// 排废舵机 // 排废舵机
int32_t paifei_duoji_moveto_reset(); int32_t paifei_duoji_moveto_reset();
@ -96,7 +132,7 @@ class IntelligentWindingRobotCtrl {
* @brief 绕线探测舵机 * @brief 绕线探测舵机
*/ */
int32_t raoxiantance_duoji_move_to_reset(); int32_t raoxiantance_duoji_move_to_reset();
int32_t raoxiantance_duoji_move_to_get_thickness(int32_t *thickness);
int32_t raoxiantance_duoji_move_to_get_thickness(int32_t* thickness);
/** /**
* @brief 压线舵机 * @brief 压线舵机
*/ */
@ -146,7 +182,6 @@ class IntelligentWindingRobotCtrl {
int32_t z_axis_reset(); int32_t z_axis_reset();
int32_t z_axis_move_to(int32_t pos); int32_t z_axis_move_to(int32_t pos);
int32_t xy_reset(); // 复位
int32_t xy_move_to_zero(); // 移动到零位 int32_t xy_move_to_zero(); // 移动到零位
int32_t xy_take_clip(int32_t index); // 取弹夹 int32_t xy_take_clip(int32_t index); // 取弹夹
int32_t xy_take_line(); // 取线 int32_t xy_take_line(); // 取线
@ -163,7 +198,6 @@ class IntelligentWindingRobotCtrl {
int32_t setcfg(const char* cfgname, int32_t cfgvalue); int32_t setcfg(const char* cfgname, int32_t cfgvalue);
int32_t dumpcfg(); int32_t dumpcfg();
int32_t do_reset_device(); int32_t do_reset_device();
int32_t do_winding(int32_t index); int32_t do_winding(int32_t index);

27
usrc/main.cpp
View File

@ -211,13 +211,6 @@ void Main::run() {
*******************************************************************************/ *******************************************************************************/
g_main_servo_motor.init(2, &g_modbusblockhost, 1); g_main_servo_motor.init(2, &g_modbusblockhost, 1);
g_xyrobotctrlmodule.initialize(3, &m_zcanCommnaderMaster); g_xyrobotctrlmodule.initialize(3, &m_zcanCommnaderMaster);
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_run_to_zero_speed, 50);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_run_to_zero_dec, 1600);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_look_zero_edge_speed, 10);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_look_zero_edge_dec, 1600);
g_xyrobotctrlmodule.module_active_cfg();
g_z_step_motor.initialize(4, &m_zcanCommnaderMaster); g_z_step_motor.initialize(4, &m_zcanCommnaderMaster);
g_mini_servo[0].initialize(11, &g_feiteservomotor_bus, 1); g_mini_servo[0].initialize(11, &g_feiteservomotor_bus, 1);
@ -248,6 +241,26 @@ void Main::run() {
g_intelligent_winding_robot_ctrl.initialize(&g_zmodule_device_manager, &g_cmdScheduler); g_intelligent_winding_robot_ctrl.initialize(&g_zmodule_device_manager, &g_cmdScheduler);
/******************************************************************************* /*******************************************************************************
* g_xyrobotctrlmodule *
*******************************************************************************/
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_run_to_zero_speed, 50);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_run_to_zero_dec, 1600);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_look_zero_edge_speed, 10);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_look_zero_edge_dec, 1600);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_default_velocity, 600);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_default_acc, 1000);
g_xyrobotctrlmodule.module_set_param(kcfg_motor_default_dec, 1000);
g_xyrobotctrlmodule.module_set_param(k_cfg_stepmotor_irun, 2);
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_shaft, 0);
g_z_step_motor.module_set_param(kcfg_motor_x_one_circle_pulse, 800);
g_z_step_motor.module_active_cfg();
/*******************************************************************************
* ´®¿Ú×Ö·û´®Ö¸Áî×ÜÏß * * ´®¿Ú×Ö·û´®Ö¸Áî×ÜÏß *
*******************************************************************************/ *******************************************************************************/
g_cmdScheduler.initialize(&DEBUG_UART, 1000); // g_cmdScheduler.initialize(&DEBUG_UART, 1000); //

Write Preview
Loading…
Cancel
Save