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
1e5ea0166a
commit
b3fb09f915
5 changed files with 188 additions and 18 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. 7
      README.md
  2. 2
      sdk
  3. 153
      usrc/intelligent_winding_robot_ctrl.cpp
  4. 33
      usrc/intelligent_winding_robot_ctrl.hpp
  5. 11
      usrc/main.cpp

7
README.md
View File

@ -146,4 +146,11 @@ bullet_holder
```
step_take_bullet 1
step_prepare_remove_line 1
step_remove_line 1
step_winding_prepare
step_winding
```

2
sdk

@ -1 +1 @@
Subproject commit a3908e8598d72d2a334ddef95a5578b7d8ba8770
Subproject commit e30c3da5f2ea4b38e3f95580a1c12867521f2272

153
usrc/intelligent_winding_robot_ctrl.cpp
View File

@ -63,11 +63,13 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() {
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 = 1885;
cfg.m16_xianlajin_winding_up_pos = 1835;
cfg.m16_xianlajin_line_entry_pos = 1800;
cfg.m16_xianlajin_reset_pos = 2047;
cfg.m16_xianlajin_tight_line_pos = 1966;
cfg.m16_xianlajin_winding_low_pos = 1885;
cfg.m16_xianlajin_winding_high_pos = 1835;
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 = 2294;
@ -85,10 +87,11 @@ int32_t IntelligentWindingRobotCtrl::initialize_device() {
cfg.xy_platform_enter_line_pos_y = 6851;
//
cfg.z_axis_cook_bullet_pos = 3377;
cfg.z_axis_take_clip_pos = 6924;
cfg.z_axis_take_clip_pos = 6850;
cfg.z_axis_take_line_high = 3400;
cfg.z_axis_transfer_line_high = 2785;
cfg.m2_zerooff = 1110;
return 0;
}
@ -103,6 +106,8 @@ void IntelligentWindingRobotCtrl::regcb() {
m_cmdparse->regCMD("step_prepare_remove_line", "()", 1, [this](PARAM) { return step_prepare_remove_line(atoi(paraV[0])); });
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("disable_all_motor", "()", 0, [this](PARAM) { return disable_all_motor(); });
@ -268,18 +273,21 @@ int32_t IntelligentWindingRobotCtrl::m14_raoxiantance_move_to_reset() { return m
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_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_up_pos, 20, 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::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::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_rotate_with_torque(23, -1, 40); }
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);
@ -295,6 +303,41 @@ int32_t IntelligentWindingRobotCtrl::m4_zmove_to(int32_t pos) {
}
}
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) {
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);
m4_zmove_to(0);
WAIT_MODULES_IDLE(4);
}
int32_t IntelligentWindingRobotCtrl::step_take_bullet(int32_t bulletindex) {
m4_zreset();
WAIT_MODULES_IDLE(4);
@ -385,9 +428,11 @@ int32_t IntelligentWindingRobotCtrl::step_prepare_remove_line(int32_t bulletinde
*/
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_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_take_line_high);
WAIT_MODULES_IDLE(4);
// 排线舵机
@ -488,12 +533,96 @@ int32_t IntelligentWindingRobotCtrl::step_winding() {
}
m_dm->module_stop(2);
ZLOGI(TAG, "step_winding end....");
return 0;
}
int32_t IntelligentWindingRobotCtrl::step_winding_lineend_prepare(int bulletindex) {
/**
* @brief
*
* 0. 主轴归零
* 1. 移动到BulletHolderPos
* 2. 取弹壳
* 3. 移动到CookPos
* 4. 放弹壳
*/
m_dm->motor_move_to_zero_forward(2, 2, 2, 0, 0);
xymove_to_bullet_pos(bulletindex);
substep_zaxis_do_bullet_action(kBulletBulletHolderPos, kTakeBackBulletCase, kReleaseLine);
xymove_to(cfg.xy_platform_cook_bullet_pos_x, cfg.xy_platform_cook_bullet_pos_y);
wait_module_idle(2);
ZLOGI(TAG, "step_winding end....");
m_dm->motor_move_to_acctime(2, cfg.m2_zerooff, 30, 100);
substep_zaxis_do_bullet_action(kCookPos, kTakeBackBullet, kReleaseLine);
/**
* @TODO:线拉紧舵机，拉紧线?
*/
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();
int32_t velocity = 30;
int32_t acctime = 10;
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 + 1000, velocity, acctime); // 沟位置
WAIT_MODULES_IDLE(2); //
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(); //
}
int32_t IntelligentWindingRobotCtrl::main_shaft_run() {
ZLOGI(TAG, "main_shaft_run");
DO(m_dm->motor_rotate_acctime(2, 1, 1000, 10000));

33
usrc/intelligent_winding_robot_ctrl.hpp
View File

@ -16,6 +16,8 @@ using namespace std;
class IntelligentWindingRobotCtrl {
public:
typedef struct {
int32_t m2_zerooff;
/**
* @brief 机械臂夹线舵机
*/
@ -64,8 +66,10 @@ class IntelligentWindingRobotCtrl {
int32_t m16_xianlajin_reset_pos;
int32_t m16_xianlajin_tight_line_pos;
int32_t m16_xianlajin_winding_low_pos;
int32_t m16_xianlajin_winding_up_pos;
int32_t m16_xianlajin_winding_high_pos;
int32_t m16_xianlajin_line_entry_pos;
int32_t m16_xianlajin_cook_line_end_high_pos;
int32_t m16_xianlajin_cook_line_end_low_pos;
/**
* @brief 钩爪
@ -125,6 +129,23 @@ class IntelligentWindingRobotCtrl {
ICmdParser* m_cmdparse;
config_t cfg;
typedef enum {
kBulletBulletHolderPos = 0,
kCookPos,
} take_bullet_pos_type_t;
typedef enum {
kTakeBullet = 0,
kTakeBackBullet,
kTakeBulletCase,
kTakeBackBulletCase,
} take_bullet_acktion_t;
typedef enum {
kTakeLine = 0,
kReleaseLine,
kKeepLine,
} take_bullet_line_acktion_t;
public:
int32_t initialize(APPDM* dm, ICmdParser* cmdparse);
@ -153,6 +174,9 @@ class IntelligentWindingRobotCtrl {
int32_t m16_xianlajin_move_to_winding_low_pos();
int32_t m16_xianlajin_move_to_winding_up_pos();
int32_t m16_xianlajin_move_to_line_entry_pos();
int32_t m16_xianlajin_move_to_cook_lineend_high_pos();
int32_t m16_xianlajin_move_to_cook_lineend_low_pos();
int32_t m21_arm_hook_claws_reset();
int32_t m21_arm_hook_claws_move_to_half_pos();
int32_t m21_arm_hook_claws_move_to_full_pos();
@ -168,13 +192,20 @@ class IntelligentWindingRobotCtrl {
int32_t disable_all_module();
int32_t enable_all_module();
int32_t 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);
int32_t step_take_bullet(int32_t bulletindex);
int32_t step_take_back_bullet(int32_t bulletindex);
int32_t step_take_bullet_case(int32_t bulletindex);
int32_t step_take_back_bullet_case(int32_t bulletindex);
int32_t step_prepare_remove_line(int32_t bulletindex);
int32_t step_remove_line();
int32_t step_winding_prepare();
int32_t step_winding();
// int32_t step_load_the_bullet_case();
int32_t step_winding_lineend_prepare(int bulletindex); // 绕线头准备
int32_t step_winding_lineend(); // 绕线头
int32_t xy_get_point(int32_t clip_index, int32_t& x, int32_t& y); // 取弹夹
int32_t xy_run_to(int32_t x, int32_t y, int32_t zpos = 0, bool jiaxian_reset = true); // 取弹夹

11
usrc/main.cpp
View File

@ -66,7 +66,7 @@ static StepMotor45::cfg_t cfg1 = {
.enable_max_pos_limit = false,
.mirror = true,
.zeroPin = PB13,
.zeroPin = PG1,
.ioPollType = ZGPIO::kMode_pullup,
.zeroPinMirror = true,
@ -80,7 +80,7 @@ static StepMotor45::cfg_t cfg2 = {
.enable_max_pos_limit = false,
.mirror = true,
.zeroPin = PG1,
.zeroPin = PB13,
.ioPollType = ZGPIO::kMode_pullup,
.zeroPinMirror = true,
@ -163,7 +163,7 @@ ScirptCmderMiniServoMotorCtrlModule g_mini_servo_motor_script_cmder;
*******************************************************************************/
Eq20ServoMotor g_main_servo_motor;
StepMotor45 g_step_motor45[7];
MiniRobotCtrlModule g_mini_servo[6];
MiniRobotCtrlModule g_mini_servo[10];
/*******************************************************************************
* CANÉ豸 *
*******************************************************************************/
@ -221,10 +221,12 @@ void Main::run() {
g_mini_servo[3].initialize(14, &g_feiteservomotor_bus, 4);
g_mini_servo[4].initialize(15, &g_feiteservomotor_bus, 5);
g_mini_servo[5].initialize(16, &g_feiteservomotor_bus, 6);
g_mini_servo[6].initialize(23, &g_feiteservomotor_bus, 23);
g_step_motor45[0].initialize(21, &step_motor45_scheduler, cfg1);
g_step_motor45[1].initialize(22, &step_motor45_scheduler, cfg2);
g_step_motor45[2].initialize(23, &step_motor45_scheduler, cfg3);
// g_step_motor45[2].initialize(23, &step_motor45_scheduler, cfg3);
step_motor45_scheduler.start();
g_zmodule_device_manager.registerModule(&g_main_servo_motor);
@ -236,6 +238,7 @@ void Main::run() {
g_zmodule_device_manager.registerModule(&g_mini_servo[3]);
g_zmodule_device_manager.registerModule(&g_mini_servo[4]);
g_zmodule_device_manager.registerModule(&g_mini_servo[5]);
g_zmodule_device_manager.registerModule(&g_mini_servo[6]);
g_zmodule_device_manager.registerModule(&g_step_motor45[0]);
g_zmodule_device_manager.registerModule(&g_step_motor45[1]);
g_zmodule_device_manager.registerModule(&g_step_motor45[2]);

Write Preview
Loading…
Cancel
Save