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
095f1035ec
commit
2ec1426ca2
3 changed files with 244 additions and 28 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. 2
      sdk
  2. 263
      usrc/intelligent_winding_robot_ctrl.cpp
  3. 7
      usrc/intelligent_winding_robot_ctrl.hpp

2
sdk

@ -1 +1 @@
Subproject commit e06be65d256688d5ab0018e4d195352654081205
Subproject commit 1a752ece8b739fe743348afe09463ed8b09565be

263
usrc/intelligent_winding_robot_ctrl.cpp
View File

@ -1,54 +1,187 @@
#include "intelligent_winding_robot_ctrl.hpp"
#include <stdlib.h>
#include <string.h>
using namespace std;
using namespace iflytop;
#define TAG "IntelligentWindingRobotCtrl"
#define DO(exptr) \
{ \
int32_t ret = exptr; \
if (ret != 0) { \
return ret; \
} \
}
void IntelligentWindingRobotCtrl::processError(int32_t err) { ZLOGE(TAG, "processError: %d", err); }
void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid) {
while (true) {
int32_t status = 0;
int32_t ecode = m_dm->module_get_status(moduleid, &status);
if (ecode != 0) {
processError(ecode);
break;
};
if (status == 0) break;
if (status == 2) {
processError(err::kfail);
break;
};
zos_delay(100);
}
}
int32_t IntelligentWindingRobotCtrl::initialize_device() { return 0; }
// 排废舵机
int32_t IntelligentWindingRobotCtrl::paifei_duoji_moveto_reset() { return 0; }
int32_t IntelligentWindingRobotCtrl::paifei_duoji_moveto_press() { return 0; }
int32_t IntelligentWindingRobotCtrl::paifei_duoji_moveto_reset() {
ZLOGI(TAG, "paifei_duoji_moveto_reset %d %d %d", 15, cfg.paifei_duoji_reset_pos, 330);
DO(m_dm->motor_move_to(15, cfg.paifei_duoji_reset_pos, 2000, 0));
wait_module_idle(15);
return 0;
}
int32_t IntelligentWindingRobotCtrl::paifei_duoji_moveto_press() {
ZLOGI(TAG, "paifei_duoji_moveto_press %d %d %d", 15, cfg.paifei_duoji_press_pos, cfg.paifei_duoji_press_torque);
DO(m_dm->motor_move_to_with_torque(15, cfg.paifei_duoji_press_pos, cfg.paifei_duoji_press_torque));
wait_module_idle(15);
}
/**
* @brief 绕线探测舵机
*/
int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_reset() { return 0; }
int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_get_thickness() { return 0; }
int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_reset() {
ZLOGI(TAG, "raoxiantance_duoji_move_to_reset %d %d %d", 14, cfg.raoxiantance_duoji_reset_pos, 330);
DO(m_dm->motor_move_to(14, cfg.raoxiantance_duoji_reset_pos, 2000, 0));
wait_module_idle(14);
return 0;
}
int32_t IntelligentWindingRobotCtrl::raoxiantance_duoji_move_to_get_thickness(int32_t* thickness) {
ZLOGI(TAG, "raoxiantance_duoji_move_to_get_thickness %d %d", 14, cfg.raoxiantance_duoji_tance_zero_pos);
DO(m_dm->motor_move_to_with_torque(14, cfg.raoxiantance_duoji_tance_zero_pos, 200));
zos_delay(2000);
int32_t nowpos = 0;
DO(m_dm->motor_read_pos(14, &nowpos));
*thickness = cfg.raoxiantance_duoji_tance_zero_pos - nowpos;
DO(raoxiantance_duoji_move_to_reset());
return 0;
}
/**
* @brief 压线舵机
*/
int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_reset() { return 0; }
int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_press_pos() { return 0; }
int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_wait_for_press_pos() { return 0; }
int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_reset() {
ZLOGI(TAG, "yaxian_duoji_move_to_reset %d %d %d", 13, cfg.yaxian_duoji_reset_pos, 330);
DO(m_dm->motor_move_to(13, cfg.yaxian_duoji_reset_pos, 2000, 0));
wait_module_idle(13);
return 0;
}
int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_press_pos() {
ZLOGI(TAG, "yaxian_duoji_move_to_press_pos %d %d %d", 13, cfg.yaxian_duoji_press_pos, cfg.yaxian_duoji_press_torque);
DO(m_dm->motor_move_to_with_torque(13, cfg.yaxian_duoji_press_pos, cfg.yaxian_duoji_press_torque));
wait_module_idle(13);
return 0;
}
int32_t IntelligentWindingRobotCtrl::yaxian_duoji_move_to_wait_for_press_pos() {
ZLOGI(TAG, "yaxian_duoji_move_to_wait_for_press_pos %d %d %d", 13, cfg.yaxian_duoji_wait_for_press_pos, cfg.yaxian_duoji_press_torque);
DO(m_dm->motor_move_to_with_torque(13, cfg.yaxian_duoji_wait_for_press_pos, cfg.yaxian_duoji_press_torque));
wait_module_idle(13);
return 0;
}
/**
* @brief 线拉紧舵机
*/
int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_reset() { return 0; } // 零位
int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_line_entry_pos() { return 0; } // 入线位
int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_tight_line_pos() { return 0; } // 拉线位置
int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_loose_line_pos() { return 0; } // 放线位置
int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_reset() {
ZLOGI(TAG, "xianlajin_duoji_move_to_reset %d %d %d", 12, cfg.xianlajin_duoji_reset_pos, 330);
DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_reset_pos, 2000, 0));
wait_module_idle(16);
return 0;
} // 零位
int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_line_entry_pos() {
ZLOGI(TAG, "xianlajin_duoji_move_to_line_entry_pos %d %d %d", 16, cfg.xianlajin_duoji_line_entry_pos, 330);
DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_line_entry_pos, 2000, 0));
wait_module_idle(16);
return 0;
} // 入线位
int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_tight_line_pos() {
ZLOGI(TAG, "xianlajin_duoji_move_to_tight_line_pos %d %d %d", 16, cfg.xianlajin_duoji_tight_line_pos, 330);
DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_tight_line_pos, 2000, 0));
wait_module_idle(16);
return 0;
} // 拉线位置
int32_t IntelligentWindingRobotCtrl::xianlajin_duoji_move_to_loose_line_pos() {
ZLOGI(TAG, "xianlajin_duoji_move_to_loose_line_pos %d %d %d", 16, cfg.xianlajin_duoji_loose_line_pos, 330);
DO(m_dm->motor_move_to(16, cfg.xianlajin_duoji_loose_line_pos, 2000, 0));
wait_module_idle(16);
return 0;
} // 放线位置
/**
* @brief 夹线舵机
* @brief 夹线舵机12
*/
int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_reset_pos() { return 0; }
int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_clamp_pos() { return 0; }
int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_reset_pos() {
ZLOGI(TAG, "jiaxian_duoji_move_to_reset_pos %d %d %d", 12, cfg.jiaxian_duoji_reset_pos, 330);
DO(m_dm->motor_move_to(12, cfg.jiaxian_duoji_reset_pos, 2000, 0));
wait_module_idle(12);
return 0;
}
int32_t IntelligentWindingRobotCtrl::jiaxian_duoji_move_to_clamp_pos() {
ZLOGI(TAG, "jiaxian_duoji_move_to_clamp_pos %d %d %d", 12, cfg.jiaxian_duoji_clamp_pos, cfg.jiaxian_duoji_clamp_torque);
DO(m_dm->motor_move_to_with_torque(12, cfg.jiaxian_duoji_clamp_pos, cfg.jiaxian_duoji_clamp_torque));
wait_module_idle(12);
return 0;
}
/**
* @brief 剪刀
*/
int32_t IntelligentWindingRobotCtrl::scissors_move_reset_pos() { return 0; } // block
int32_t IntelligentWindingRobotCtrl::scissors_cut() { return 0; } // block
int32_t IntelligentWindingRobotCtrl::scissors_move_reset_pos() {
// ZLOGI(TAG, "scissors_move_reset_pos %d", 11);
ZLOGI(TAG, "scissors_move_reset_pos");
return 0;
} // block
int32_t IntelligentWindingRobotCtrl::scissors_cut() {
ZLOGI(TAG, "scissors_cut %d", 24);
DO(m_dm->motor_move_by(24, 4095, 0, 0));
wait_module_idle(24);
return 0;
} // block
/**
* @brief 机械臂夹线舵机
*/
int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_reset_pos() { return 0; }
int32_t IntelligentWindingRobotCtrl::arm_jiaxian_duoji_move_to_clamp_pos() { return 0; }
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);
DO(m_dm->motor_move_to(11, cfg.arm_jiaxian_duoji_reset_pos, 2000, 0));
wait_module_idle(11);
return 0;
}
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));
wait_module_idle(11);
return 0;
}
/**
* @brief 机械臂钩爪
*/
int32_t IntelligentWindingRobotCtrl::arm_hook_claws_reset() { return 0; }
int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_half_pos() { return 0; }
int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_full_pos() { return 0; }
int32_t IntelligentWindingRobotCtrl::arm_hook_claws_reset() {
ZLOGI(TAG, "arm_hook_claws_reset");
DO(m_dm->motor_move_to_zero_backward(21, 0, 0, 0, 0));
wait_module_idle(21);
return 0;
}
int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_half_pos() {
ZLOGI(TAG, "arm_hook_claws_move_to_half_pos");
DO(m_dm->motor_move_to(21, cfg.arm_hook_claws_half_pos, 0, 0));
wait_module_idle(21);
return 0;
}
int32_t IntelligentWindingRobotCtrl::arm_hook_claws_move_to_full_pos() {
ZLOGI(TAG, "arm_hook_claws_move_to_full_pos");
DO(m_dm->motor_move_to(21, cfg.arm_hook_claws_full_pos, 0, 0));
wait_module_idle(21);
return 0;
}
/**
* @brief XY平台
*/
@ -71,10 +204,82 @@ int32_t IntelligentWindingRobotCtrl::xy_remove_line() { return 0; }
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::setcfg(const char* cfgname, int32_t cfgvalue) { return 0; }
int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; }
int32_t IntelligentWindingRobotCtrl::setcfg(const char* cfgname, int32_t cfgvalue) {
if (strcmp(cfgname, "paifei_duoji_reset_pos") == 0)
cfg.paifei_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "paifei_duoji_press_pos") == 0)
cfg.paifei_duoji_press_pos = cfgvalue;
else if (strcmp(cfgname, "paifei_duoji_press_torque") == 0)
cfg.paifei_duoji_press_torque = cfgvalue;
else if (strcmp(cfgname, "raoxiantance_duoji_reset_pos") == 0)
cfg.raoxiantance_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "raoxiantance_duoji_tance_zero_pos") == 0)
cfg.raoxiantance_duoji_tance_zero_pos = cfgvalue;
else if (strcmp(cfgname, "yaxian_duoji_reset_pos") == 0)
cfg.yaxian_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "yaxian_duoji_press_pos") == 0)
cfg.yaxian_duoji_press_pos = cfgvalue;
else if (strcmp(cfgname, "yaxian_duoji_press_torque") == 0)
cfg.yaxian_duoji_press_torque = cfgvalue;
else if (strcmp(cfgname, "yaxian_duoji_wait_for_press_pos") == 0)
cfg.yaxian_duoji_wait_for_press_pos = cfgvalue;
else if (strcmp(cfgname, "xianlajin_duoji_reset_pos") == 0)
cfg.xianlajin_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "xianlajin_duoji_line_entry_pos") == 0)
cfg.xianlajin_duoji_line_entry_pos = cfgvalue;
else if (strcmp(cfgname, "xianlajin_duoji_tight_line_pos") == 0)
cfg.xianlajin_duoji_tight_line_pos = cfgvalue;
else if (strcmp(cfgname, "xianlajin_duoji_loose_line_pos") == 0)
cfg.xianlajin_duoji_loose_line_pos = cfgvalue;
else if (strcmp(cfgname, "jiaxian_duoji_reset_pos") == 0)
cfg.jiaxian_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "jiaxian_duoji_clamp_pos") == 0)
cfg.jiaxian_duoji_clamp_pos = cfgvalue;
else if (strcmp(cfgname, "jiaxian_duoji_clamp_torque") == 0)
cfg.jiaxian_duoji_clamp_torque = cfgvalue;
else if (strcmp(cfgname, "arm_jiaxian_duoji_reset_pos") == 0)
cfg.arm_jiaxian_duoji_reset_pos = cfgvalue;
else if (strcmp(cfgname, "arm_jiaxian_duoji_clamp_pos") == 0)
cfg.arm_jiaxian_duoji_clamp_pos = cfgvalue;
else if (strcmp(cfgname, "arm_jiaxian_duoji_clamp_torque") == 0)
cfg.arm_jiaxian_duoji_clamp_torque = cfgvalue;
else if (strcmp(cfgname, "scissors_reset_pos") == 0)
cfg.scissors_reset_pos = cfgvalue;
else if (strcmp(cfgname, "scissors_cut_pos") == 0)
cfg.scissors_cut_pos = cfgvalue;
else if (strcmp(cfgname, "arm_hook_claws_half_pos") == 0)
cfg.arm_hook_claws_half_pos = cfgvalue;
else if (strcmp(cfgname, "arm_hook_claws_full_pos") == 0)
cfg.arm_hook_claws_full_pos = cfgvalue;
return 0;
}
int32_t IntelligentWindingRobotCtrl::dumpcfg() {
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_torque %d", cfg.paifei_duoji_press_torque);
ZLOGI(TAG, "raoxiantance_duoji_reset_pos %d", cfg.raoxiantance_duoji_reset_pos);
ZLOGI(TAG, "raoxiantance_duoji_tance_zero_pos %d", cfg.raoxiantance_duoji_tance_zero_pos);
ZLOGI(TAG, "yaxian_duoji_reset_pos %d", cfg.yaxian_duoji_reset_pos);
ZLOGI(TAG, "yaxian_duoji_press_pos %d", cfg.yaxian_duoji_press_pos);
ZLOGI(TAG, "yaxian_duoji_press_torque %d", cfg.yaxian_duoji_press_torque);
ZLOGI(TAG, "yaxian_duoji_wait_for_press_pos %d", cfg.yaxian_duoji_wait_for_press_pos);
ZLOGI(TAG, "xianlajin_duoji_reset_pos %d", cfg.xianlajin_duoji_reset_pos);
ZLOGI(TAG, "xianlajin_duoji_line_entry_pos %d", cfg.xianlajin_duoji_line_entry_pos);
ZLOGI(TAG, "xianlajin_duoji_tight_line_pos %d", cfg.xianlajin_duoji_tight_line_pos);
ZLOGI(TAG, "xianlajin_duoji_loose_line_pos %d", cfg.xianlajin_duoji_loose_line_pos);
ZLOGI(TAG, "jiaxian_duoji_reset_pos %d", cfg.jiaxian_duoji_reset_pos);
ZLOGI(TAG, "jiaxian_duoji_clamp_pos %d", cfg.jiaxian_duoji_clamp_pos);
ZLOGI(TAG, "jiaxian_duoji_clamp_torque %d", cfg.jiaxian_duoji_clamp_torque);
ZLOGI(TAG, "arm_jiaxian_duoji_reset_pos %d", cfg.arm_jiaxian_duoji_reset_pos);
ZLOGI(TAG, "arm_jiaxian_duoji_clamp_pos %d", cfg.arm_jiaxian_duoji_clamp_pos);
ZLOGI(TAG, "arm_jiaxian_duoji_clamp_torque %d", cfg.arm_jiaxian_duoji_clamp_torque);
ZLOGI(TAG, "scissors_reset_pos %d", cfg.scissors_reset_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_full_pos %d", cfg.arm_hook_claws_full_pos);
return 0;
}
#if 1
int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse) {
m_dm = dm;
m_cmdparse = cmdparse;
@ -82,7 +287,12 @@ int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdPa
cmdparse->regCMD("paifei_duoji_moveto_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_reset(); });
cmdparse->regCMD("paifei_duoji_moveto_press", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return paifei_duoji_moveto_press(); });
cmdparse->regCMD("raoxiantance_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return raoxiantance_duoji_move_to_reset(); });
cmdparse->regCMD("raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return raoxiantance_duoji_move_to_get_thickness(); });
cmdparse->regCMD("raoxiantance_duoji_move_to_get_thickness", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) {
int32_t thickness = 0;
int32_t err = raoxiantance_duoji_move_to_get_thickness(ack->getAck(1));
ack->acktype = ICmdParserACK::kAckType_int32;
ack->rawlen = sizeof(int32_t);
});
cmdparse->regCMD("yaxian_duoji_move_to_reset", "()", 0, [this](int32_t paramN, const char** paraV, ICmdParserACK* ack) { return yaxian_duoji_move_to_reset(); });
cmdparse->regCMD("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("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(); });
@ -116,3 +326,4 @@ int32_t IntelligentWindingRobotCtrl::initialize(ZModuleDeviceManager* dm, ICmdPa
return 0;
}
#endif

7
usrc/intelligent_winding_robot_ctrl.hpp
View File

@ -84,6 +84,7 @@ class IntelligentWindingRobotCtrl {
ZModuleDeviceManager* m_dm;
ICmdParser* m_cmdparse;
config_t cfg;
public:
int32_t initialize(ZModuleDeviceManager* dm, ICmdParser* cmdparse);
@ -95,7 +96,7 @@ class IntelligentWindingRobotCtrl {
* @brief ÈÆÏß̽²âæ»ú
*/
int32_t raoxiantance_duoji_move_to_reset();
int32_t raoxiantance_duoji_move_to_get_thickness();
int32_t raoxiantance_duoji_move_to_get_thickness(int32_t *thickness);
/**
* @brief ѹÏßæ»ú
*/
@ -156,6 +157,10 @@ class IntelligentWindingRobotCtrl {
int32_t setcfg(const char* cfgname, int32_t cfgvalue);
int32_t dumpcfg();
void processError(int32_t err);
void wait_module_idle(int32_t moduleid);
};
} // namespace iflytop
Write Preview
Loading…
Cancel
Save