diff --git a/.gitignore b/.gitignore index 054488a..1523582 100644 --- a/.gitignore +++ b/.gitignore @@ -8,3 +8,5 @@ **/log/ ethercat_dev/csp_log.csv + +csp_log.csv diff --git a/ethercat_dev/src/ethercat_control/src/ethercat_core.cpp b/ethercat_dev/src/ethercat_control/src/ethercat_core.cpp index 3f42af3..8ffb07f 100644 --- a/ethercat_dev/src/ethercat_control/src/ethercat_core.cpp +++ b/ethercat_dev/src/ethercat_control/src/ethercat_core.cpp @@ -287,7 +287,8 @@ void cyclic_task() // 读取从站状态字、当前位置、速度、扭矩、运行模式 for (int i = 0; i < NUM_SLAVES; ++i) { - if (i == 0) + if (i == 4) + // if (false) { uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word); int32_t pv = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value); @@ -668,10 +669,16 @@ bool start() std::cout << "Configuring slave " << i << "..." << std::endl; + int position = i+1; - if (i == 0) + if (i > 3) { - sc[i] = ecrt_master_slave_config(master, 0, i+1, ZER_VID_PID); + position = i+2; + } + + if (i == 4) + { + sc[i] = ecrt_master_slave_config(master, 0, position, ZER_VID_PID); if (!sc[i]) { std::cout << "Failed slave cfg at pos " << i << std::endl; @@ -683,17 +690,9 @@ bool start() std::cout << "Failed PDO config " << i << std::endl; // fprintf(stderr,"[S%02d] Failed PDO config\n", i); g_started.store(false); return false; } - } else{ - int position = i+1; - - if (i > 5) - { - position = i+3; - } - sc[i] = ecrt_master_slave_config(master, 0, position, MT_VID_PID); if (!sc[i]) { @@ -714,7 +713,14 @@ bool start() // 注册 PDO entry 偏移 for (int i = 0; i < NUM_SLAVES; ++i) { - if (i == 0) + int position = i+1; + + if (i > 3) + { + position = i+2; + } + + if (i == 4) { // ---- CSP/CSV/CST PDO ---- const size_t N = sizeof(zer_domain1_regs)/sizeof(zer_domain1_regs[0]); @@ -723,7 +729,7 @@ bool start() for (size_t j = 0; j + 1 < N; ++j) { regs[j].alias = 0; - regs[j].position = i+1; + regs[j].position = position; } regs[0].offset = &zer_offsets[i].ctrl_word; @@ -752,13 +758,6 @@ bool start() memcpy(regs, mt_domain1_regs, sizeof(mt_domain1_regs)); for (size_t j = 0; j + 1 < N; ++j) { - int position = i+1; - - if (i > 5) - { - position = i+3; - } - regs[j].alias = 0; regs[j].position = position; }