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.
66 Commits
1 Branch
0 Tags
1.7 MiB
 
 
 
Tree: 0a01347d1e
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '0a01347d1e'
${ noResults }
iflytop_cancmder/usrc/intelligent_winding_robot_c...

1041 lines
36 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"; }
};
class WidthDetector {
ZGPIO g_detectGpio;
int32_t g_nowpos = 0;
int32_t g_nowdpos = 0;
bool g_isrunning = false;
ZThread m_detect_thread;
int32_t m_lastState = false;
IntelligentWindingRobotCtrl::config_t* cfg;
APPDM* m_dm;
int32_t m_bullet_distance = 33; // 理论值33,这里取保守值20
int32_t m_bullet_full_distance = 127;
public:
void init(IntelligentWindingRobotCtrl::config_t* cfg, APPDM* dm) {
this->cfg = cfg;
m_dm = dm;
g_detectGpio.initAsInput(PB13, ZGPIO::kMode_pullup, ZGPIO::kIRQ_noIrq, true);
m_detect_thread.init("detect", 512);
getDetectGPIOState();
}
int32_t getDetectGPIOState() {
int32_t nowstate = g_detectGpio.getState();
for (size_t i = 0; i < 5; i++) {
if (g_detectGpio.getState() != nowstate) {
return m_lastState;
}
osDelay(3);
}
m_lastState = nowstate;
return m_lastState;
}
void wait_module_idle(int32_t moduleid, int32_t timeout_ms) {
zos_delay(100);
int i = 0;
int32_t enterticket = zos_get_tick();
while (true) {
int32_t status = 0;
int32_t ecode = m_dm->module_get_status(moduleid, &status);
if (ecode != 0) {
break;
};
if (timeout_ms != 0 && zos_haspassedms(enterticket) > timeout_ms) {
ZLOGE(TAG, "wait_module_idle timeout");
throw (int32_t)err::kmotor_run_overtime;
}
if (status == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
break;
}
if (status == 2) {
throw (int32_t)err::kcatch_exception;
break;
};
if (i % 30 == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
}
i++;
zos_delay(10);
}
}
void m14_raoxiantance_move_to_reset() { m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0); }
void start_run_back() {
m_dm->motor_move_to(14, cfg->m14_raoxiantance_reset_pos, 30, 0);
g_isrunning = true;
}
void start_run_forward_slow() {
m_dm->motor_move_to(14, cfg->m14_raoxiantance_tance_zero_pos + 100, 30, 0);
g_isrunning = true;
}
void start_run_forward() {
m_dm->motor_move_to(14, cfg->m14_raoxiantance_tance_zero_pos + 100, 330, 0);
g_isrunning = true;
}
void stop_run() {
m_dm->module_stop(14);
g_isrunning = false;
}
void startDetect(bool forward = false) {
m_detect_thread.stop();
m14_raoxiantance_move_to_reset();
wait_module_idle(14, 0);
int32_t enterticket = zos_get_tick();
start_run_forward();
while (true) {
if (g_isrunning && getDetectGPIOState() != 0) {
stop_run();
break;
}
if (zos_haspassedms(enterticket) > 3000) {
ZLOGE(TAG, "start_probe_bullet_pos timeout");
/**
* @brief TODO add exception here
*
*/
stop_run();
break;
}
zos_delay(10);
}
start_run_back();
while (true) {
if (g_isrunning && getDetectGPIOState() == 0) {
stop_run();
m_dm->motor_read_pos(14, &g_nowpos);
g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos;
ZLOGI(TAG, "--------------now pos %d bullet width %d", g_nowpos, g_nowdpos);
break;
}
zos_delay(3);
}
m_detect_thread.start([this, forward]() {
if (!forward) {
while (!m_detect_thread.getExitFlag()) {
if (g_isrunning && getDetectGPIOState() == 0) {
stop_run();
m_dm->motor_read_pos(14, &g_nowpos);
g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos;
ZLOGI(TAG, "--------------now pos %d bullet width %d", g_nowpos, g_nowdpos);
} else if (!g_isrunning && getDetectGPIOState() != 0) {
start_run_back();
}
zos_delay(3);
}
} else {
while (!m_detect_thread.getExitFlag()) {
if (g_isrunning && getDetectGPIOState() != 0) {
stop_run();
m_dm->motor_read_pos(14, &g_nowpos);
g_nowdpos = cfg->m14_raoxiantance_tance_zero_pos - g_nowpos;
ZLOGI(TAG, "--------------now pos %d bullet width %d", g_nowpos, g_nowdpos);
} else if (!g_isrunning && getDetectGPIOState() == 0) {
start_run_forward_slow();
}
zos_delay(3);
}
}
});
}
void stopDetect() {
m_detect_thread.stop();
m_dm->motor_move_to_torque(14, cfg->m14_raoxiantance_reset_pos, 330, 0);
wait_module_idle(14, 0);
}
bool isFull() { return g_nowdpos > m_bullet_full_distance - 20; }
bool isHasBullet() { return g_nowdpos > 5; }
bool isRemoveLineEnd() { return g_nowdpos < m_bullet_distance + 10; }
};
WidthDetector g_widthDetector;
int32_t IntelligentWindingRobotCtrl::initialize(APPDM* dm, ICmdParser* cmdparse) {
m_dm = dm;
m_cmdparse = cmdparse;
m_work_thread.init("work", 512);
// m_detect_thread.init("detect", 512);
initialize_device();
regcb();
g_widthDetector.init(&cfg, dm);
return 0;
}
int32_t IntelligentWindingRobotCtrl::initialize_device() {
cfg.xy_platform_takeline_clip00_pos_x = 1833;
cfg.xy_platform_takeline_clip00_pos_y = 16819;
cfg.xy_platform_takeline_clipXX_pos_x = 52381;
cfg.xy_platform_takeline_clipXX_pos_y = 32087;
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 = 600;
cfg.m14_raoxiantance_reset_pos = 2047;
cfg.m14_raoxiantance_tance_zero_pos = 2829;
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_cook_line_end_ready_pos = 1900;
cfg.m16_xianlajin_tight_line_pos = 1966;
cfg.m16_xianlajin_winding_low_pos = 1885;
cfg.m16_xianlajin_winding_high_pos = 1815;
cfg.m16_xianlajin_line_entry_pos = 1800;
cfg.m16_xianlajin_cook_line_end_low_pos = 1820;
cfg.m16_xianlajin_cook_line_end_high_pos = 1741;
cfg.m21_arm_hook_claws_full_pos = 2558;
cfg.m21_arm_hook_claws_half_pos = 2194;
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 = -599;
cfg.xy_platform_remove_line_pos_y = 4861;
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 = 6600;
//
cfg.z_axis_cook_bullet_pos = 3400; // 3277
cfg.z_axis_take_clip_pos = 6850;
cfg.z_axis_take_line_high = 3500;
cfg.z_axis_transfer_line_high = 2275;
cfg.z_axis_remove_line_high = 3567;
cfg.m2_zerooff = 1110;
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) {
bool hasbullet = false;
return step_prepare_remove_line(atoi(paraV[0]), hasbullet);
});
m_cmdparse->regCMD("step_winding_prepare", "()", 0, [this](PARAM) { return step_winding_prepare(); });
m_cmdparse->regCMD("step_winding", "()", 0, [this](PARAM) { return step_winding(); });
m_cmdparse->regCMD("step_remove_line", "()", 0, [this](PARAM) { return step_remove_line(); });
m_cmdparse->regCMD("step_winding_lineend", "()", 0, [this](PARAM) { return step_winding_lineend(); });
m_cmdparse->regCMD("step_winding_lineend_prepare", "()", 1, [this](PARAM) { return step_winding_lineend_prepare(atoi(paraV[0])); });
m_cmdparse->regCMD("step_winding_take_bullet_from_cooking_to_origin_pos", "()", 1, [this](PARAM) { return step_winding_take_bullet_from_cooking_to_origin_pos(atoi(paraV[0])); });
m_cmdparse->regCMD("start_winding", "()", 0, [this](PARAM) { return start_winding(); });
m_cmdparse->regCMD("stop_winding", "()", 0, [this](PARAM) { return stop_winding(); });
// m_cmdparse->regCMD("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); });
m_cmdparse->regCMD("m11_arm_jiaxian_move_to_reset_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_reset_pos(); });
m_cmdparse->regCMD("m11_arm_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m11_arm_jiaxian_move_to_clamp_pos(); });
m_cmdparse->regCMD("m12_jiaxian_move_to_open_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_open_pos(); });
m_cmdparse->regCMD("m12_jiaxian_move_to_clamp_pos", "()", 0, [this](PARAM) { return m12_jiaxian_move_to_clamp_pos(); });
m_cmdparse->regCMD("m13_yaxian_move_to_reset_forward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_forward(); });
m_cmdparse->regCMD("m13_yaxian_move_to_reset_backward", "()", 0, [this](PARAM) { return m13_yaxian_move_to_reset_backward(); });
m_cmdparse->regCMD("m13_yaxian_press_clip", "()", 0, [this](PARAM) { return m13_yaxian_press_clip(); });
m_cmdparse->regCMD("m14_raoxiantance_move_to_reset", "()", 0, [this](PARAM) { return m14_raoxiantance_move_to_reset(); });
m_cmdparse->regCMD("m15_paifei_moveto_reset", "()", 0, [this](PARAM) { return m15_paifei_moveto_reset(); });
m_cmdparse->regCMD("m15_paifei_moveto_press", "()", 0, [this](PARAM) { return m15_paifei_moveto_press(); });
m_cmdparse->regCMD("m16_xianlajin_move_to_reset", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_reset(); });
m_cmdparse->regCMD("m16_xianlajin_move_to_tight_line_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_tight_line_pos(); });
m_cmdparse->regCMD("m16_xianlajin_move_to_winding_low_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_winding_low_pos(); });
m_cmdparse->regCMD("m16_xianlajin_move_to_winding_up_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_winding_up_pos(); });
m_cmdparse->regCMD("m16_xianlajin_move_to_line_entry_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_line_entry_pos(); });
m_cmdparse->regCMD("m16_xianlajin_move_to_cook_lineend_high_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_cook_lineend_high_pos(); });
m_cmdparse->regCMD("m16_xianlajin_move_to_cook_lineend_low_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_cook_lineend_low_pos(); });
m_cmdparse->regCMD("m16_xianlajin_move_to_cook_lineend_ready_pos", "()", 0, [this](PARAM) { return m16_xianlajin_move_to_cook_lineend_ready_pos(); });
m_cmdparse->regCMD("m21_arm_hook_claws_reset", "()", 0, [this](PARAM) { return m21_arm_hook_claws_reset(); });
m_cmdparse->regCMD("m21_arm_hook_claws_move_to_half_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_half_pos(); });
m_cmdparse->regCMD("m21_arm_hook_claws_move_to_full_pos", "()", 0, [this](PARAM) { return m21_arm_hook_claws_move_to_full_pos(); });
m_cmdparse->regCMD("m22_scissors_move_reset_pos", "()", 0, [this](PARAM) { return m22_scissors_move_reset_pos(); });
m_cmdparse->regCMD("m22_scissors_cut", "()", 0, [this](PARAM) { return m22_scissors_cut(); });
m_cmdparse->regCMD("m23_laxian_motor_move_to_reset_pos", "()", 0, [this](PARAM) { return m23_laxian_motor_move_to_reset_pos(); });
m_cmdparse->regCMD("m23_laxian_motor_move_to_tight_line_pos", "()", 0, [this](PARAM) { return m23_laxian_motor_move_to_tight_line_pos(); });
m_cmdparse->regCMD("start_probe_bullet_pos", "()", 0, [this](PARAM) { return start_probe_bullet_pos(); });
m_cmdparse->regCMD("start_probe_bullet_pos_forward", "()", 0, [this](PARAM) { return start_probe_bullet_pos_forward(); });
}
void IntelligentWindingRobotCtrl::wait_module_idle(int32_t moduleid, int32_t timeout_ms) {
zos_delay(100);
int i = 0;
int32_t enterticket = zos_get_tick();
while (true) {
int32_t status = 0;
int32_t ecode = m_dm->module_get_status(moduleid, &status);
if (ecode != 0) {
break;
};
if (timeout_ms != 0 && zos_haspassedms(enterticket) > timeout_ms) {
ZLOGE(TAG, "wait_module_idle timeout");
throw (int32_t)err::kmotor_run_overtime;
}
if (status == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
break;
}
if (status == 2) {
throw (int32_t)err::kcatch_exception;
break;
};
if (i % 30 == 0) {
ZLOGI(TAG, "wait_module_idle %d %d....", moduleid, status);
}
i++;
zos_delay(10);
}
}
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::stop_all_module() {
m_dm->module_stop(2);
m_dm->module_stop(3);
m_dm->module_stop(4);
m_dm->module_stop(11);
m_dm->module_stop(12);
m_dm->module_stop(13);
m_dm->module_stop(14);
m_dm->module_stop(15);
m_dm->module_stop(16);
m_dm->module_stop(21);
m_dm->module_stop(22);
m_dm->module_stop(23);
stop_probe_bullet_pos();
}
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);
stop_all_module();
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. 机械臂复位
*/
m_dm->module_stop(2);
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);
m22_scissors_move_reset_pos();
wait_module_idle(22);
/**
* @brief 优化复位逻辑，超时复位失败，报警
*/
m13_yaxian_move_to_reset_backward();
wait_module_idle(13, 5000);
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 g_widthDetector.isHasBullet(); }
void IntelligentWindingRobotCtrl::start_probe_bullet_pos() { g_widthDetector.startDetect(); }
void IntelligentWindingRobotCtrl::start_probe_bullet_pos_forward() { g_widthDetector.startDetect(true); }
void IntelligentWindingRobotCtrl::stop_probe_bullet_pos() { g_widthDetector.stopDetect(); }
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, 300, 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_high_pos, 20, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_line_entry_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_line_entry_pos, 20, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_cook_lineend_high_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_cook_line_end_high_pos, 300, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_cook_lineend_low_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_cook_line_end_low_pos, 300, 0); }
int32_t IntelligentWindingRobotCtrl::m16_xianlajin_move_to_cook_lineend_ready_pos() { return m_dm->motor_move_to(16, cfg.m16_xianlajin_cook_line_end_ready_pos, 300, 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() {
m_dm->motor_move_to_zero_backward(22, 0, 0, 0, 0);
WAIT_MODULES_IDLE(22);
m_dm->motor_move_to(22, 1250, 0, 0);
}
int32_t IntelligentWindingRobotCtrl::m22_scissors_cut() { return m_dm->motor_move_to(22, 2500, 0, 0); }
int32_t IntelligentWindingRobotCtrl::m23_laxian_motor_move_to_reset_pos() { return m_dm->motor_move_to_torque(23, 2040, 200, 0); }
int32_t IntelligentWindingRobotCtrl::m23_laxian_motor_move_to_tight_line_pos() { return m_dm->motor_move_to_torque(23, 1800, 40, 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::substep_zaxis_do_bullet_action(take_bullet_pos_type_t zpos, //
take_bullet_acktion_t take_bullet_acktion, //
take_bullet_line_acktion_t take_bullet_line_acktion, //
function<void()> bottomoperation) {
if (take_bullet_line_acktion == kTakeLine) {
m11_arm_jiaxian_move_to_reset_pos();
} else if (take_bullet_line_acktion == kReleaseLine) {
} else if (take_bullet_line_acktion == kKeepLine) {
}
WAIT_MODULES_IDLE(11);
if (zpos == kCookPos) {
m4_zmove_to(cfg.z_axis_cook_bullet_pos);
} else if (zpos == kBulletBulletHolderPos) {
m4_zmove_to(cfg.z_axis_take_clip_pos);
}
WAIT_MODULES_IDLE(4);
if (take_bullet_line_acktion == kTakeLine) {
m11_arm_jiaxian_move_to_clamp_pos();
} else if (take_bullet_line_acktion == kReleaseLine) {
m11_arm_jiaxian_move_to_reset_pos();
} else if (take_bullet_line_acktion == kKeepLine) {
}
if (take_bullet_acktion == kTakeBullet) {
m21_arm_hook_claws_move_to_full_pos();
} else if (take_bullet_acktion == kTakeBackBullet) {
m21_arm_hook_claws_reset();
} else if (take_bullet_acktion == kTakeBulletCase) {
m21_arm_hook_claws_move_to_half_pos();
} else if (take_bullet_acktion == kTakeBackBulletCase) {
m21_arm_hook_claws_reset();
}
WAIT_MODULES_IDLE(11, 21);
if (bottomoperation) {
bottomoperation();
}
m4_zmove_to(0);
WAIT_MODULES_IDLE(4);
}
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, bool& hasbullet) {
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);
xymove_to(cfg.xy_platform_remove_line_pos_x, cfg.xy_platform_cook_bullet_pos_y - 3623);
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_remove_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);
hasbullet = true;
} else {
hasbullet = false;
stop_probe_bullet_pos();
substep_zaxis_do_bullet_action(kCookPos, kTakeBullet, kKeepLine, NULL);
xymove_to_bullet_pos(bulletindex);
substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBackBullet, kReleaseLine, NULL);
}
}
int32_t IntelligentWindingRobotCtrl::step_remove_line() {
ZLOGI(TAG, "step_remove_line");
start_probe_bullet_pos_forward();
m15_paifei_moveto_press();
WAIT_MODULES_IDLE(15);
m_dm->motor_rotate_acctime(2, -1, 1000, 1000);
int overtime = 90;
for (int i = 0; i < overtime; i++) {
osDelay(1000);
if (g_widthDetector.isRemoveLineEnd()) {
ZLOGI(TAG, "detect remove line end");
break;
}
if (i == overtime - 1) {
ZLOGI(TAG, "detect remove line end timeout");
}
}
osDelay(5000);
m_dm->module_stop(2);
// m15_paifei_moveto_reset();
m15_paifei_moveto_reset();
stop_probe_bullet_pos();
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);
m16_xianlajin_move_to_line_entry_pos();
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);
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);
m13_yaxian_press_clip();
WAIT_MODULES_IDLE(13);
m11_arm_jiaxian_move_to_reset_pos();
WAIT_MODULES_IDLE(11);
m16_xianlajin_move_to_tight_line_pos();
m4_zmove_to(0);
osDelay(500);
WAIT_MODULES_IDLE(4);
xymove_to(0, 0);
WAIT_MODULES_IDLE(16);
// WAIT_MODULES_IDLE(13);
}
int32_t IntelligentWindingRobotCtrl::step_winding() {
start_probe_bullet_pos();
m_dm->motor_rotate_acctime(2, 1, 1000, 10000);
osDelay(1000);
m23_laxian_motor_move_to_reset_pos();
for (size_t i = 0; i < 80; i++) { // TODO:60这里是个常量可能会造成绕线提前释放。
m16_xianlajin_move_to_winding_low_pos();
WAIT_MODULES_IDLE(16);
m16_xianlajin_move_to_winding_up_pos();
WAIT_MODULES_IDLE(16);
if (g_widthDetector.isFull()) {
break;
}
}
m_dm->module_stop(2);
stop_probe_bullet_pos();
WAIT_MODULES_IDLE(16);
ZLOGI(TAG, "step_winding end....");
return 0;
}
int32_t IntelligentWindingRobotCtrl::step_winding_lineend_prepare(int bulletindex) {
/**
* @brief
*
* 0. 主轴归零
* 1. 移动到BulletHolderPos
* 2. 取弹壳
* 3. 移动到CookPos
* 4. 放弹壳
*/
m13_yaxian_move_to_reset_backward();
m_dm->motor_move_to_zero_forward(2, 2, 2, 0, 0);
wait_module_idle(2);
m_dm->motor_move_to_acctime(2, cfg.m2_zerooff, 30, 100);
wait_module_idle(2);
WAIT_MODULES_IDLE(13);
m_dm->motor_move_to_torque(23, 1900, 40, 0);
WAIT_MODULES_IDLE(23);
m16_xianlajin_move_to_cook_lineend_ready_pos();
WAIT_MODULES_IDLE(16);
xymove_to_bullet_pos(bulletindex);
substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBulletCase, kReleaseLine);
xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y);
WAIT_MODULES_IDLE(16);
substep_zaxis_do_bullet_action(kCookPos, kTakeBackBullet, kReleaseLine);
return 0;
}
int32_t IntelligentWindingRobotCtrl::step_winding_lineend() {
/**
* @brief
*
* 1. 抬升舵机到低位
* 2. 主轴正旋转
* 3. 主轴逆旋
* 4. 抬升舵机移动到高位
* 5. 主轴正旋
* 6. 抬升舵机移动到低位
* 7. 主轴逆旋
* 8. 抬升舵机移动到高位
* 9. 主轴正旋
* 10.抬升舵机移动到低位
* 11.主轴逆旋
*/
#if 0
eq20_move_by 1 840 30 100
mini_servo_move_to 6 1550 600 500
eq20_move_by 1 -840 30 100
mini_servo_move_to 6 1750 600 500
eq20_move_to 1 280 30 100
mini_servo_move_to 6 1600 600 500
eq20_move_to 1 -560 30 100
mini_servo_move_to 6 1550 600 500
eq20_move_to 1 1110 30 100
mini_servo_move_to 6 1850 600 500
#endif
// m23_laxian_motor_move_to_tight_line_pos();
#if 0
int32_t velocity = 30;
int32_t acctime = 10;
m_dm->motor_move_to_acctime(2, cfg.m2_zerooff, velocity, acctime);
WAIT_MODULES_IDLE(2);
WAIT_MODULES_IDLE(16); //
m_dm->motor_move_to_acctime(2, cfg.m2_zerooff + 1000, velocity, acctime); // 沟位置
WAIT_MODULES_IDLE(2); //
m16_xianlajin_move_to_cook_lineend_low_pos(); //
WAIT_MODULES_IDLE(16); //
m_dm->motor_move_to_acctime(2, cfg.m2_zerooff - 1600, velocity, acctime); // 逆向上线位置
WAIT_MODULES_IDLE(2); //
m16_xianlajin_move_to_cook_lineend_high_pos(); //
WAIT_MODULES_IDLE(16); //
m_dm->motor_move_to_acctime(2, cfg.m2_zerooff, velocity, acctime); // 零点
WAIT_MODULES_IDLE(2); //
m16_xianlajin_move_to_cook_lineend_low_pos(); //
WAIT_MODULES_IDLE(16); //
m_dm->motor_move_to_acctime(2, cfg.m2_zerooff - 1600, velocity, acctime); //
WAIT_MODULES_IDLE(2); //
m16_xianlajin_move_to_cook_lineend_high_pos(); //
WAIT_MODULES_IDLE(16); //
m_dm->motor_move_to_acctime(2, cfg.m2_zerooff, velocity, acctime); //
WAIT_MODULES_IDLE(2); //
m16_xianlajin_move_to_cook_lineend_low_pos(); //
#endif
#if 0
motor_move_to 16 1850 300 0
motor_move_to_acctime 2 1400 5 50
motor_move_to 16 1800 300 0
motor_move_to_acctime 2 2200 5 50
motor_move_to 16 1800 300 0
motor_move_to_acctime 2 -200 1 50
motor_move_to 16 1741 300 0
motor_move_to_acctime 2 1000 5 50
motor_move_to 16 1850 300 0
motor_move_to_acctime 2 -200 5 50
motor_move_to 16 1741 300 0
motor_move_to_acctime 2 1100 5 50
motor_move_to 16 1850 300 0
#endif
// m_dm->motor_move_to(16,1980,300,0);
m_dm->motor_move_to(16, 1850, 300, 0);
wait_module_idle(16);
m_dm->motor_move_to_acctime(2, 1400, 5, 50);
wait_module_idle(2);
m_dm->motor_move_to(16, 1850, 300, 0);
wait_module_idle(16);
m_dm->motor_move_to_acctime(2, 2200, 5, 50);
wait_module_idle(2);
m_dm->motor_move_to(16, 1800, 300, 0);
wait_module_idle(16);
m_dm->motor_move_to_acctime(2, -200, 3, 50);
osDelay(4000);
wait_module_idle(2);
m_dm->motor_move_to(16, 1741, 300, 0);
wait_module_idle(16);
m_dm->motor_move_to_acctime(2, 1000, 5, 50);
wait_module_idle(2);
m_dm->motor_move_to(16, 1850, 300, 0);
wait_module_idle(16);
m_dm->motor_move_to_acctime(2, -200, 5, 50);
wait_module_idle(2);
m_dm->motor_move_to(16, 1741, 300, 0);
wait_module_idle(16);
m_dm->motor_move_to_acctime(2, 1100, 5, 50);
wait_module_idle(2);
m_dm->motor_move_to(16, 1850, 300, 0);
wait_module_idle(16);
}
int32_t IntelligentWindingRobotCtrl::step_winding_take_bullet_from_cooking_to_origin_pos(int bulletindex) {
m16_xianlajin_move_to_reset();
m4_zmove_to(0);
WAIT_MODULES_IDLE(4);
xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y);
WAIT_MODULES_IDLE(3);
m12_jiaxian_move_to_clamp_pos();
WAIT_MODULES_IDLE(12);
m22_scissors_cut();
WAIT_MODULES_IDLE(22);
m22_scissors_move_reset_pos();
substep_zaxis_do_bullet_action(kCookPos, kTakeBullet, kReleaseLine, [this]() {});
WAIT_MODULES_IDLE(22);
xymove_to_bullet_pos(bulletindex);
WAIT_MODULES_IDLE(3);
substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBackBullet, kReleaseLine);
}
/**
* @brief
*/
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;
}
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::setcfg(const char* cfgname, int32_t cfgvalue) { return 0; }
int32_t IntelligentWindingRobotCtrl::dumpcfg() { return 0; }
int32_t IntelligentWindingRobotCtrl::start_winding() { //
m_work_thread.start([this]() {
try {
ZLOGI(TAG, "start_winding");
m_iswinding = true;
m_nowwinding_index = 0;
device_reset();
bool hasbullet = false;
for (size_t i = 0; i < 5 * 12; i++) {
if (m_work_thread.getExitFlag()) {
break;
}
if (!(i / 5 == 0 || i / 5 == 6 || i / 5 == 11)) {
continue;
}
m_nowwinding_index = i + 1;
step_take_bullet(i);
step_prepare_remove_line(i, hasbullet);
if (!hasbullet) {
continue;
}
step_remove_line();
if (m_work_thread.getExitFlag()) break;
step_winding_prepare();
if (m_work_thread.getExitFlag()) break;
step_winding();
if (m_work_thread.getExitFlag()) break;
step_winding_lineend_prepare(i);
if (m_work_thread.getExitFlag()) break;
step_winding_lineend();
if (m_work_thread.getExitFlag()) break;
step_winding_take_bullet_from_cooking_to_origin_pos(i);
if (m_work_thread.getExitFlag()) break;
}
m4_zmove_to(0);
wait_module_idle(4);
xymove_to(0, 0);
wait_module_idle(3);
stop_all_module();
} catch (int32_t ecode) {
ZLOGE(TAG, "work thread catch exception %d", ecode);
disable_all_module(); // todo : 完成一个无异常版本
} catch (...) {
ZLOGE(TAG, "work thread catch unkown exception");
disable_all_module(); // todo : 完成一个无异常版本
}
m_iswinding = false;
});
return 0;
}
int32_t IntelligentWindingRobotCtrl::stop_winding() {
m_work_thread.stop();
m_iswinding = false;
return 0;
}