zuniversal_hardware_driver
/
iflytop_cancmder
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
正点原子开发板 alientek_develop_board cancmder
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
55 Commits
1 Branch
0 Tags
1.7 MiB
 
 
 
Tree: 2cc445c927
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '2cc445c927'
${ noResults }
iflytop_cancmder/usrc/intelligent_winding_robot_c...

655 lines
23 KiB
Raw Blame History

#include "intelligent_winding_robot_ctrl.hpp"
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>
#include <exception>
using namespace std;
using namespace iflytop;
#define TAG "APPDM"
#define DO(exptr) \
{ \
int32_t ret = exptr; \
if (ret != 0) { \
return ret; \
} \
}
#define PARAM int32_t paramN, const char **paraV, ICmdParserACK *ack
void IntelligentWindingRobotCtrl::processError(int32_t err) { ZLOGE(TAG, "processError: %d", err); }
class myexception : public exception {
virtual const char* what() const throw() { return "My exception happened"; }
};
int32_t IntelligentWindingRobotCtrl::initialize(APPDM* dm, ICmdParser* cmdparse) {
m_dm = dm;
m_cmdparse = cmdparse;
initialize_device();
regcb();
return 0;
}
int32_t IntelligentWindingRobotCtrl::initialize_device() {
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.m11_arm_jiaxian_reset_pos = 2619;
cfg.m11_arm_jiaxian_clamp_torque = 330;
cfg.m11_arm_jiaxian_clamp_direction = -1;
cfg.m12_jiaxian_reset_pos = 2600;
cfg.m12_jiaxian_clamp_direction = -1;
cfg.m12_jiaxian_clamp_torque = 330;
cfg.m13_yaxian_forward_reset_pos = 1015;
cfg.m13_yaxian_backward_reset_pos = 2885;
cfg.m13_jiaxian_clamp_direction = -1;
cfg.m13_jiaxian_clamp_torque = 200;
cfg.m14_raoxiantance_reset_pos = 2047;
cfg.m14_raoxiantance_tance_zero_pos = 2517;
cfg.m15_paifei_reset_pos = 2046;
cfg.m15_paifei_press_direction = 1;
cfg.m15_paifei_press_torque = 330;
cfg.m16_xianlajin_reset_pos = 2047;
cfg.m16_xianlajin_tight_line_pos = 1966;
cfg.m16_xianlajin_winding_low_pos = 1900;
cfg.m16_xianlajin_winding_up_pos = 1865;
cfg.m16_xianlajin_line_entry_pos = 1833;
cfg.m21_arm_hook_claws_full_pos = 2558;
cfg.m21_arm_hook_claws_half_pos = 2294;
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.xy_platform_takeline_pos_x = 37359;
cfg.xy_platform_takeline_pos_y = 7047;
cfg.xy_platform_enter_line_pos_x = 17625;
cfg.xy_platform_enter_line_pos_y = 7001;
//
cfg.z_axis_cook_bullet_pos = 3377;
cfg.z_axis_take_clip_pos = 6924;
cfg.z_axis_take_line_high = 3400;
cfg.z_axis_transfer_line_high = 2785;
return 0;
}
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("step_take_bullet", "()", 1, [this](PARAM) { return step_take_bullet(atoi(paraV[0])); });
m_cmdparse->regCMD("step_take_back_bullet", "()", 1, [this](PARAM) { return step_take_back_bullet(atoi(paraV[0])); });
m_cmdparse->regCMD("step_prepare_remove_line", "()", 1, [this](PARAM) { return step_prepare_remove_line(atoi(paraV[0])); });
m_cmdparse->regCMD("step_winding_prepare", "()", 1, [this](PARAM) { return step_winding_prepare(); });
// 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(); });
m_cmdparse->regCMD("app_m12_jiaxian_move_to_open_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_open_pos(); });
m_cmdparse->regCMD("app_m12_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_clamp_pos(); });
m_cmdparse->regCMD("app_m13_yaxian_move_to_reset_forward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_forward(); });
m_cmdparse->regCMD("app_m13_yaxian_move_to_reset_backward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_backward(); });
m_cmdparse->regCMD("app_m13_yaxian_press_clip", "()", 0, [this](PARAM) { return m13_yaxian_press_clip(); });
m_cmdparse->regCMD("app_m14_raoxiantance_move_to_reset", "()", 0, [this](PARAM) { return m14_raoxiantance_move_to_reset(); });
m_cmdparse->regCMD("app_m15_paifei_moveto_reset", "()", 0, [this](PARAM) { return m15_paifei_moveto_reset(); });
m_cmdparse->regCMD("app_m15_paifei_moveto_press", "()", 0, [this](PARAM) { return m15_paifei_moveto_press(); });
m_cmdparse->regCMD("app_m16_xianlajin_move_to_reset", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_reset(); });
m_cmdparse->regCMD("app_m16_xianlajin_move_to_tight_line_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_tight_line_pos(); });
m_cmdparse->regCMD("app_m16_xianlajin_move_to_winding_low_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_winding_low_pos(); });
m_cmdparse->regCMD("app_m16_xianlajin_move_to_winding_up_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_winding_up_pos(); });
m_cmdparse->regCMD("app_m16_xianlajin_move_to_line_entry_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_line_entry_pos(); });
m_cmdparse->regCMD("app_m21_arm_hook_claws_reset", "()", 0, [this](PARAM) { return m21_arm_hook_claws_reset(); });
m_cmdparse->regCMD("app_m21_arm_hook_claws_move_to_half_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_half_pos(); });
m_cmdparse->regCMD("app_m21_arm_hook_claws_move_to_full_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_full_pos(); });
m_cmdparse->regCMD("app_m22_scissors_move_reset_pos", "()", 0, [this](PARAM) { return m22_scissors_move_reset_pos(); });
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(); });
}
void IntelligentWindingRobotCtrl::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) {
processError(ecode);
break;
};
if (status == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
break;
}
if (status == 2) {
processError(err::kfail);
break;
};
if (i % 30 == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
}
i++;
zos_delay(100);
}
}
void IntelligentWindingRobotCtrl::wait_modules_idle(void* mark, ...) {
int32_t moduleid = 0;
va_list args;
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轴复位
/**
* @
*
* 1. Z轴及其组件复位
* 2. 除了M13外，其他电机复位
* 3. M13复位,等待一定时间，查看M13是否复位成功
* 4. 机械臂复位
*/
enable_all_module();
m4_zreset();
m11_arm_jiaxian_move_to_reset_pos();
WAIT_MODULES_IDLE(4, 11);
m12_jiaxian_move_to_clamp_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);
m21_arm_hook_claws_reset();
wait_module_idle(21);
ZLOGI(TAG, "device_reset finished....");
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_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_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_torque(13, cfg.m13_yaxian_forward_reset_pos, 330, 0); }
int32_t IntelligentWindingRobotCtrl::m13_yaxian_move_to_reset_backward() { return m_dm->motor_move_to_torque(13, cfg.m13_yaxian_backward_reset_pos, 100, 0); }
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_torque(14, cfg.m14_raoxiantance_reset_pos, 330, 0); }
int32_t IntelligentWindingRobotCtrl::m15_paifei_moveto_reset() { return m_dm->motor_move_to_torque(15, cfg.m15_paifei_reset_pos, 330, 0); }
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); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_winding_up_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_winding_up_pos, 100, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_line_entry_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_line_entry_pos, 100, 0); }
int32_t IntelligentWindingRobotCtrl::m21_arm_hook_claws_reset() { return m_dm->motor_move_to_zero_backward(21, 0, 0, 0, 0); }
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_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::step_take_bullet(int32_t bulletindex) {
m4_zreset();
WAIT_MODULES_IDLE(4);
xy_reset();
WAIT_MODULES_IDLE(3);
m21_arm_hook_claws_reset();
m11_arm_jiaxian_move_to_reset_pos();
WAIT_MODULES_IDLE(3, 4, 11, 21);
int32_t x = 0;
int32_t y = 0;
xy_get_point(bulletindex, x, y);
xymove_to(x, y);
WAIT_MODULES_IDLE(3);
m4_zmove_to(cfg.z_axis_take_clip_pos);
WAIT_MODULES_IDLE(4);
m21_arm_hook_claws_move_to_full_pos();
m11_arm_jiaxian_move_to_clamp_pos();
WAIT_MODULES_IDLE(21, 11);
m4_zmove_to(0);
WAIT_MODULES_IDLE(4);
m4_zreset();
WAIT_MODULES_IDLE(4);
}
int32_t IntelligentWindingRobotCtrl::step_take_back_bullet(int32_t bulletindex) {
m4_zmove_to(0);
WAIT_MODULES_IDLE(4);
xymove_to_bullet_pos(bulletindex);
WAIT_MODULES_IDLE(3);
m4_zmove_to(cfg.z_axis_take_clip_pos);
WAIT_MODULES_IDLE(4);
m21_arm_hook_claws_reset();
m11_arm_jiaxian_move_to_reset_pos();
WAIT_MODULES_IDLE(21, 11);
m4_zmove_to(0);
WAIT_MODULES_IDLE(4);
m4_zreset();
WAIT_MODULES_IDLE(4);
}
int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line(int32_t bulletindex) {
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 // 20mm
* |
* COOK()
*
*
*/
if (is_hasbullet()) {
xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y - 3623);
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_take_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);
step_take_back_bullet(bulletindex);
xymove_to(0, 0);
WAIT_MODULES_IDLE(3);
} else {
/**
* @brief TODO:如果没有子弹，需要在这里做的事情
*/
}
}
int32_t IntelligentWindingRobotCtrl::step_remove_line() {
ZLOGI(TAG, "step_remove_line");
start_probe_bullet_pos();
m15_paifei_moveto_press();
WAIT_MODULES_IDLE(15);
m_dm->motor_rotate_acctime(2, -1, 1000, 1000);
/**
* @TODO:添加排线结束检测
*/
for (size_t i = 0; i < 10; i++) {
osDelay(1000);
}
m_dm->module_stop(2);
stop_probe_bullet_pos();
// m15_paifei_moveto_reset();
return 0;
}
int32_t IntelligentWindingRobotCtrl::step_winding_prepare() {
m4_zmove_to(0);
WAIT_MODULES_IDLE(4);
xymove_to(cfg.xy_platform_takeline_pos_x, cfg.xy_platform_takeline_pos_y);
WAIT_MODULES_IDLE(3);
// 线夹张开
m11_arm_jiaxian_move_to_reset_pos();
WAIT_MODULES_IDLE(11);
// Z轴移动到夹线位置
m4_zmove_to(cfg.z_axis_take_line_high);
WAIT_MODULES_IDLE(4);
// 夹线
m11_arm_jiaxian_move_to_clamp_pos();
WAIT_MODULES_IDLE(11);
// 线夹张开
m12_jiaxian_move_to_open_pos();
WAIT_MODULES_IDLE(12);
// 移动到转移线高度
m16_xianlajin_move_to_line_entry_pos();
WAIT_MODULES_IDLE(16);
m4_zmove_to(cfg.z_axis_transfer_line_high);
WAIT_MODULES_IDLE(4);
// 移动到转移线位置
xymove_to(cfg.xy_platform_takeline_pos_x, cfg.xy_platform_takeline_pos_y + 4000);
WAIT_MODULES_IDLE(4);
xymove_to(cfg.xy_platform_enter_line_pos_x, cfg.xy_platform_enter_line_pos_y + 4000);
WAIT_MODULES_IDLE(4);
xymove_to(cfg.xy_platform_enter_line_pos_x, cfg.xy_platform_enter_line_pos_y);
WAIT_MODULES_IDLE(3);
m_dm->motor_enable(13,0);
// m13_yaxian_press_clip();
// WAIT_MODULES_IDLE(13);
}
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;
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;
}
#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;
xy_get_point(bulletindex, x, y);
return xymove_to(x, y);
}
int32_t IntelligentWindingRobotCtrl::xymove_to(int32_t x, int32_t 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);
if (y > nowy) {
m_dm->xymotor_move_to(XYRobot_ID, nowx, y, 0);
WAIT_MODULES_IDLE(3);
m_dm->xymotor_move_to(XYRobot_ID, x, y, 0);
WAIT_MODULES_IDLE(3);
} else {
m_dm->xymotor_move_to(XYRobot_ID, x, nowy, 0);
WAIT_MODULES_IDLE(3);
m_dm->xymotor_move_to(XYRobot_ID, x, y, 0);
WAIT_MODULES_IDLE(3);
}
return 0;
}
int32_t IntelligentWindingRobotCtrl::xy_reset() {
ZLOGI(TAG, "xy_reset");
m_dm->xymotor_move_to_zero(XYRobot_ID);
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
return 0;
}
int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; }
#if 1
#endif