├── README.md ├── el2008_dc_rt.c ├── ipipe_64.c ├── ipipe_64.h ├── is620n_dc.c ├── is620n_dc_csp.c ├── is620n_dc_cst.c ├── is620n_dc_csv.c ├── is620n_dc_rt_csp.c ├── is620n_dc_rt_csv.c ├── libipipe_64.so ├── main_dc_4slaves.c ├── main_dc_el2008.c └── rtai_rtdm_dc ├── 4slaves.c ├── 4slaves_2.c ├── 5slaves.c ├── is620n_dc_csp.c ├── is620n_dc_rt_csp.c ├── is620n_dc_rt_csv.c ├── main.c └── xmc4800_relax.cpp /README.md: -------------------------------------------------------------------------------- 1 | # igh-Master-samples 2 | -------------------------------------------------------------------------------------------------------------- 3 | (这个目录下的.c或.cpp代码都是运行在打了preempt rt补丁的内核下的,rtai_rtdm_dc目录下的.c或.cpp代码都是运行在打了rtai5.1补丁的内核下的) 4 | 5 | 1,is620n_dc.c - 运行后,汇川is620n伺服电机就以csv模式转动了 6 | 7 | 2,main_dc_el2008.c - EK1100+EL1008+EL2008,程序运行后可看到EL2008的指示灯在闪 8 | 9 | 3,main_dc_4slaves.c - EK1100+EL1008+El2008+IS620N伺服,程序运行后可看到EL2008的指示灯在闪,IS620N伺服以CSV模式运行 10 | 11 | 4,is620n_dc_csv.c - 运行后,汇川is620n伺服电机就以csv模式转动了 12 | 13 | 5,is620n_dc_csp.c - 运行后,汇川is620n伺服电机就以csp模式转动了 14 | 15 | 6,is620n_dc_cst.c - 运行后,汇川is620n伺服电机就以cst模式转动了 16 | -------------------------------------------------------------------------------- /el2008_dc_rt.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * $Id$ 4 | * 5 | * Copyright (C) 2011 IgH Andreas Stewering-Bone 6 | * 2012 Florian Pose 7 | * 8 | * This file is part of the IgH EtherCAT master 9 | * 10 | * The IgH EtherCAT Master is free software; you can redistribute it and/or 11 | * modify it under the terms of the GNU General Public License version 2, as 12 | * published by the Free Software Foundation. 13 | * 14 | * The IgH EtherCAT master is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 17 | * Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License along 20 | * with the IgH EtherCAT master. If not, see . 21 | * 22 | * --- 23 | * 24 | * The license mentioned above concerns the source code only. Using the 25 | * EtherCAT technology and brand is only permitted in compliance with the 26 | * industrial property and similar rights of Beckhoff Automation GmbH. 27 | * 这个例子实时性更好,在preempt rt下,使用ec_generic.ko都跑得很好 28 | * gcc -o el2008_dc_rt el2008_dc_rt.c -I/opt/etherlab/include -L. -L/opt/etherlab/lib -Wl,--rpath=./ -Wl,--rpath=/opt/etherlab/lib -lethercat -lrt -lipipe_64 29 | *****************************************************************************/ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | //#include 37 | //#include 38 | 39 | #include 40 | 41 | #include "ecrt.h" 42 | 43 | #include 44 | #include 45 | #include "ipipe_64.h" 46 | 47 | //#define rt_printf(X, Y) 48 | 49 | #define CLOCK_TO_USE CLOCK_MONOTONIC //CLOCK_REALTIME 50 | #define NSEC_PER_SEC 1000000000 51 | 52 | //RT_TASK *task; 53 | 54 | static unsigned int cycle_ns = 1000000; /* 1 ms */ 55 | 56 | static int run = 1; 57 | 58 | /****************************************************************************/ 59 | 60 | // EtherCAT 61 | static ec_master_t *master = NULL; 62 | static ec_master_state_t master_state = {}; 63 | 64 | static ec_domain_t *domain1 = NULL; 65 | static ec_domain_state_t domain1_state = {}; 66 | 67 | static uint8_t *domain1_pd = NULL; 68 | 69 | static ec_slave_config_t *sc_dig_out_01 = NULL; 70 | 71 | /****************************************************************************/ 72 | 73 | // EtherCAT distributed clock variables 74 | 75 | #define DC_FILTER_CNT 1024 76 | #define SYNC_MASTER_TO_REF 1 77 | 78 | static uint64_t dc_start_time_ns = 0LL; 79 | static uint64_t dc_time_ns = 0; 80 | #if SYNC_MASTER_TO_REF 81 | static uint8_t dc_started = 0; 82 | static int32_t dc_diff_ns = 0; 83 | static int32_t prev_dc_diff_ns = 0; 84 | static int64_t dc_diff_total_ns = 0LL; 85 | static int64_t dc_delta_total_ns = 0LL; 86 | static int dc_filter_idx = 0; 87 | static int64_t dc_adjust_ns; 88 | #endif 89 | static int64_t system_time_base = 0LL; 90 | static uint64_t wakeup_time = 0LL; 91 | static uint64_t overruns = 0LL; 92 | 93 | /****************************************************************************/ 94 | 95 | // process data 96 | 97 | #define BusCoupler01_Pos 0, 0 98 | #define DigOutSlave01_Pos 0, 2 99 | 100 | #define Beckhoff_EK1100 0x00000002, 0x044c2c52 101 | //#define Beckhoff_EL2004 0x00000002, 0x07d43052 102 | #define Beckhoff_EL2008 0x00000002, 0x07d83052 103 | 104 | // offsets for PDO entries 105 | static unsigned int off_dig_out0 = 0; 106 | 107 | // process data 108 | 109 | const static ec_pdo_entry_reg_t domain1_regs[] = { 110 | //{DigOutSlave01_Pos, Beckhoff_EL2004, 0x7000, 0x01, &off_dig_out0, NULL}, 111 | {DigOutSlave01_Pos, Beckhoff_EL2008, 0x7000, 0x01, &off_dig_out0, NULL}, 112 | {} 113 | }; 114 | 115 | /****************************************************************************/ 116 | 117 | /* Slave 1, "EL2004" 118 | * Vendor ID: 0x00000002 119 | * Product code: 0x07d43052 120 | * Revision number: 0x00100000 121 | */ 122 | 123 | /* Slave 1, "EL2008" 124 | * Vendor ID: 0x00000002 125 | * Product code: 0x07d83052 126 | * Revision number: 0x00100000 127 | */ 128 | 129 | ec_pdo_entry_info_t slave_1_pdo_entries[] = { 130 | {0x7000, 0x01, 1}, /* Output */ 131 | {0x7010, 0x01, 1}, /* Output */ 132 | {0x7020, 0x01, 1}, /* Output */ 133 | {0x7030, 0x01, 1}, /* Output */ 134 | }; 135 | 136 | ec_pdo_info_t slave_1_pdos[] = { 137 | {0x1600, 1, slave_1_pdo_entries + 0}, /* Channel 1 */ 138 | {0x1601, 1, slave_1_pdo_entries + 1}, /* Channel 2 */ 139 | {0x1602, 1, slave_1_pdo_entries + 2}, /* Channel 3 */ 140 | {0x1603, 1, slave_1_pdo_entries + 3}, /* Channel 4 */ 141 | }; 142 | 143 | ec_sync_info_t slave_1_syncs[] = { 144 | {0, EC_DIR_OUTPUT, 4, slave_1_pdos + 0, EC_WD_ENABLE}, 145 | {0xff} 146 | }; 147 | 148 | /***************************************************************************** 149 | * Realtime task 150 | ****************************************************************************/ 151 | 152 | /** Get the time in ns for the current cpu, adjusted by system_time_base. 153 | * 154 | * \attention Rather than calling rt_get_time_ns() directly, all application 155 | * time calls should use this method instead. 156 | * 157 | * \ret The time in ns. 158 | */ 159 | uint64_t system_time_ns(void) 160 | { 161 | RTIME time = rt_get_time_ns(); 162 | 163 | if (system_time_base > time) { 164 | /*rt_printk("%s() error: system_time_base greater than" 165 | " system time (system_time_base: %lld, time: %llu\n", 166 | __func__, system_time_base, time);*/ 167 | return time; 168 | } 169 | else { 170 | return time - system_time_base; 171 | } 172 | } 173 | 174 | /****************************************************************************/ 175 | 176 | /** Convert system time to RTAI time in counts (via the system_time_base). 177 | */ 178 | RTIME system2count( 179 | uint64_t time 180 | ) 181 | { 182 | RTIME ret; 183 | 184 | if ((system_time_base < 0) && 185 | ((uint64_t) (-system_time_base) > time)) { 186 | /*rt_printk("%s() error: system_time_base less than" 187 | " system time (system_time_base: %lld, time: %llu\n", 188 | __func__, system_time_base, time);*/ 189 | ret = time; 190 | } 191 | else { 192 | ret = time + system_time_base; 193 | } 194 | 195 | return nano2count(ret); 196 | } 197 | 198 | /*****************************************************************************/ 199 | 200 | /** Synchronise the distributed clocks 201 | */ 202 | void sync_distributed_clocks(void) 203 | { 204 | #if SYNC_MASTER_TO_REF 205 | uint32_t ref_time = 0; 206 | uint64_t prev_app_time = dc_time_ns; 207 | #endif 208 | 209 | dc_time_ns = system_time_ns(); 210 | 211 | #if SYNC_MASTER_TO_REF 212 | // get reference clock time to synchronize master cycle 213 | ecrt_master_reference_clock_time(master, &ref_time); 214 | dc_diff_ns = (uint32_t) prev_app_time - ref_time; 215 | #else 216 | // sync reference clock to master 217 | ecrt_master_sync_reference_clock_to(master, dc_time_ns); 218 | #endif 219 | 220 | // call to sync slaves to ref slave 221 | ecrt_master_sync_slave_clocks(master); 222 | } 223 | 224 | /*****************************************************************************/ 225 | 226 | /** Return the sign of a number 227 | * 228 | * ie -1 for -ve value, 0 for 0, +1 for +ve value 229 | * 230 | * \retval the sign of the value 231 | */ 232 | #define sign(val) \ 233 | ({ typeof (val) _val = (val); \ 234 | ((_val > 0) - (_val < 0)); }) 235 | 236 | /*****************************************************************************/ 237 | 238 | /** Update the master time based on ref slaves time diff 239 | * 240 | * called after the ethercat frame is sent to avoid time jitter in 241 | * sync_distributed_clocks() 242 | */ 243 | void update_master_clock(void) 244 | { 245 | #if SYNC_MASTER_TO_REF 246 | // calc drift (via un-normalised time diff) 247 | int32_t delta = dc_diff_ns - prev_dc_diff_ns; 248 | prev_dc_diff_ns = dc_diff_ns; 249 | 250 | // normalise the time diff 251 | dc_diff_ns = 252 | ((dc_diff_ns + (cycle_ns / 2)) % cycle_ns) - (cycle_ns / 2); 253 | 254 | // only update if primary master 255 | if (dc_started) { 256 | 257 | // add to totals 258 | dc_diff_total_ns += dc_diff_ns; 259 | dc_delta_total_ns += delta; 260 | dc_filter_idx++; 261 | 262 | if (dc_filter_idx >= DC_FILTER_CNT) { 263 | // add rounded delta average 264 | dc_adjust_ns += 265 | ((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT); 266 | 267 | // and add adjustment for general diff (to pull in drift) 268 | dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT); 269 | 270 | // limit crazy numbers (0.1% of std cycle time) 271 | if (dc_adjust_ns < -1000) { 272 | dc_adjust_ns = -1000; 273 | } 274 | if (dc_adjust_ns > 1000) { 275 | dc_adjust_ns = 1000; 276 | } 277 | 278 | // reset 279 | dc_diff_total_ns = 0LL; 280 | dc_delta_total_ns = 0LL; 281 | dc_filter_idx = 0; 282 | } 283 | 284 | // add cycles adjustment to time base (including a spot adjustment) 285 | system_time_base += dc_adjust_ns + sign(dc_diff_ns); 286 | } 287 | else { 288 | dc_started = (dc_diff_ns != 0); 289 | 290 | if (dc_started) { 291 | // output first diff 292 | //rt_printk("First master diff: %d.\n", dc_diff_ns); 293 | 294 | // record the time of this initial cycle 295 | dc_start_time_ns = dc_time_ns; 296 | } 297 | } 298 | #endif 299 | } 300 | 301 | /****************************************************************************/ 302 | 303 | void rt_check_domain_state(void) 304 | { 305 | ec_domain_state_t ds = {}; 306 | 307 | ecrt_domain_state(domain1, &ds); 308 | 309 | if (ds.working_counter != domain1_state.working_counter) { 310 | //rt_printf("Domain1: WC %u.\n", ds.working_counter); 311 | } 312 | 313 | if (ds.wc_state != domain1_state.wc_state) { 314 | //rt_printf("Domain1: State %u.\n", ds.wc_state); 315 | } 316 | 317 | domain1_state = ds; 318 | } 319 | 320 | /****************************************************************************/ 321 | 322 | void rt_check_master_state(void) 323 | { 324 | ec_master_state_t ms; 325 | 326 | ecrt_master_state(master, &ms); 327 | 328 | if (ms.slaves_responding != master_state.slaves_responding) { 329 | //rt_printf("%u slave(s).\n", ms.slaves_responding); 330 | } 331 | 332 | if (ms.al_states != master_state.al_states) { 333 | //rt_printf("AL states: 0x%02X.\n", ms.al_states); 334 | } 335 | 336 | if (ms.link_up != master_state.link_up) { 337 | //rt_printf("Link is %s.\n", ms.link_up ? "up" : "down"); 338 | } 339 | 340 | master_state = ms; 341 | } 342 | 343 | /****************************************************************************/ 344 | 345 | /** Wait for the next period 346 | */ 347 | 348 | #if 0 349 | void wait_period(void) 350 | { 351 | while (1) 352 | { 353 | RTIME wakeup_count = system2count(wakeup_time); 354 | RTIME current_count = rt_get_time(); 355 | 356 | if ((wakeup_count < current_count) 357 | || (wakeup_count > current_count + (50 * cycle_ns))) { 358 | //rt_printk("%s(): unexpected wake time!\n", __func__); 359 | } 360 | 361 | /*switch (rt_sleep_until(wakeup_count)) { 362 | case RTE_UNBLKD: 363 | rt_printk("rt_sleep_until(): RTE_UNBLKD\n"); 364 | continue; 365 | 366 | case RTE_TMROVRN: 367 | rt_printk("rt_sleep_until(): RTE_TMROVRN\n"); 368 | overruns++; 369 | 370 | if (overruns % 100 == 0) { 371 | // in case wake time is broken ensure other processes get 372 | // some time slice (and error messages can get displayed) 373 | rt_sleep(cycle_ns / 100); 374 | } 375 | break; 376 | 377 | default: 378 | break; 379 | }*/ 380 | 381 | // done if we got to here 382 | break; 383 | } 384 | 385 | // set master time in nano-seconds 386 | ecrt_master_application_time(master, wakeup_time); 387 | 388 | // calc next wake time (in sys time) 389 | wakeup_time += cycle_ns; 390 | } 391 | #endif 392 | 393 | 394 | void wait_period(void) 395 | { 396 | while (1) 397 | { 398 | RTIME wakeup_count = system2count(wakeup_time); 399 | RTIME current_count = rt_get_time(); 400 | 401 | if ((wakeup_count < current_count) 402 | || (wakeup_count > current_count + (50 * cycle_ns))) { 403 | //rt_printk("%s(): unexpected wake time!\n", __func__); 404 | } 405 | 406 | struct timespec wakeupTime; 407 | /*wakeupTime.tv_sec= wakeup_time / NSEC_PER_SEC; 408 | wakeupTime.tv_nsec= wakeup_time % NSEC_PER_SEC; 409 | 410 | int s =clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);*/ 411 | 412 | wakeupTime.tv_sec= cycle_ns / NSEC_PER_SEC; 413 | wakeupTime.tv_nsec= cycle_ns % NSEC_PER_SEC; 414 | 415 | int s =clock_nanosleep(CLOCK_TO_USE, 0, &wakeupTime, NULL); 416 | 417 | if (s != 0) { //避免过度睡眠 418 | if (s == EINTR) 419 | printf("Interrupted by signal handler\n"); 420 | else 421 | printf("clock_nanosleep:errno=%d\n",s); 422 | } 423 | 424 | // done if we got to here 425 | break; 426 | } 427 | 428 | // set master time in nano-seconds 429 | ecrt_master_application_time(master, wakeup_time); 430 | 431 | // calc next wake time (in sys time) 432 | wakeup_time += cycle_ns; 433 | } 434 | /****************************************************************************/ 435 | 436 | void my_cyclic(void) 437 | { 438 | int cycle_counter = 0; 439 | unsigned int blink = 0; 440 | 441 | // oneshot mode to allow adjustable wake time 442 | //rt_set_oneshot_mode(); 443 | 444 | // set first wake time in a few cycles 445 | wakeup_time = system_time_ns() + 10 * cycle_ns; 446 | 447 | // start the timer 448 | //start_rt_timer(nano2count(cycle_ns)); 449 | 450 | //rt_make_hard_real_time(); 451 | 452 | while (run) { 453 | // wait for next period (using adjustable system time) 454 | wait_period(); 455 | 456 | cycle_counter++; 457 | 458 | if (!run) { 459 | break; 460 | } 461 | 462 | // receive EtherCAT 463 | ecrt_master_receive(master); 464 | ecrt_domain_process(domain1); 465 | 466 | rt_check_domain_state(); 467 | 468 | if (!(cycle_counter % 1000)) { 469 | rt_check_master_state(); 470 | } 471 | 472 | if (!(cycle_counter % (200))) { 473 | blink = !blink; 474 | } 475 | 476 | EC_WRITE_U8(domain1_pd + off_dig_out0, blink ? 0x00 : 0x0F); 477 | 478 | // queue process data 479 | ecrt_domain_queue(domain1); 480 | 481 | // sync distributed clock just before master_send to set 482 | // most accurate master clock time 483 | sync_distributed_clocks(); 484 | 485 | // send EtherCAT data 486 | ecrt_master_send(master); 487 | 488 | // update the master clock 489 | // Note: called after ecrt_master_send() to reduce time 490 | // jitter in the sync_distributed_clocks() call 491 | update_master_clock(); 492 | } 493 | 494 | //rt_make_soft_real_time(); 495 | //stop_rt_timer(); 496 | } 497 | 498 | /**************************************************************************** 499 | * Signal handler 500 | ***************************************************************************/ 501 | 502 | void signal_handler(int sig) 503 | { 504 | run = 0; 505 | } 506 | 507 | /**************************************************************************** 508 | * Main function 509 | ***************************************************************************/ 510 | 511 | int main(int argc, char *argv[]) 512 | { 513 | ec_slave_config_t *sc_ek1100; 514 | int ret; 515 | 516 | signal(SIGTERM, signal_handler); 517 | signal(SIGINT, signal_handler); 518 | 519 | mlockall(MCL_CURRENT | MCL_FUTURE); 520 | 521 | printf("Requesting master...\n"); 522 | master = ecrt_request_master(0); 523 | if (!master) { 524 | return -1; 525 | } 526 | 527 | domain1 = ecrt_master_create_domain(master); 528 | if (!domain1) { 529 | return -1; 530 | } 531 | 532 | printf("Creating slave configurations...\n"); 533 | 534 | // Create configuration for bus coupler 535 | sc_ek1100 = 536 | ecrt_master_slave_config(master, BusCoupler01_Pos, Beckhoff_EK1100); 537 | if (!sc_ek1100) { 538 | return -1; 539 | } 540 | 541 | /*sc_dig_out_01 = 542 | ecrt_master_slave_config(master, DigOutSlave01_Pos, Beckhoff_EL2004); 543 | if (!sc_dig_out_01) { 544 | fprintf(stderr, "Failed to get slave configuration.\n"); 545 | return -1; 546 | }*/ 547 | 548 | sc_dig_out_01 = 549 | ecrt_master_slave_config(master, DigOutSlave01_Pos, Beckhoff_EL2008); 550 | if (!sc_dig_out_01) { 551 | fprintf(stderr, "Failed to get slave configuration.\n"); 552 | return -1; 553 | } 554 | //设置EL2008　eeprom 555 | if (ecrt_slave_config_pdos(sc_dig_out_01, EC_END, slave_1_syncs)) { 556 | fprintf(stderr, "Failed to configure PDOs.\n"); 557 | return -1; 558 | } 559 | //设置PDO映射 560 | if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { 561 | fprintf(stderr, "PDO entry registration failed!\n"); 562 | return -1; 563 | } 564 | 565 | /* Set the initial master time and select a slave to use as the DC 566 | * reference clock, otherwise pass NULL to auto select the first capable 567 | * slave. Note: This can be used whether the master or the ref slave will 568 | * be used as the systems master DC clock. 569 | */ 570 | dc_start_time_ns = system_time_ns(); 571 | dc_time_ns = dc_start_time_ns; 572 | 573 | ret = ecrt_master_select_reference_clock(master, sc_ek1100); 574 | if (ret < 0) { 575 | fprintf(stderr, "Failed to select reference clock: %s\n", 576 | strerror(-ret)); 577 | return ret; 578 | } 579 | 580 | printf("Activating master...\n"); 581 | if (ecrt_master_activate(master)) { 582 | return -1; 583 | } 584 | 585 | if (!(domain1_pd = ecrt_domain_data(domain1))) { 586 | fprintf(stderr, "Failed to get domain data pointer.\n"); 587 | return -1; 588 | } 589 | 590 | /* Create cyclic RT-thread */ 591 | struct sched_param param; 592 | param.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1; 593 | if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { 594 | puts("ERROR IN SETTING THE SCHEDULER"); 595 | perror("errno"); 596 | return -1; 597 | } 598 | 599 | //task = rt_task_init(nam2num("ec_rtai_rtdm_example"), 600 | // 0 /* priority */, 0 /* stack size */, 0 /* msg size */); 601 | 602 | my_cyclic(); 603 | 604 | //rt_task_delete(task); 605 | 606 | printf("End of Program\n"); 607 | ecrt_release_master(master); 608 | 609 | return 0; 610 | } 611 | 612 | /****************************************************************************/ 613 | -------------------------------------------------------------------------------- /ipipe_64.c: -------------------------------------------------------------------------------- 1 | /*1,gcc -c -fPIC ipipe_64.c 2 | *2,gcc -shared -fPIC -o libipipe_64.so ipipe_64.o 3 | */ 4 | #include "ipipe_64.h" 5 | 6 | double cpu_mhz; 7 | unsigned long clock_freq; 8 | 9 | /*x86_64*/ 10 | #define ipipe_read_tsc(t) do { \ 11 | unsigned int __a,__d; \ 12 | asm volatile("rdtsc" : "=a" (__a), "=d" (__d)); \ 13 | (t) = ((unsigned long)__a) | (((unsigned long)__d)<<32); \ 14 | } while(0) 15 | 16 | #define rtai_rdtsc() ({ unsigned long long t; ipipe_read_tsc(t); t; }) 17 | 18 | static inline long rtai_imuldiv (long i, long mult, long div) { 19 | 20 | /* Returns (int)i = (int)i*(int)(mult)/(int)div. */ 21 | 22 | int dummy; 23 | 24 | __asm__ __volatile__ ( \ 25 | "mulq %%rdx\t\n" \ 26 | "divq %%rcx\t\n" \ 27 | : "=a" (i), "=d" (dummy) 28 | : "a" (i), "d" (mult), "c" (div)); 29 | 30 | return i; 31 | } 32 | 33 | static inline long long rtai_llimd(long long ll, long mult, long div) { 34 | return rtai_imuldiv(ll, mult, div); 35 | } 36 | 37 | static double get_cpu_freq(void){ 38 | FILE *fp; 39 | char s[81]; 40 | memset(s,0,81); 41 | fp=popen("cat /proc/cpuinfo|grep cpu\\ MHz|sed -e 's/.*:[^0-9]//'","r"); 42 | if(fp<0){ 43 | printf("read CPU freq failed.\n"); 44 | return 0; 45 | } 46 | fgets(s,80,fp); 47 | fclose(fp); 48 | 49 | return (double)atof(s); 50 | } 51 | 52 | RTIME rt_get_time(void) 53 | { 54 | return rtai_rdtsc(); 55 | } 56 | 57 | RTIME rt_get_time_ns(void) 58 | { 59 | return rtai_llimd(rtai_rdtsc(), 1000000000, clock_freq); 60 | } 61 | 62 | /* ++++++++++++++++++++++++++ TIME CONVERSIONS +++++++++++++++++++++++++++++ */ 63 | 64 | RTIME count2nano(RTIME counts) 65 | { 66 | return (counts >= 0 ? rtai_llimd(counts, 1000000000, clock_freq) : -rtai_llimd(-counts, 1000000000, clock_freq)); 67 | } 68 | 69 | RTIME nano2count(RTIME ns) 70 | { 71 | return (ns >= 0 ? rtai_llimd(ns, clock_freq, 1000000000) : -rtai_llimd(-ns, clock_freq, 1000000000)); 72 | } 73 | 74 | /*initialize the library*/ 75 | void __attribute__ ((constructor)) my_init(void){ 76 | cpu_mhz = get_cpu_freq(); 77 | clock_freq = (unsigned long)((double)cpu_mhz * 1000000l); 78 | } 79 | 80 | /*finitialize the library*/ 81 | void __attribute__ ((destructor)) my_fini(void){ 82 | } 83 | 84 | /*no more*/ 85 | -------------------------------------------------------------------------------- /ipipe_64.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | typedef long long RTIME; 7 | 8 | RTIME rt_get_time(void); 9 | RTIME rt_get_time_ns(void); 10 | RTIME count2nano(RTIME counts); 11 | RTIME nano2count(RTIME ns); 12 | /*no more*/ 13 | -------------------------------------------------------------------------------- /is620n_dc.c: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * $Id: is620n_dc.c,v bc2d4bf9cbe5 2012/09/06 18:22:24 fp $ 4 | * 5 | * Copyright (C) 2007-2009 Florian Pose, Ingenieurgemeinschaft IgH 6 | * 7 | * This file is part of the IgH EtherCAT Master. 8 | * 9 | * The IgH EtherCAT Master is free software; you can redistribute it and/or 10 | * modify it under the terms of the GNU General Public License version 2, as 11 | * published by the Free Software Foundation. 12 | * 13 | * The IgH EtherCAT Master is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 16 | * Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License along 19 | * with the IgH EtherCAT Master; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | * --- 23 | * 24 | * The license mentioned above concerns the source code only. Using the 25 | * EtherCAT technology and brand is only permitted in compliance with the 26 | * industrial property and similar rights of Beckhoff Automation GmbH. 27 | * 28 | * 适用于汇川IS620N伺服(IS620N只有DC模式没有Free Run模式) 29 | * compile: gcc -o is620n_dc is620n_dc.c -Wall -I/opt/etherlab/include -lethercat -L/opt/etherlab/lib 30 | * run: $sudo ./is620n_dc 31 | ****************************************************************************/ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | /* 46 | #include 47 | #include 48 | */ 49 | /****************************************************************************/ 50 | 51 | #include "ecrt.h" 52 | 53 | /****************************************************************************/ 54 | 55 | // Application parameters 56 | #define FREQUENCY 500 57 | #define CLOCK_TO_USE CLOCK_REALTIME 58 | #define CONFIGURE_PDOS 1 59 | 60 | // Optional features 61 | #define PDO_SETTING1 1 62 | 63 | /****************************************************************************/ 64 | 65 | #define NSEC_PER_SEC (1000000000L) 66 | #define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) 67 | 68 | #define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \ 69 | (B).tv_nsec - (A).tv_nsec) 70 | 71 | #define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec) 72 | 73 | /****************************************************************************/ 74 | 75 | // EtherCAT 76 | static ec_master_t *master = NULL; 77 | static ec_master_state_t master_state = {}; 78 | 79 | static ec_domain_t *domain1 = NULL; 80 | static ec_domain_state_t domain1_state = {}; 81 | 82 | /****************************************************************************/ 83 | static ec_slave_config_t *sc = NULL; 84 | static ec_slave_config_state_t sc_state = {}; 85 | /****************************************************************************/ 86 | 87 | // process data 88 | static uint8_t *domain1_pd = NULL; 89 | 90 | //signal to turn off servo on state 91 | static unsigned int servooff; 92 | static unsigned int deactive; 93 | static signed long temp[8]={}; 94 | 95 | float value = 0; 96 | static unsigned int counter = 0; 97 | static unsigned int blink = 0; 98 | static unsigned int sync_ref_counter = 0; 99 | const struct timespec cycletime = {0, PERIOD_NS}; 100 | 101 | /*****************************************************************************/ 102 | #define IS620NSlavePos 0,0 /*EtherCAT address on the bus*/ 103 | #define INOVANCE_IS620N 0x00100000, 0x000C0108 /*IS620N Vendor ID, product code*/ 104 | 105 | /*x86_64: char-1byte,int-4bytes,long-8bytes*/ 106 | struct IS620N_offset { 107 | //RxPDO 108 | unsigned int control_word_6040_0; //0x6040:控制字,subindex:0,bitlen:16 109 | unsigned int target_position_607a_0;//0x607A:目标位置,subindex:0,bitlen:32 110 | unsigned int touch_probe_60b8_0; //0x60B8:探针,subindex:0,bitlen:16 111 | unsigned int pysical_outputs_60fe_1;//0x60FE:pysical_outputs,subindex:1,bitlen:32 112 | unsigned int target_velocity_60ff_0;//0x60FF:target_velocity,subindex:0,bitlen:32 113 | unsigned int target_torque_6071_0;//0x6071:int target_torque,subindex:0,bitlen:16 114 | unsigned int modes_operation_6060_0;//0x6060:Modes of operation,subindex:0,bitlen:8 115 | unsigned int max_profile_velocity_607f_0;//0x607F:max_profile_velocity,subindex:0,bitlen:32 116 | unsigned int positive_torque_limit_value_60e0_0;//0x60E0:positive_torque_limit_value,subindex:0,bitlen:16 117 | unsigned int negative_torque_limit_value_60e1_0;//0x60E1:negaitive_torque_limit_value,subindex:0,bitlen:16 118 | unsigned int torque_offset_60b2_0;//0x60B2:torque offset,subindex:0,bitlen:16 119 | unsigned int max_torque_6072_0;//0x6072:max torque,subindex:0,bitlen:16 120 | 121 | //TxPDo 122 | unsigned int status_word_6041_0;//0x6041:status_word,subindex:0,bitlen:16 123 | unsigned int position_actual_value_6064_0;//0x6064:position_actual_value,subindex:0,bitlen:32 124 | unsigned int touch_probe_status_60b9_0;//0x60B9,subindex:0,bitlen:16 125 | unsigned int touch_probe_pos1_pos_value_60ba_0;//0x60BA,subindex:0,bitlen:32 126 | unsigned int touch_probe_pos2_pos_value_60bc_0;//0x60BC ,subindex:0,bitlen:32 127 | unsigned int error_code_603f_0;//0x603F,subindex:0,bitlen:16 128 | unsigned int digital_inputs_60fd_0;//0x60FD,subindex:0,bitlen:32 129 | unsigned int torque_actual_value_6077_0;//0x6077,subindex:0,bitlen:16 130 | unsigned int following_error_actual_value_60f4_0;//0x60F4,subindex:0,bitlen:32 131 | unsigned int modes_of_operation_display_6061_0;//0x6061,subindex:0,bitlen:8 132 | unsigned int velocity_actual_value_606c_0;//0x606C,subindex:0,bitlen:32 133 | }; 134 | 135 | static struct IS620N_offset offset; 136 | 137 | const static ec_pdo_entry_reg_t domain1_regs[] = { 138 | //RxPDO 139 | {IS620NSlavePos, INOVANCE_IS620N, 0x6040, 0, &offset.control_word_6040_0}, 140 | {IS620NSlavePos, INOVANCE_IS620N, 0x607A, 0, &offset.target_position_607a_0}, 141 | {IS620NSlavePos, INOVANCE_IS620N, 0x60FF, 0, &offset.target_velocity_60ff_0}, 142 | {IS620NSlavePos, INOVANCE_IS620N, 0x6071, 0, &offset.target_torque_6071_0}, 143 | {IS620NSlavePos, INOVANCE_IS620N, 0x6060, 0, &offset.modes_operation_6060_0}, 144 | {IS620NSlavePos, INOVANCE_IS620N, 0x60B8, 0, &offset.touch_probe_60b8_0}, 145 | {IS620NSlavePos, INOVANCE_IS620N, 0x607F, 0, &offset.max_profile_velocity_607f_0}, 146 | //TxPDO 147 | {IS620NSlavePos, INOVANCE_IS620N, 0x603F, 0, &offset.error_code_603f_0}, 148 | {IS620NSlavePos, INOVANCE_IS620N, 0x6041, 0, &offset.status_word_6041_0}, 149 | {IS620NSlavePos, INOVANCE_IS620N, 0x6064, 0, &offset.position_actual_value_6064_0}, 150 | {IS620NSlavePos, INOVANCE_IS620N, 0x6077, 0, &offset.torque_actual_value_6077_0}, 151 | {IS620NSlavePos, INOVANCE_IS620N, 0x6061, 0, &offset.modes_of_operation_display_6061_0}, 152 | {IS620NSlavePos, INOVANCE_IS620N, 0x60B9, 0, &offset.touch_probe_status_60b9_0}, 153 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BA, 0, &offset.touch_probe_pos1_pos_value_60ba_0}, 154 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BC, 0, &offset.touch_probe_pos2_pos_value_60bc_0}, 155 | {IS620NSlavePos, INOVANCE_IS620N, 0x60FD, 0, &offset.digital_inputs_60fd_0}, 156 | {} 157 | }; 158 | 159 | /*重新映射PDO才需要定义下面这3个数组,如果使用servo的固定映射则不需要*/ 160 | 161 | #if PDO_SETTING1 162 | /*Config PDOs*/ 163 | static ec_pdo_entry_info_t IS620N_pdo_entries[] = { 164 | //RxPdo 0x1702 165 | {0x6040, 0x00, 16}, 166 | {0x607A, 0x00, 32}, 167 | {0x60FF, 0x00, 32}, 168 | {0x6071, 0x00, 16}, 169 | {0x6060, 0x00, 8}, 170 | {0x60B8, 0x00, 16}, 171 | {0x607F, 0x00, 32}, 172 | //TxPdo 0x1B02 173 | {0x603F, 0x00, 16}, 174 | {0x6041, 0x00, 16}, 175 | {0x6064, 0x00, 32}, 176 | {0x6077, 0x00, 16}, 177 | {0x6061, 0x00, 8}, 178 | {0x60B9, 0x00, 16}, 179 | {0x60BA, 0x00, 32}, 180 | {0x60BC, 0x00, 32}, 181 | {0x60FD, 0x00, 32}, 182 | }; 183 | 184 | static ec_pdo_info_t IS620N_pdos[] = { 185 | //RxPdo 186 | {0x1600, 7, IS620N_pdo_entries + 0 }, 187 | //TxPdo 188 | {0x1A00, 9, IS620N_pdo_entries + 7 } 189 | }; 190 | 191 | static ec_sync_info_t IS620N_syncs[] = { 192 | /*{ 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE }, 193 | { 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE },*/ 194 | { 2, EC_DIR_OUTPUT, 1, IS620N_pdos + 0/*,EC_WD_ENABLE*//*, EC_WD_DISABLE*/ }, 195 | { 3, EC_DIR_INPUT, 1, IS620N_pdos + 1/*,EC_WD_DISABLE*//*, EC_WD_DISABLE*/ }, 196 | { 0xFF} 197 | }; 198 | #endif 199 | 200 | /*****************************************************************************/ 201 | 202 | struct timespec timespec_add(struct timespec time1, struct timespec time2) 203 | { 204 | struct timespec result; 205 | 206 | if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) { 207 | result.tv_sec = time1.tv_sec + time2.tv_sec + 1; 208 | result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC; 209 | } else { 210 | result.tv_sec = time1.tv_sec + time2.tv_sec; 211 | result.tv_nsec = time1.tv_nsec + time2.tv_nsec; 212 | } 213 | 214 | return result; 215 | } 216 | 217 | /*****************************************************************************/ 218 | 219 | void endsignal(int sig) 220 | { 221 | servooff = 1; 222 | signal( SIGINT , SIG_DFL ); 223 | } 224 | 225 | /*****************************************************************************/ 226 | 227 | void check_domain1_state(void) 228 | { 229 | ec_domain_state_t ds; 230 | ecrt_domain_state(domain1, &ds); 231 | 232 | //struct timespec time_wc1,time_wc2; 233 | if (ds.working_counter != domain1_state.working_counter) 234 | printf("Domain1: WC %u.\n", ds.working_counter); 235 | if (ds.wc_state != domain1_state.wc_state) 236 | printf("Domain1: State %u.\n", ds.wc_state); 237 | 238 | domain1_state = ds; 239 | } 240 | 241 | /*****************************************************************************/ 242 | 243 | void check_master_state(void) 244 | { 245 | ec_master_state_t ms; 246 | 247 | ecrt_master_state(master, &ms); 248 | 249 | if (ms.slaves_responding != master_state.slaves_responding) 250 | printf("%u slave(s).\n", ms.slaves_responding); 251 | if (ms.al_states != master_state.al_states) 252 | printf("AL states: 0x%02X.\n", ms.al_states); 253 | if (ms.link_up != master_state.link_up) 254 | printf("Link is %s.\n", ms.link_up ? "up" : "down"); 255 | 256 | master_state = ms; 257 | } 258 | 259 | /****************************************************************************/ 260 | 261 | void cyclic_task() 262 | { 263 | struct timespec wakeupTime, time; 264 | // get current time 265 | clock_gettime(CLOCK_TO_USE, &wakeupTime); 266 | 267 | while(1) { 268 | 269 | if(deactive==1){ 270 | break; 271 | } 272 | 273 | wakeupTime = timespec_add(wakeupTime, cycletime); 274 | clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); 275 | 276 | ecrt_master_receive(master); 277 | ecrt_domain_process(domain1); 278 | 279 | temp[0]=EC_READ_U16(domain1_pd + offset.status_word_6041_0); 280 | temp[1]=EC_READ_U32(domain1_pd + offset.position_actual_value_6064_0); 281 | 282 | printf("\r%6f \t ",((float)temp[1]/1000) ); 283 | 284 | if(counter) { 285 | counter--; 286 | } else { // do this at 1 Hz 287 | counter = FREQUENCY; 288 | blink = !blink; 289 | } 290 | 291 | /*if(servooff==1) 292 | value=0; 293 | else if(value<=6.28) 294 | value=value+0.000628; 295 | else 296 | value=0;*/ 297 | 298 | 299 | //printf("after value = %x\n",temp[0]); 300 | // write process data 301 | if(servooff==1){//servo off 302 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0006 ); 303 | //deactive++; 304 | } 305 | else if( (temp[0]&0x004f) == 0x0040 ){ 306 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0006 ); 307 | //printf("1.state = %x\n",temp[0]); 308 | } 309 | else if( (temp[0]&0x006f) == 0x0021){ 310 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0007 ); 311 | //printf("2.state = %x\n",temp[0]); 312 | } 313 | else if( (temp[0]&0x027f) == 0x0233){ 314 | //EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x000f); 315 | //EC_WRITE_S32(domain1_pd+interpolateddata, 0); 316 | //EC_WRITE_S32(domain1_pd+tar_velo, 0xfffff); 317 | //EC_WRITE_S32(domain1_pd+max_torq, 0xf00); 318 | EC_WRITE_S32(domain1_pd+offset.modes_operation_6060_0, 9); 319 | EC_WRITE_U32(domain1_pd+offset.target_velocity_60ff_0, 0x1000000); 320 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x000f); 321 | //printf("3.state = %x\n",temp[0]); 322 | } 323 | else if( (temp[0]&0x027f) == 0x0237){//600 800 324 | //EC_WRITE_S32(domain1_pd+interpolateddata,( value+=1000 )); 325 | //EC_WRITE_U32(domain1_pd+offset.target_velocity_60ff_0, 0x1000000); 326 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x001f); 327 | //printf("4.state = %x\n",temp[0]); 328 | } 329 | 330 | // write application time to master 331 | clock_gettime(CLOCK_TO_USE, &time); 332 | ecrt_master_application_time(master, TIMESPEC2NS(time)); 333 | 334 | if (sync_ref_counter) { 335 | sync_ref_counter--; 336 | } 337 | else { 338 | sync_ref_counter = 1; // sync every cycle 339 | //ecrt_master_sync_reference_clock(master); 340 | ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); 341 | } 342 | ecrt_master_sync_slave_clocks(master); 343 | 344 | //send process data 345 | ecrt_domain_queue(domain1); 346 | //ecrt_domain_queue(domain2); 347 | 348 | ecrt_master_send(master); 349 | } 350 | } 351 | 352 | /****************************************************************************/ 353 | 354 | int main(int argc, char **argv) 355 | { 356 | ec_slave_config_t *sc; 357 | 358 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { 359 | perror("mlockall failed"); 360 | return -1; 361 | } 362 | 363 | master = ecrt_request_master(0); 364 | if (!master) 365 | return -1; 366 | 367 | domain1 = ecrt_master_create_domain(master); 368 | if (!domain1) 369 | return -1; 370 | 371 | if (!(sc = ecrt_master_slave_config( 372 | master, IS620NSlavePos, INOVANCE_IS620N))) { 373 | fprintf(stderr, "Failed to get slave1 configuration.\n"); 374 | return -1; 375 | } 376 | 377 | #if CONFIGURE_PDOS 378 | printf("Configuring PDOs...\n"); 379 | 380 | if (ecrt_slave_config_pdos(sc, EC_END, IS620N_syncs)) { 381 | fprintf(stderr, "Failed to configure 1st PDOs.\n"); 382 | return -1; 383 | } 384 | #endif 385 | /****************motor1馬達domain註冊到domain_process data *******************/ 386 | if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { 387 | fprintf(stderr, "1st motor RX_PDO entry registration failed!\n"); 388 | return -1; 389 | } 390 | 391 | ecrt_slave_config_dc(sc, 0x0300, 4000000, 125000, 0, 0); 392 | 393 | printf("Activating master...\n"); 394 | 395 | if (ecrt_master_activate(master)) 396 | return -1; 397 | 398 | if (!(domain1_pd = ecrt_domain_data(domain1))) { 399 | return -1; 400 | } 401 | 402 | pid_t pid = getpid(); 403 | if (setpriority(PRIO_PROCESS, pid, -20)) 404 | fprintf(stderr, "Warning: Failed to set priority: %s\n", 405 | strerror(errno)); 406 | 407 | signal( SIGINT , endsignal ); //按CTRL+C 利用中斷結束程式 408 | printf("Starting cyclic function.\n"); 409 | cyclic_task(); 410 | ecrt_release_master(master); 411 | 412 | return 0; 413 | } 414 | //no more 415 | -------------------------------------------------------------------------------- /is620n_dc_csp.c: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * $Id: is620n_dc.c,v bc2d4bf9cbe5 2012/09/06 18:22:24 fp $ 4 | * 5 | * Copyright (C) 2007-2009 Florian Pose, Ingenieurgemeinschaft IgH 6 | * 7 | * This file is part of the IgH EtherCAT Master. 8 | * 9 | * The IgH EtherCAT Master is free software; you can redistribute it and/or 10 | * modify it under the terms of the GNU General Public License version 2, as 11 | * published by the Free Software Foundation. 12 | * 13 | * The IgH EtherCAT Master is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 16 | * Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License along 19 | * with the IgH EtherCAT Master; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | * --- 23 | * 24 | * The license mentioned above concerns the source code only. Using the 25 | * EtherCAT technology and brand is only permitted in compliance with the 26 | * industrial property and similar rights of Beckhoff Automation GmbH. 27 | * 28 | * 适用于汇川IS620N伺服(IS620N只有DC模式没有Free Run模式) 29 | * compile: gcc -o is620n_dc_csp is620n_dc_csp.c -Wall -I/opt/etherlab/include -L/opt/etherlab/lib -Wl,--rpath=/opt/etherlab/lib -lethercat -lrt 30 | * run: $sudo ./is620n_dc_csp 31 | * 必须使用igh提供的实时网卡驱动，使用ec_generic.ko的话伺服会出现Er.E08(过程数据错) 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | /* 47 | #include 48 | #include 49 | */ 50 | /****************************************************************************/ 51 | 52 | #include "ecrt.h" 53 | 54 | /****************************************************************************/ 55 | 56 | // Application parameters 57 | #define FREQUENCY 1000 //500 58 | #define CLOCK_TO_USE CLOCK_REALTIME 59 | #define CONFIGURE_PDOS 1 60 | 61 | // Optional features 62 | #define PDO_SETTING1 1 63 | 64 | /****************************************************************************/ 65 | 66 | #define NSEC_PER_SEC (1000000000L) 67 | #define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) 68 | 69 | #define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \ 70 | (B).tv_nsec - (A).tv_nsec) 71 | 72 | #define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec) 73 | 74 | /****************************************************************************/ 75 | 76 | // EtherCAT 77 | static ec_master_t *master = NULL; 78 | static ec_master_state_t master_state = {}; 79 | 80 | static ec_domain_t *domain1 = NULL; 81 | static ec_domain_state_t domain1_state = {}; 82 | 83 | /****************************************************************************/ 84 | static ec_slave_config_t *sc = NULL; 85 | static ec_slave_config_state_t sc_state = {}; 86 | /****************************************************************************/ 87 | 88 | // process data 89 | static uint8_t *domain1_pd = NULL; 90 | 91 | //signal to turn off servo on state 92 | static unsigned int servooff; 93 | static unsigned int deactive; 94 | static signed long temp[8]={}; 95 | 96 | float value = 0; 97 | static unsigned int counter = 0; 98 | static unsigned int blink = 0; 99 | static unsigned int sync_ref_counter = 0; 100 | const struct timespec cycletime = {0, PERIOD_NS}; 101 | 102 | /*****************************************************************************/ 103 | #define IS620NSlavePos 0,0 /*EtherCAT address on the bus*/ 104 | #define INOVANCE_IS620N 0x00100000, 0x000C0108 /*IS620N Vendor ID, product code*/ 105 | 106 | /*x86_64: char-1byte,int-4bytes,long-8bytes*/ 107 | struct IS620N_offset { 108 | //RxPDO 109 | unsigned int control_word_6040_0; //0x6040:控制字,subindex:0,bitlen:16 110 | unsigned int target_position_607a_0;//0x607A:目标位置,subindex:0,bitlen:32 111 | unsigned int touch_probe_60b8_0; //0x60B8:探针,subindex:0,bitlen:16 112 | unsigned int pysical_outputs_60fe_1;//0x60FE:pysical_outputs,subindex:1,bitlen:32 113 | unsigned int target_velocity_60ff_0;//0x60FF:target_velocity,subindex:0,bitlen:32 114 | unsigned int target_torque_6071_0;//0x6071:int target_torque,subindex:0,bitlen:16 115 | unsigned int modes_operation_6060_0;//0x6060:Modes of operation,subindex:0,bitlen:8 116 | unsigned int max_profile_velocity_607f_0;//0x607F:max_profile_velocity,subindex:0,bitlen:32 117 | unsigned int positive_torque_limit_value_60e0_0;//0x60E0:positive_torque_limit_value,subindex:0,bitlen:16 118 | unsigned int negative_torque_limit_value_60e1_0;//0x60E1:negaitive_torque_limit_value,subindex:0,bitlen:16 119 | unsigned int torque_offset_60b2_0;//0x60B2:torque offset,subindex:0,bitlen:16 120 | unsigned int max_torque_6072_0;//0x6072:max torque,subindex:0,bitlen:16 121 | 122 | //TxPDo 123 | unsigned int status_word_6041_0;//0x6041:status_word,subindex:0,bitlen:16 124 | unsigned int position_actual_value_6064_0;//0x6064:position_actual_value,subindex:0,bitlen:32 125 | unsigned int touch_probe_status_60b9_0;//0x60B9,subindex:0,bitlen:16 126 | unsigned int touch_probe_pos1_pos_value_60ba_0;//0x60BA,subindex:0,bitlen:32 127 | unsigned int touch_probe_pos2_pos_value_60bc_0;//0x60BC ,subindex:0,bitlen:32 128 | unsigned int error_code_603f_0;//0x603F,subindex:0,bitlen:16 129 | unsigned int digital_inputs_60fd_0;//0x60FD,subindex:0,bitlen:32 130 | unsigned int torque_actual_value_6077_0;//0x6077,subindex:0,bitlen:16 131 | unsigned int following_error_actual_value_60f4_0;//0x60F4,subindex:0,bitlen:32 132 | unsigned int modes_of_operation_display_6061_0;//0x6061,subindex:0,bitlen:8 133 | unsigned int velocity_actual_value_606c_0;//0x606C,subindex:0,bitlen:32 134 | }; 135 | 136 | static struct IS620N_offset offset; 137 | 138 | const static ec_pdo_entry_reg_t domain1_regs[] = { 139 | //RxPDO 140 | {IS620NSlavePos, INOVANCE_IS620N, 0x6040, 0, &offset.control_word_6040_0}, 141 | {IS620NSlavePos, INOVANCE_IS620N, 0x607A, 0, &offset.target_position_607a_0}, 142 | //{IS620NSlavePos, INOVANCE_IS620N, 0x60FF, 0, &offset.target_velocity_60ff_0}, 143 | //{IS620NSlavePos, INOVANCE_IS620N, 0x6071, 0, &offset.target_torque_6071_0}, 144 | {IS620NSlavePos, INOVANCE_IS620N, 0x6060, 0, &offset.modes_operation_6060_0}, 145 | //{IS620NSlavePos, INOVANCE_IS620N, 0x60B8, 0, &offset.touch_probe_60b8_0}, 146 | //{IS620NSlavePos, INOVANCE_IS620N, 0x607F, 0, &offset.max_profile_velocity_607f_0}, 147 | {IS620NSlavePos, INOVANCE_IS620N, 0x6072, 0, &offset.max_torque_6072_0}, 148 | //TxPDO 149 | {IS620NSlavePos, INOVANCE_IS620N, 0x603F, 0, &offset.error_code_603f_0}, 150 | {IS620NSlavePos, INOVANCE_IS620N, 0x6041, 0, &offset.status_word_6041_0}, 151 | {IS620NSlavePos, INOVANCE_IS620N, 0x6064, 0, &offset.position_actual_value_6064_0}, 152 | {IS620NSlavePos, INOVANCE_IS620N, 0x6077, 0, &offset.torque_actual_value_6077_0}, 153 | {IS620NSlavePos, INOVANCE_IS620N, 0x6061, 0, &offset.modes_of_operation_display_6061_0}, 154 | {IS620NSlavePos, INOVANCE_IS620N, 0x60B9, 0, &offset.touch_probe_status_60b9_0}, 155 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BA, 0, &offset.touch_probe_pos1_pos_value_60ba_0}, 156 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BC, 0, &offset.touch_probe_pos2_pos_value_60bc_0}, 157 | {IS620NSlavePos, INOVANCE_IS620N, 0x60FD, 0, &offset.digital_inputs_60fd_0}, 158 | {} 159 | }; 160 | 161 | /*重新映射PDO才需要定义下面这3个数组,如果使用servo的固定映射则不需要*/ 162 | 163 | #if PDO_SETTING1 164 | /*Config PDOs*/ 165 | static ec_pdo_entry_info_t IS620N_pdo_entries[] = { 166 | //RxPdo 0x1702 167 | {0x6040, 0x00, 16}, 168 | {0x607A, 0x00, 32}, 169 | //{0x60FF, 0x00, 32}, 170 | //{0x6071, 0x00, 16}, 171 | {0x6060, 0x00, 8}, 172 | //{0x60B8, 0x00, 16}, 173 | //{0x607F, 0x00, 32}, 174 | {0x6072, 0x00, 16}, 175 | //TxPdo 0x1B02 176 | {0x603F, 0x00, 16}, 177 | {0x6041, 0x00, 16}, 178 | {0x6064, 0x00, 32}, 179 | {0x6077, 0x00, 16}, 180 | {0x6061, 0x00, 8}, 181 | {0x60B9, 0x00, 16}, 182 | {0x60BA, 0x00, 32}, 183 | {0x60BC, 0x00, 32}, 184 | {0x60FD, 0x00, 32}, 185 | }; 186 | 187 | static ec_pdo_info_t IS620N_pdos[] = { 188 | //RxPdo 189 | {0x1600, 4, IS620N_pdo_entries + 0 }, 190 | //TxPdo 191 | {0x1A00, 9, IS620N_pdo_entries + 4 } 192 | }; 193 | 194 | static ec_sync_info_t IS620N_syncs[] = { 195 | /*{ 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE }, 196 | { 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE },*/ 197 | { 2, EC_DIR_OUTPUT, 1, IS620N_pdos + 0/*,EC_WD_ENABLE*//*, EC_WD_DISABLE*/ }, 198 | { 3, EC_DIR_INPUT, 1, IS620N_pdos + 1/*,EC_WD_DISABLE*//*, EC_WD_DISABLE*/ }, 199 | { 0xFF} 200 | }; 201 | #endif 202 | 203 | /*****************************************************************************/ 204 | 205 | struct timespec timespec_add(struct timespec time1, struct timespec time2) 206 | { 207 | struct timespec result; 208 | 209 | if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) { 210 | result.tv_sec = time1.tv_sec + time2.tv_sec + 1; 211 | result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC; 212 | } else { 213 | result.tv_sec = time1.tv_sec + time2.tv_sec; 214 | result.tv_nsec = time1.tv_nsec + time2.tv_nsec; 215 | } 216 | 217 | return result; 218 | } 219 | 220 | /*****************************************************************************/ 221 | 222 | void endsignal(int sig) 223 | { 224 | servooff = 1; 225 | signal( SIGINT , SIG_DFL ); 226 | } 227 | 228 | /*****************************************************************************/ 229 | 230 | void check_domain1_state(void) 231 | { 232 | ec_domain_state_t ds; 233 | ecrt_domain_state(domain1, &ds); 234 | 235 | //struct timespec time_wc1,time_wc2; 236 | if (ds.working_counter != domain1_state.working_counter) 237 | printf("Domain1: WC %u.\n", ds.working_counter); 238 | if (ds.wc_state != domain1_state.wc_state) 239 | printf("Domain1: State %u.\n", ds.wc_state); 240 | 241 | domain1_state = ds; 242 | } 243 | 244 | /*****************************************************************************/ 245 | 246 | void check_master_state(void) 247 | { 248 | ec_master_state_t ms; 249 | 250 | ecrt_master_state(master, &ms); 251 | 252 | if (ms.slaves_responding != master_state.slaves_responding) 253 | printf("%u slave(s).\n", ms.slaves_responding); 254 | if (ms.al_states != master_state.al_states) 255 | printf("AL states: 0x%02X.\n", ms.al_states); 256 | if (ms.link_up != master_state.link_up) 257 | printf("Link is %s.\n", ms.link_up ? "up" : "down"); 258 | 259 | master_state = ms; 260 | } 261 | 262 | /****************************************************************************/ 263 | 264 | void cyclic_task() 265 | { 266 | uint32_t target_position = 0;int once=0; 267 | 268 | struct timespec wakeupTime, time; 269 | // get current time 270 | clock_gettime(CLOCK_TO_USE, &wakeupTime); 271 | 272 | while(1) { 273 | 274 | /*if(deactive==1){ 275 | break; 276 | }*/ 277 | 278 | wakeupTime = timespec_add(wakeupTime, cycletime); 279 | clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); 280 | 281 | ecrt_master_application_time(master,TIMESPEC2NS(wakeupTime)); //added by me 282 | 283 | ecrt_master_receive(master); 284 | ecrt_domain_process(domain1); 285 | 286 | //printf("\r%6f \t ",((float)temp[1]/1000) ); 287 | 288 | if(counter) { 289 | counter--; 290 | } else { // do this at 1 Hz 291 | counter = FREQUENCY; 292 | blink = !blink; 293 | } 294 | 295 | /*if(servooff==1) 296 | value=0; 297 | else if(value<=6.28) 298 | value=value+0.000628; 299 | else 300 | value=0;*/ 301 | 302 | temp[0]=EC_READ_U16(domain1_pd + offset.status_word_6041_0); 303 | temp[1]=EC_READ_U32(domain1_pd + offset.position_actual_value_6064_0);if(once==0) {target_position=temp[1];once++;} 304 | //printf("\r%6f \t ",((float)temp[1]/1000) ); 305 | //printf("after value = %x\n",temp[0]); 306 | // write process data 307 | /*if(servooff==1){//servo off 308 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0006 ); 309 | //deactive++; 310 | } 311 | else*/if(temp[0] == 0x218){ 312 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0080 ); 313 | } 314 | else if( (temp[0]&0x004f) == 0x0040 ){ //temp[0]==0x250 315 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0006 ); 316 | //printf("1.state = %x\n",temp[0]); 317 | } 318 | else if( (temp[0]&0x006f) == 0x0021){ //temp[0]==0x231 319 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0007 ); 320 | //printf("2.state = %x\n",temp[0]); 321 | } 322 | else if( (temp[0]&0x06f) == 0x023){ //temp[0]==0x233 323 | EC_WRITE_U8(domain1_pd+offset.modes_operation_6060_0, 8);//csp mode 324 | EC_WRITE_S32(domain1_pd+offset.target_position_607a_0,0); 325 | EC_WRITE_S32(domain1_pd+offset.max_torque_6072_0, 0xf00); 326 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x000f); 327 | //printf("3.state = %x\n",temp[0]); 328 | } 329 | else if( (temp[0]&0x06f) == 0x027){//600 800 //temp[0]=0x237 330 | //EC_WRITE_U32(domain1_pd+offset.target_velocity_60ff_0, 0x1000000); 331 | EC_WRITE_S32(domain1_pd+offset.target_position_607a_0,target_position); /////// 332 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x001f); 333 | //printf("4.state = %x\n",temp[0]); 334 | } 335 | 336 | target_position+=0xff; 337 | // write application time to master 338 | //clock_gettime(CLOCK_TO_USE, &time); 339 | //ecrt_master_application_time(master, TIMESPEC2NS(time)); 340 | 341 | if (sync_ref_counter) { 342 | sync_ref_counter--; 343 | } 344 | else { 345 | sync_ref_counter = 1; // sync every cycle 346 | 347 | clock_gettime(CLOCK_TO_USE,&time); //added by me 348 | 349 | //ecrt_master_sync_reference_clock(master); 350 | ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); 351 | } 352 | ecrt_master_sync_slave_clocks(master); 353 | 354 | //send process data 355 | ecrt_domain_queue(domain1); 356 | //ecrt_domain_queue(domain2); 357 | 358 | ecrt_master_send(master); 359 | } 360 | } 361 | 362 | /****************************************************************************/ 363 | 364 | int main(int argc, char **argv) 365 | { 366 | ec_slave_config_t *sc; 367 | 368 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { 369 | perror("mlockall failed"); 370 | return -1; 371 | } 372 | 373 | master = ecrt_request_master(0); 374 | if (!master) 375 | return -1; 376 | 377 | domain1 = ecrt_master_create_domain(master); 378 | if (!domain1) 379 | return -1; 380 | 381 | //配置IS620 382 | if (!(sc = ecrt_master_slave_config( 383 | master, IS620NSlavePos, INOVANCE_IS620N))) { 384 | fprintf(stderr, "Failed to get slave1 configuration.\n"); 385 | return -1; 386 | } 387 | 388 | #if CONFIGURE_PDOS 389 | printf("Configuring PDOs...\n"); 390 | 391 | if (ecrt_slave_config_pdos(sc, EC_END, IS620N_syncs)) { 392 | fprintf(stderr, "Failed to configure 1st PDOs.\n"); 393 | return -1; 394 | } 395 | #endif 396 | /****************motor1馬達domain註冊到domain_process data *******************/ 397 | if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { 398 | fprintf(stderr, "1st motor RX_PDO entry registration failed!\n"); 399 | return -1; 400 | } 401 | //配置IS620结束 402 | 403 | //ecrt_slave_config_dc(sc, 0x0300, 4000000, 125000, 0, 0); 404 | //ecrt_slave_config_dc(sc, 0x0300, 4000000, 4000000/2, 0, 0); 405 | ecrt_slave_config_dc(sc, 0x0300, 2000000, 2000000/2, 0, 0);//行 406 | 407 | printf("Activating master...\n"); 408 | 409 | if (ecrt_master_activate(master)) 410 | return -1; 411 | 412 | if (!(domain1_pd = ecrt_domain_data(domain1))) { 413 | return -1; 414 | } 415 | 416 | pid_t pid = getpid(); 417 | if (setpriority(PRIO_PROCESS, pid, -20)) 418 | fprintf(stderr, "Warning: Failed to set priority: %s\n", 419 | strerror(errno)); 420 | 421 | signal( SIGINT , endsignal ); //按CTRL+C 利用中斷結束程式 422 | printf("Starting cyclic function.\n"); 423 | cyclic_task(); 424 | ecrt_release_master(master); 425 | 426 | return 0; 427 | } 428 | //no more 429 | -------------------------------------------------------------------------------- /is620n_dc_cst.c: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * $Id: is620n_dc.c,v bc2d4bf9cbe5 2012/09/06 18:22:24 fp $ 4 | * 5 | * Copyright (C) 2007-2009 Florian Pose, Ingenieurgemeinschaft IgH 6 | * 7 | * This file is part of the IgH EtherCAT Master. 8 | * 9 | * The IgH EtherCAT Master is free software; you can redistribute it and/or 10 | * modify it under the terms of the GNU General Public License version 2, as 11 | * published by the Free Software Foundation. 12 | * 13 | * The IgH EtherCAT Master is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 16 | * Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License along 19 | * with the IgH EtherCAT Master; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | * --- 23 | * 24 | * The license mentioned above concerns the source code only. Using the 25 | * EtherCAT technology and brand is only permitted in compliance with the 26 | * industrial property and similar rights of Beckhoff Automation GmbH. 27 | * 28 | * 适用于汇川IS620N伺服(IS620N只有DC模式没有Free Run模式) 29 | * compile: gcc -o is620n_dc_cst is620n_dc_cst.c -Wall -I/opt/etherlab/include -L/opt/etherlab/lib -Wl,--rpath=/opt/etherlab/lib -lethercat -lrt 30 | * run: $sudo ./is620n_dc_cst 31 | * 必须使用igh提供的实时网卡驱动，使用ec_generic.ko的话伺服会出现Er.E08(过程数据错) 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | /* 47 | #include 48 | #include 49 | */ 50 | /****************************************************************************/ 51 | 52 | #include "ecrt.h" 53 | 54 | /****************************************************************************/ 55 | 56 | // Application parameters 57 | #define FREQUENCY 1000//500 58 | #define CLOCK_TO_USE CLOCK_REALTIME 59 | #define CONFIGURE_PDOS 1 60 | 61 | // Optional features 62 | #define PDO_SETTING1 1 63 | 64 | /****************************************************************************/ 65 | 66 | #define NSEC_PER_SEC (1000000000L) 67 | #define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) 68 | 69 | #define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \ 70 | (B).tv_nsec - (A).tv_nsec) 71 | 72 | #define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec) 73 | 74 | /****************************************************************************/ 75 | 76 | // EtherCAT 77 | static ec_master_t *master = NULL; 78 | static ec_master_state_t master_state = {}; 79 | 80 | static ec_domain_t *domain1 = NULL; 81 | static ec_domain_state_t domain1_state = {}; 82 | 83 | /****************************************************************************/ 84 | static ec_slave_config_t *sc = NULL; 85 | static ec_slave_config_state_t sc_state = {}; 86 | /****************************************************************************/ 87 | 88 | // process data 89 | static uint8_t *domain1_pd = NULL; 90 | 91 | //signal to turn off servo on state 92 | static unsigned int servooff; 93 | static unsigned int deactive; 94 | static signed long temp[8]={}; 95 | 96 | float value = 0; 97 | static unsigned int counter = 0; 98 | static unsigned int blink = 0; 99 | static unsigned int sync_ref_counter = 0; 100 | const struct timespec cycletime = {0, PERIOD_NS}; 101 | 102 | /*****************************************************************************/ 103 | #define IS620NSlavePos 0,0 /*EtherCAT address on the bus*/ 104 | #define INOVANCE_IS620N 0x00100000, 0x000C0108 /*IS620N Vendor ID, product code*/ 105 | 106 | /*x86_64: char-1byte,int-4bytes,long-8bytes*/ 107 | struct IS620N_offset { 108 | //RxPDO 109 | unsigned int control_word_6040_0; //0x6040:控制字,subindex:0,bitlen:16 110 | unsigned int target_position_607a_0;//0x607A:目标位置,subindex:0,bitlen:32 111 | unsigned int touch_probe_60b8_0; //0x60B8:探针,subindex:0,bitlen:16 112 | unsigned int pysical_outputs_60fe_1;//0x60FE:pysical_outputs,subindex:1,bitlen:32 113 | unsigned int target_velocity_60ff_0;//0x60FF:target_velocity,subindex:0,bitlen:32 114 | unsigned int target_torque_6071_0;//0x6071:int target_torque,subindex:0,bitlen:16 115 | unsigned int modes_operation_6060_0;//0x6060:Modes of operation,subindex:0,bitlen:8 116 | unsigned int max_profile_velocity_607f_0;//0x607F:max_profile_velocity,subindex:0,bitlen:32 117 | unsigned int positive_torque_limit_value_60e0_0;//0x60E0:positive_torque_limit_value,subindex:0,bitlen:16 118 | unsigned int negative_torque_limit_value_60e1_0;//0x60E1:negaitive_torque_limit_value,subindex:0,bitlen:16 119 | unsigned int torque_offset_60b2_0;//0x60B2:torque offset,subindex:0,bitlen:16 120 | unsigned int max_torque_6072_0;//0x6072:max torque,subindex:0,bitlen:16 121 | 122 | //TxPDo 123 | unsigned int status_word_6041_0;//0x6041:status_word,subindex:0,bitlen:16 124 | unsigned int position_actual_value_6064_0;//0x6064:position_actual_value,subindex:0,bitlen:32 125 | unsigned int touch_probe_status_60b9_0;//0x60B9,subindex:0,bitlen:16 126 | unsigned int touch_probe_pos1_pos_value_60ba_0;//0x60BA,subindex:0,bitlen:32 127 | unsigned int touch_probe_pos2_pos_value_60bc_0;//0x60BC ,subindex:0,bitlen:32 128 | unsigned int error_code_603f_0;//0x603F,subindex:0,bitlen:16 129 | unsigned int digital_inputs_60fd_0;//0x60FD,subindex:0,bitlen:32 130 | unsigned int torque_actual_value_6077_0;//0x6077,subindex:0,bitlen:16 131 | unsigned int following_error_actual_value_60f4_0;//0x60F4,subindex:0,bitlen:32 132 | unsigned int modes_of_operation_display_6061_0;//0x6061,subindex:0,bitlen:8 133 | unsigned int velocity_actual_value_606c_0;//0x606C,subindex:0,bitlen:32 134 | }; 135 | 136 | static struct IS620N_offset offset; 137 | 138 | const static ec_pdo_entry_reg_t domain1_regs[] = { 139 | //RxPDO 140 | {IS620NSlavePos, INOVANCE_IS620N, 0x6040, 0, &offset.control_word_6040_0}, 141 | //{IS620NSlavePos, INOVANCE_IS620N, 0x607A, 0, &offset.target_position_607a_0}, 142 | //{IS620NSlavePos, INOVANCE_IS620N, 0x60FF, 0, &offset.target_velocity_60ff_0}, 143 | {IS620NSlavePos, INOVANCE_IS620N, 0x6071, 0, &offset.target_torque_6071_0}, 144 | {IS620NSlavePos, INOVANCE_IS620N, 0x6060, 0, &offset.modes_operation_6060_0}, 145 | //{IS620NSlavePos, INOVANCE_IS620N, 0x60B8, 0, &offset.touch_probe_60b8_0}, 146 | //{IS620NSlavePos, INOVANCE_IS620N, 0x607F, 0, &offset.max_profile_velocity_607f_0}, 147 | //TxPDO 148 | {IS620NSlavePos, INOVANCE_IS620N, 0x603F, 0, &offset.error_code_603f_0}, 149 | {IS620NSlavePos, INOVANCE_IS620N, 0x6041, 0, &offset.status_word_6041_0}, 150 | {IS620NSlavePos, INOVANCE_IS620N, 0x6064, 0, &offset.position_actual_value_6064_0}, 151 | {IS620NSlavePos, INOVANCE_IS620N, 0x6077, 0, &offset.torque_actual_value_6077_0}, 152 | {IS620NSlavePos, INOVANCE_IS620N, 0x6061, 0, &offset.modes_of_operation_display_6061_0}, 153 | {IS620NSlavePos, INOVANCE_IS620N, 0x60B9, 0, &offset.touch_probe_status_60b9_0}, 154 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BA, 0, &offset.touch_probe_pos1_pos_value_60ba_0}, 155 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BC, 0, &offset.touch_probe_pos2_pos_value_60bc_0}, 156 | {IS620NSlavePos, INOVANCE_IS620N, 0x60FD, 0, &offset.digital_inputs_60fd_0}, 157 | {} 158 | }; 159 | 160 | /*重新映射PDO才需要定义下面这3个数组,如果使用servo的固定映射则不需要*/ 161 | 162 | #if PDO_SETTING1 163 | /*Config PDOs*/ 164 | static ec_pdo_entry_info_t IS620N_pdo_entries[] = { 165 | //RxPdo 0x1702 166 | {0x6040, 0x00, 16}, 167 | //{0x607A, 0x00, 32}, 168 | //{0x60FF, 0x00, 32}, 169 | {0x6071, 0x00, 16}, 170 | {0x6060, 0x00, 8}, 171 | //{0x60B8, 0x00, 16}, 172 | //{0x607F, 0x00, 32}, 173 | //TxPdo 0x1B02 174 | {0x603F, 0x00, 16}, 175 | {0x6041, 0x00, 16}, 176 | {0x6064, 0x00, 32}, 177 | {0x6077, 0x00, 16}, 178 | {0x6061, 0x00, 8}, 179 | {0x60B9, 0x00, 16}, 180 | {0x60BA, 0x00, 32}, 181 | {0x60BC, 0x00, 32}, 182 | {0x60FD, 0x00, 32}, 183 | }; 184 | 185 | static ec_pdo_info_t IS620N_pdos[] = { 186 | //RxPdo 187 | {0x1600, 3, IS620N_pdo_entries + 0 }, 188 | //TxPdo 189 | {0x1A00, 9, IS620N_pdo_entries + 3 } 190 | }; 191 | 192 | static ec_sync_info_t IS620N_syncs[] = { 193 | /*{ 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE }, 194 | { 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE },*/ 195 | { 2, EC_DIR_OUTPUT, 1, IS620N_pdos + 0/*,EC_WD_ENABLE*//*, EC_WD_DISABLE*/ }, 196 | { 3, EC_DIR_INPUT, 1, IS620N_pdos + 1/*,EC_WD_DISABLE*//*, EC_WD_DISABLE*/ }, 197 | { 0xFF} 198 | }; 199 | #endif 200 | 201 | /*****************************************************************************/ 202 | 203 | struct timespec timespec_add(struct timespec time1, struct timespec time2) 204 | { 205 | struct timespec result; 206 | 207 | if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) { 208 | result.tv_sec = time1.tv_sec + time2.tv_sec + 1; 209 | result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC; 210 | } else { 211 | result.tv_sec = time1.tv_sec + time2.tv_sec; 212 | result.tv_nsec = time1.tv_nsec + time2.tv_nsec; 213 | } 214 | 215 | return result; 216 | } 217 | 218 | /*****************************************************************************/ 219 | 220 | void endsignal(int sig) 221 | { 222 | servooff = 1; 223 | signal( SIGINT , SIG_DFL ); 224 | } 225 | 226 | /*****************************************************************************/ 227 | 228 | void check_domain1_state(void) 229 | { 230 | ec_domain_state_t ds; 231 | ecrt_domain_state(domain1, &ds); 232 | 233 | //struct timespec time_wc1,time_wc2; 234 | if (ds.working_counter != domain1_state.working_counter) 235 | printf("Domain1: WC %u.\n", ds.working_counter); 236 | if (ds.wc_state != domain1_state.wc_state) 237 | printf("Domain1: State %u.\n", ds.wc_state); 238 | 239 | domain1_state = ds; 240 | } 241 | 242 | /*****************************************************************************/ 243 | 244 | void check_master_state(void) 245 | { 246 | ec_master_state_t ms; 247 | 248 | ecrt_master_state(master, &ms); 249 | 250 | if (ms.slaves_responding != master_state.slaves_responding) 251 | printf("%u slave(s).\n", ms.slaves_responding); 252 | if (ms.al_states != master_state.al_states) 253 | printf("AL states: 0x%02X.\n", ms.al_states); 254 | if (ms.link_up != master_state.link_up) 255 | printf("Link is %s.\n", ms.link_up ? "up" : "down"); 256 | 257 | master_state = ms; 258 | } 259 | 260 | /****************************************************************************/ 261 | 262 | void cyclic_task() 263 | { 264 | uint32_t target_position = 0xfffff; 265 | 266 | struct timespec wakeupTime, time; 267 | // get current time 268 | clock_gettime(CLOCK_TO_USE, &wakeupTime); 269 | 270 | while(1) { 271 | 272 | /*if(deactive==1){ 273 | break; 274 | }*/ 275 | 276 | wakeupTime = timespec_add(wakeupTime, cycletime); 277 | clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); 278 | 279 | ecrt_master_application_time(master,TIMESPEC2NS(wakeupTime)); //added by me 280 | 281 | ecrt_master_receive(master); 282 | ecrt_domain_process(domain1); 283 | 284 | //printf("\r%6f \t ",((float)temp[1]/1000) ); 285 | 286 | if(counter) { 287 | counter--; 288 | } else { // do this at 1 Hz 289 | counter = FREQUENCY; 290 | blink = !blink; 291 | } 292 | 293 | /*if(servooff==1) 294 | value=0; 295 | else if(value<=6.28) 296 | value=value+0.000628; 297 | else 298 | value=0;*/ 299 | 300 | temp[0]=EC_READ_U16(domain1_pd + offset.status_word_6041_0); 301 | temp[1]=EC_READ_U32(domain1_pd + offset.position_actual_value_6064_0); 302 | 303 | //printf("after value = %x\n",temp[0]); 304 | // write process data 305 | /*if(servooff==1){//servo off 306 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0006 ); 307 | //deactive++; 308 | } 309 | else*/if(temp[0] == 0x218){ 310 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0080 ); 311 | } 312 | else if( (temp[0]&0x004f) == 0x0040 ){ //temp[0]==0x250 313 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0006 ); 314 | //printf("1.state = %x\n",temp[0]); 315 | } 316 | else if( (temp[0]&0x006f) == 0x0021){ //temp[0]==0x231 317 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0007 ); 318 | //printf("2.state = %x\n",temp[0]); 319 | } 320 | else if( (temp[0]&0x06f) == 0x0023){ //temp[0]==0x233 321 | //EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x000f); 322 | //EC_WRITE_S32(domain1_pd+interpolateddata, 0); 323 | //EC_WRITE_S32(domain1_pd+tar_velo, 0xfffff); 324 | //EC_WRITE_S32(domain1_pd+max_torq, 0xf00); 325 | 326 | EC_WRITE_S8(domain1_pd+offset.modes_operation_6060_0, 0x0a);//cst mode 327 | //EC_WRITE_S32(domain1_pd+offset.target_position_607a_0,target_position); 328 | //EC_WRITE_S32(domain1_pd+offset.target_velocity_60ff_0, 0x1000000); 329 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x000f); 330 | //printf("3.state = %x\n",temp[0]); 331 | } 332 | else if( (temp[0]&0x06f) == 0x0027){//600 800 //temp[0]=0x237 333 | //EC_WRITE_S32(domain1_pd+interpolateddata,( value+=1000 )); 334 | //EC_WRITE_U32(domain1_pd+offset.target_velocity_60ff_0, 0x1000000); 335 | 336 | //EC_WRITE_U16(domain1_pd+offset.target_position_607a_0,(target_position+=0xfff)); /////// 337 | EC_WRITE_S16(domain1_pd+offset.target_torque_6071_0, 1000); 338 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x001f); 339 | //printf("4.state = %x\n",temp[0]); 340 | } 341 | 342 | // write application time to master 343 | //clock_gettime(CLOCK_TO_USE, &time); 344 | //ecrt_master_application_time(master, TIMESPEC2NS(time)); 345 | 346 | if (sync_ref_counter) { 347 | sync_ref_counter--; 348 | } 349 | else { 350 | sync_ref_counter = 1; // sync every cycle 351 | 352 | clock_gettime(CLOCK_TO_USE,&time); //added by me 353 | 354 | //ecrt_master_sync_reference_clock(master); 355 | ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); 356 | } 357 | ecrt_master_sync_slave_clocks(master); 358 | 359 | //send process data 360 | ecrt_domain_queue(domain1); 361 | //ecrt_domain_queue(domain2); 362 | 363 | ecrt_master_send(master); 364 | } 365 | } 366 | 367 | /****************************************************************************/ 368 | 369 | int main(int argc, char **argv) 370 | { 371 | ec_slave_config_t *sc; 372 | 373 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { 374 | perror("mlockall failed"); 375 | return -1; 376 | } 377 | 378 | master = ecrt_request_master(0); 379 | if (!master) 380 | return -1; 381 | 382 | domain1 = ecrt_master_create_domain(master); 383 | if (!domain1) 384 | return -1; 385 | 386 | //配置IS620 387 | if (!(sc = ecrt_master_slave_config( 388 | master, IS620NSlavePos, INOVANCE_IS620N))) { 389 | fprintf(stderr, "Failed to get slave1 configuration.\n"); 390 | return -1; 391 | } 392 | 393 | #if CONFIGURE_PDOS 394 | printf("Configuring PDOs...\n"); 395 | 396 | if (ecrt_slave_config_pdos(sc, EC_END, IS620N_syncs)) { 397 | fprintf(stderr, "Failed to configure 1st PDOs.\n"); 398 | return -1; 399 | } 400 | #endif 401 | /****************motor1馬達domain註冊到domain_process data *******************/ 402 | if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { 403 | fprintf(stderr, "1st motor RX_PDO entry registration failed!\n"); 404 | return -1; 405 | } 406 | //配置IS620结束 407 | 408 | //ecrt_slave_config_dc(sc, 0x0300, 4000000, 125000, 0, 0); 409 | //ecrt_slave_config_dc(sc, 0x0300, 4000000, 4000000/2, 0, 0); 410 | ecrt_slave_config_dc(sc, 0x0300, 2000000, 2000000/2, 0, 0);//不行 411 | 412 | printf("Activating master...\n"); 413 | 414 | if (ecrt_master_activate(master)) 415 | return -1; 416 | 417 | if (!(domain1_pd = ecrt_domain_data(domain1))) { 418 | return -1; 419 | } 420 | 421 | pid_t pid = getpid(); 422 | if (setpriority(PRIO_PROCESS, pid, -20)) 423 | fprintf(stderr, "Warning: Failed to set priority: %s\n", 424 | strerror(errno)); 425 | 426 | signal( SIGINT , endsignal ); //按CTRL+C 利用中斷結束程式 427 | printf("Starting cyclic function.\n"); 428 | cyclic_task(); 429 | ecrt_release_master(master); 430 | 431 | return 0; 432 | } 433 | //no more 434 | -------------------------------------------------------------------------------- /is620n_dc_csv.c: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * $Id: is620n_dc.c,v bc2d4bf9cbe5 2012/09/06 18:22:24 fp $ 4 | * 5 | * Copyright (C) 2007-2009 Florian Pose, Ingenieurgemeinschaft IgH 6 | * 7 | * This file is part of the IgH EtherCAT Master. 8 | * 9 | * The IgH EtherCAT Master is free software; you can redistribute it and/or 10 | * modify it under the terms of the GNU General Public License version 2, as 11 | * published by the Free Software Foundation. 12 | * 13 | * The IgH EtherCAT Master is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 16 | * Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License along 19 | * with the IgH EtherCAT Master; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | * --- 23 | * 24 | * The license mentioned above concerns the source code only. Using the 25 | * EtherCAT technology and brand is only permitted in compliance with the 26 | * industrial property and similar rights of Beckhoff Automation GmbH. 27 | * 28 | * 适用于汇川IS620N伺服(IS620N只有DC模式没有Free Run模式) 29 | * compile: gcc -o is620n_dc_csv is620n_dc_csv.c -Wall -Wl,--rpath=/opt/etherlab/lib -I/opt/etherlab/include -lethercat -L/opt/etherlab/lib 30 | * run: $sudo ./is620n_dc_csv 31 | * 必须使用igh提供的实时网卡驱动，使用ec_generic.ko的话伺服会出现Er.E08(过程数据错) 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | /* 47 | #include 48 | #include 49 | */ 50 | /****************************************************************************/ 51 | 52 | #include "ecrt.h" 53 | 54 | /****************************************************************************/ 55 | 56 | // Application parameters 57 | #define FREQUENCY 1000 //500 58 | #define CLOCK_TO_USE CLOCK_REALTIME 59 | #define CONFIGURE_PDOS 1 60 | 61 | // Optional features 62 | #define PDO_SETTING1 1 63 | 64 | /****************************************************************************/ 65 | 66 | #define NSEC_PER_SEC (1000000000L) 67 | #define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) 68 | 69 | #define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \ 70 | (B).tv_nsec - (A).tv_nsec) 71 | 72 | #define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec) 73 | 74 | /****************************************************************************/ 75 | 76 | // EtherCAT 77 | static ec_master_t *master = NULL; 78 | static ec_master_state_t master_state = {}; 79 | 80 | static ec_domain_t *domain1 = NULL; 81 | static ec_domain_state_t domain1_state = {}; 82 | 83 | /****************************************************************************/ 84 | static ec_slave_config_t *sc = NULL; 85 | static ec_slave_config_state_t sc_state = {}; 86 | /****************************************************************************/ 87 | 88 | // process data 89 | static uint8_t *domain1_pd = NULL; 90 | 91 | //signal to turn off servo on state 92 | static unsigned int servooff; 93 | static unsigned int deactive; 94 | static signed long temp[8]={}; 95 | 96 | float value = 0; 97 | static unsigned int counter = 0; 98 | static unsigned int blink = 0; 99 | static unsigned int sync_ref_counter = 0; 100 | const struct timespec cycletime = {0, PERIOD_NS}; 101 | 102 | /*****************************************************************************/ 103 | #define IS620NSlavePos 0,0 /*EtherCAT address on the bus*/ 104 | #define INOVANCE_IS620N 0x00100000, 0x000C0108 /*IS620N Vendor ID, product code*/ 105 | 106 | /*x86_64: char-1byte,int-4bytes,long-8bytes*/ 107 | struct IS620N_offset { 108 | //RxPDO 109 | unsigned int control_word_6040_0; //0x6040:控制字,subindex:0,bitlen:16 110 | unsigned int target_position_607a_0;//0x607A:目标位置,subindex:0,bitlen:32 111 | unsigned int touch_probe_60b8_0; //0x60B8:探针,subindex:0,bitlen:16 112 | unsigned int pysical_outputs_60fe_1;//0x60FE:pysical_outputs,subindex:1,bitlen:32 113 | unsigned int target_velocity_60ff_0;//0x60FF:target_velocity,subindex:0,bitlen:32 114 | unsigned int target_torque_6071_0;//0x6071:int target_torque,subindex:0,bitlen:16 115 | unsigned int modes_operation_6060_0;//0x6060:Modes of operation,subindex:0,bitlen:8 116 | unsigned int max_profile_velocity_607f_0;//0x607F:max_profile_velocity,subindex:0,bitlen:32 117 | unsigned int positive_torque_limit_value_60e0_0;//0x60E0:positive_torque_limit_value,subindex:0,bitlen:16 118 | unsigned int negative_torque_limit_value_60e1_0;//0x60E1:negaitive_torque_limit_value,subindex:0,bitlen:16 119 | unsigned int torque_offset_60b2_0;//0x60B2:torque offset,subindex:0,bitlen:16 120 | unsigned int max_torque_6072_0;//0x6072:max torque,subindex:0,bitlen:16 121 | 122 | //TxPDo 123 | unsigned int status_word_6041_0;//0x6041:status_word,subindex:0,bitlen:16 124 | unsigned int position_actual_value_6064_0;//0x6064:position_actual_value,subindex:0,bitlen:32 125 | unsigned int touch_probe_status_60b9_0;//0x60B9,subindex:0,bitlen:16 126 | unsigned int touch_probe_pos1_pos_value_60ba_0;//0x60BA,subindex:0,bitlen:32 127 | unsigned int touch_probe_pos2_pos_value_60bc_0;//0x60BC ,subindex:0,bitlen:32 128 | unsigned int error_code_603f_0;//0x603F,subindex:0,bitlen:16 129 | unsigned int digital_inputs_60fd_0;//0x60FD,subindex:0,bitlen:32 130 | unsigned int torque_actual_value_6077_0;//0x6077,subindex:0,bitlen:16 131 | unsigned int following_error_actual_value_60f4_0;//0x60F4,subindex:0,bitlen:32 132 | unsigned int modes_of_operation_display_6061_0;//0x6061,subindex:0,bitlen:8 133 | unsigned int velocity_actual_value_606c_0;//0x606C,subindex:0,bitlen:32 134 | }; 135 | 136 | static struct IS620N_offset offset; 137 | 138 | const static ec_pdo_entry_reg_t domain1_regs[] = { 139 | //RxPDO 140 | {IS620NSlavePos, INOVANCE_IS620N, 0x6040, 0, &offset.control_word_6040_0}, 141 | {IS620NSlavePos, INOVANCE_IS620N, 0x607A, 0, &offset.target_position_607a_0}, 142 | {IS620NSlavePos, INOVANCE_IS620N, 0x60FF, 0, &offset.target_velocity_60ff_0}, 143 | {IS620NSlavePos, INOVANCE_IS620N, 0x6071, 0, &offset.target_torque_6071_0}, 144 | {IS620NSlavePos, INOVANCE_IS620N, 0x6060, 0, &offset.modes_operation_6060_0}, 145 | {IS620NSlavePos, INOVANCE_IS620N, 0x60B8, 0, &offset.touch_probe_60b8_0}, 146 | {IS620NSlavePos, INOVANCE_IS620N, 0x607F, 0, &offset.max_profile_velocity_607f_0}, 147 | //TxPDO 148 | {IS620NSlavePos, INOVANCE_IS620N, 0x603F, 0, &offset.error_code_603f_0}, 149 | {IS620NSlavePos, INOVANCE_IS620N, 0x6041, 0, &offset.status_word_6041_0}, 150 | {IS620NSlavePos, INOVANCE_IS620N, 0x6064, 0, &offset.position_actual_value_6064_0}, 151 | {IS620NSlavePos, INOVANCE_IS620N, 0x6077, 0, &offset.torque_actual_value_6077_0}, 152 | {IS620NSlavePos, INOVANCE_IS620N, 0x6061, 0, &offset.modes_of_operation_display_6061_0}, 153 | {IS620NSlavePos, INOVANCE_IS620N, 0x60B9, 0, &offset.touch_probe_status_60b9_0}, 154 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BA, 0, &offset.touch_probe_pos1_pos_value_60ba_0}, 155 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BC, 0, &offset.touch_probe_pos2_pos_value_60bc_0}, 156 | {IS620NSlavePos, INOVANCE_IS620N, 0x60FD, 0, &offset.digital_inputs_60fd_0}, 157 | {} 158 | }; 159 | 160 | /*重新映射PDO才需要定义下面这3个数组,如果使用servo的固定映射则不需要*/ 161 | 162 | #if PDO_SETTING1 163 | /*Config PDOs*/ 164 | static ec_pdo_entry_info_t IS620N_pdo_entries[] = { 165 | //RxPdo 0x1702 166 | {0x6040, 0x00, 16}, 167 | {0x607A, 0x00, 32}, 168 | {0x60FF, 0x00, 32}, 169 | {0x6071, 0x00, 16}, 170 | {0x6060, 0x00, 8}, 171 | {0x60B8, 0x00, 16}, 172 | {0x607F, 0x00, 32}, 173 | //TxPdo 0x1B02 174 | {0x603F, 0x00, 16}, 175 | {0x6041, 0x00, 16}, 176 | {0x6064, 0x00, 32}, 177 | {0x6077, 0x00, 16}, 178 | {0x6061, 0x00, 8}, 179 | {0x60B9, 0x00, 16}, 180 | {0x60BA, 0x00, 32}, 181 | {0x60BC, 0x00, 32}, 182 | {0x60FD, 0x00, 32}, 183 | }; 184 | 185 | static ec_pdo_info_t IS620N_pdos[] = { 186 | //RxPdo 187 | {0x1600, 7, IS620N_pdo_entries + 0 }, 188 | //TxPdo 189 | {0x1A00, 9, IS620N_pdo_entries + 7 } 190 | }; 191 | 192 | static ec_sync_info_t IS620N_syncs[] = { 193 | /*{ 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE }, 194 | { 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE },*/ 195 | { 2, EC_DIR_OUTPUT, 1, IS620N_pdos + 0/*,EC_WD_ENABLE*//*, EC_WD_DISABLE*/ }, 196 | { 3, EC_DIR_INPUT, 1, IS620N_pdos + 1/*,EC_WD_DISABLE*//*, EC_WD_DISABLE*/ }, 197 | { 0xFF} 198 | }; 199 | #endif 200 | 201 | /*****************************************************************************/ 202 | 203 | struct timespec timespec_add(struct timespec time1, struct timespec time2) 204 | { 205 | struct timespec result; 206 | 207 | if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) { 208 | result.tv_sec = time1.tv_sec + time2.tv_sec + 1; 209 | result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC; 210 | } else { 211 | result.tv_sec = time1.tv_sec + time2.tv_sec; 212 | result.tv_nsec = time1.tv_nsec + time2.tv_nsec; 213 | } 214 | 215 | return result; 216 | } 217 | 218 | /*****************************************************************************/ 219 | 220 | void endsignal(int sig) 221 | { 222 | servooff = 1; 223 | signal( SIGINT , SIG_DFL ); 224 | } 225 | 226 | /*****************************************************************************/ 227 | 228 | void check_domain1_state(void) 229 | { 230 | ec_domain_state_t ds; 231 | ecrt_domain_state(domain1, &ds); 232 | 233 | //struct timespec time_wc1,time_wc2; 234 | if (ds.working_counter != domain1_state.working_counter) 235 | printf("Domain1: WC %u.\n", ds.working_counter); 236 | if (ds.wc_state != domain1_state.wc_state) 237 | printf("Domain1: State %u.\n", ds.wc_state); 238 | 239 | domain1_state = ds; 240 | } 241 | 242 | /*****************************************************************************/ 243 | 244 | void check_master_state(void) 245 | { 246 | ec_master_state_t ms; 247 | 248 | ecrt_master_state(master, &ms); 249 | 250 | if (ms.slaves_responding != master_state.slaves_responding) 251 | printf("%u slave(s).\n", ms.slaves_responding); 252 | if (ms.al_states != master_state.al_states) 253 | printf("AL states: 0x%02X.\n", ms.al_states); 254 | if (ms.link_up != master_state.link_up) 255 | printf("Link is %s.\n", ms.link_up ? "up" : "down"); 256 | 257 | master_state = ms; 258 | } 259 | 260 | /****************************************************************************/ 261 | 262 | void cyclic_task() 263 | { 264 | uint32_t target_position = 0xfffff; 265 | 266 | struct timespec wakeupTime, time,rx_time_start,rx_time_end,tx_time_start,tx_time_end; 267 | // get current time 268 | clock_gettime(CLOCK_TO_USE, &wakeupTime); 269 | 270 | while(1) { 271 | 272 | /*if(deactive==1){ 273 | break; 274 | }*/ 275 | 276 | wakeupTime = timespec_add(wakeupTime, cycletime); 277 | clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); 278 | 279 | ecrt_master_application_time(master,TIMESPEC2NS(wakeupTime)); //added by me 280 | /*clock_gettime(CLOCK_TO_USE, &rx_time_start);*/ 281 | ecrt_master_receive(master); 282 | /*clock_gettime(CLOCK_TO_USE, &rx_time_end); 283 | printf("rx:%ldns ",(rx_time_end.tv_sec-rx_time_start.tv_sec)*NSEC_PER_SEC+rx_time_end.tv_nsec-rx_time_start.tv_nsec);*/ 284 | ecrt_domain_process(domain1); 285 | 286 | //printf("\r%6f \t ",((float)temp[1]/1000) ); 287 | 288 | if(counter) { 289 | counter--; 290 | } else { // do this at 1 Hz 291 | counter = FREQUENCY; 292 | blink = !blink; 293 | } 294 | 295 | /*if(servooff==1) 296 | value=0; 297 | else if(value<=6.28) 298 | value=value+0.000628; 299 | else 300 | value=0;*/ 301 | 302 | temp[0]=EC_READ_U16(domain1_pd + offset.status_word_6041_0); 303 | temp[1]=EC_READ_U32(domain1_pd + offset.position_actual_value_6064_0); 304 | //printf("\r%6f \t ",((float)temp[1]/1000) ); 305 | //printf("after value = %x\n",temp[0]); 306 | // write process data 307 | /*if(servooff==1){//servo off 308 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0006 ); 309 | //deactive++; 310 | } 311 | else*/if(temp[0] == 0x218){ 312 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0080 ); 313 | } 314 | else if( (temp[0]&0x04f) == 0x0040 ){ //temp[0]==0x250 315 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0006 ); 316 | //printf("1.state = %x\n",temp[0]); 317 | } 318 | else if( (temp[0]&0x06f) == 0x0021){ //temp[0]==0x231 319 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0007 ); 320 | //printf("2.state = %x\n",temp[0]); 321 | } 322 | else if( (temp[0]&0x06f) == 0x023){ //temp[0]==0x233 323 | //EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x000f); 324 | //EC_WRITE_S32(domain1_pd+interpolateddata, 0); 325 | //EC_WRITE_S32(domain1_pd+tar_velo, 0xfffff); 326 | //EC_WRITE_S32(domain1_pd+max_torq, 0xf00); 327 | 328 | EC_WRITE_S8(domain1_pd+offset.modes_operation_6060_0, 9);//csv mode 329 | //EC_WRITE_U16(domain1_pd+offset.target_position_607a_0,temp[1]); 330 | //EC_WRITE_U32(domain1_pd+offset.target_velocity_60ff_0, 0x1000000); 331 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x000f); 332 | //printf("3.state = %x\n",temp[0]); 333 | } 334 | else if( (temp[0]&0x06f) == 0x027){//600 800 //temp[0]=0x237 335 | //EC_WRITE_S32(domain1_pd+interpolateddata,( value+=1000 )); 336 | EC_WRITE_U32(domain1_pd+offset.target_velocity_60ff_0, 0x1000000); 337 | 338 | //EC_WRITE_U16(domain1_pd+offset.target_position_607a_0,temp[1]+0x1000); /////// 339 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x001f); 340 | //printf("4.state = %x\n",temp[0]); 341 | } 342 | 343 | // write application time to master 344 | //clock_gettime(CLOCK_TO_USE, &time); 345 | //ecrt_master_application_time(master, TIMESPEC2NS(time)); 346 | 347 | if (sync_ref_counter) { 348 | sync_ref_counter--; 349 | } 350 | else { 351 | sync_ref_counter = 1; // sync every cycle 352 | 353 | clock_gettime(CLOCK_TO_USE,&time); //added by me 354 | 355 | //ecrt_master_sync_reference_clock(master); 356 | ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); 357 | } 358 | ecrt_master_sync_slave_clocks(master); 359 | 360 | //send process data 361 | ecrt_domain_queue(domain1); 362 | //ecrt_domain_queue(domain2); 363 | /*clock_gettime(CLOCK_TO_USE, &tx_time_start);*/ 364 | ecrt_master_send(master); 365 | /*clock_gettime(CLOCK_TO_USE, &tx_time_end); 366 | printf("tx:%ldns\n",(tx_time_end.tv_sec-tx_time_start.tv_sec)*NSEC_PER_SEC+tx_time_end.tv_nsec-tx_time_start.tv_nsec);*/ 367 | } 368 | } 369 | 370 | /****************************************************************************/ 371 | 372 | int main(int argc, char **argv) 373 | { 374 | ec_slave_config_t *sc; 375 | 376 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { 377 | perror("mlockall failed"); 378 | return -1; 379 | } 380 | 381 | master = ecrt_request_master(0); 382 | if (!master) 383 | return -1; 384 | 385 | domain1 = ecrt_master_create_domain(master); 386 | if (!domain1) 387 | return -1; 388 | 389 | //配置IS620 390 | if (!(sc = ecrt_master_slave_config( 391 | master, IS620NSlavePos, INOVANCE_IS620N))) { 392 | fprintf(stderr, "Failed to get slave1 configuration.\n"); 393 | return -1; 394 | } 395 | 396 | #if CONFIGURE_PDOS 397 | printf("Configuring PDOs...\n"); 398 | 399 | if (ecrt_slave_config_pdos(sc, EC_END, IS620N_syncs)) { 400 | fprintf(stderr, "Failed to configure 1st PDOs.\n"); 401 | return -1; 402 | } 403 | #endif 404 | /****************motor1馬達domain註冊到domain_process data *******************/ 405 | if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { 406 | fprintf(stderr, "1st motor RX_PDO entry registration failed!\n"); 407 | return -1; 408 | } 409 | //配置IS620结束 410 | 411 | //ecrt_slave_config_dc(sc, 0x0300, 4000000, 125000, 0, 0); 412 | //ecrt_slave_config_dc(sc, 0x0300, 4000000, 4000000/2, 0, 0); //orig 413 | ecrt_slave_config_dc(sc, 0x0300, 2000000, 2000000/2, 0, 0);//行 414 | //ecrt_slave_config_dc(sc, 0x0300, 1500000, 1500000/2, 0, 0);//不行 415 | 416 | printf("Activating master...\n"); 417 | 418 | if (ecrt_master_activate(master)) 419 | return -1; 420 | 421 | if (!(domain1_pd = ecrt_domain_data(domain1))) { 422 | return -1; 423 | } 424 | 425 | pid_t pid = getpid(); 426 | if (setpriority(PRIO_PROCESS, pid, -20)) 427 | fprintf(stderr, "Warning: Failed to set priority: %s\n", 428 | strerror(errno)); 429 | 430 | signal( SIGINT , endsignal ); //按CTRL+C 利用中斷結束程式 431 | printf("Starting cyclic function.\n"); 432 | cyclic_task(); 433 | ecrt_release_master(master); 434 | 435 | return 0; 436 | } 437 | //no more 438 | -------------------------------------------------------------------------------- /libipipe_64.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/liuzq71/igh-Master-samples/9e90a41911f9e434ae7a9a8f4640a8949fd9d6d3/libipipe_64.so -------------------------------------------------------------------------------- /main_dc_4slaves.c: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * $Id$ 4 | * 5 | * Copyright (C) 2007-2009 Florian Pose, Ingenieurgemeinschaft IgH 6 | * 7 | * This file is part of the IgH EtherCAT Master. 8 | * 9 | * The IgH EtherCAT Master is free software; you can redistribute it and/or 10 | * modify it under the terms of the GNU General Public License version 2, as 11 | * published by the Free Software Foundation. 12 | * 13 | * The IgH EtherCAT Master is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 16 | * Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License along 19 | * with the IgH EtherCAT Master; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | * --- 23 | * 24 | * The license mentioned above concerns the source code only. Using the 25 | * EtherCAT technology and brand is only permitted in compliance with the 26 | * industrial property and similar rights of Beckhoff Automation GmbH. 27 | * 28 | * $gcc -o main_dc_4slaves main_dc_4slaves.c -I/opt/etherlab/include -L/opt/etherlab/lib -lethercat -lrt 29 | ****************************************************************************/ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include /* sched_setscheduler() */ 43 | 44 | /****************************************************************************/ 45 | 46 | #include "ecrt.h" 47 | 48 | /****************************************************************************/ 49 | 50 | // Application parameters 51 | #define FREQUENCY 1000 52 | #define CLOCK_TO_USE CLOCK_MONOTONIC 53 | #define MEASURE_TIMING 54 | 55 | /****************************************************************************/ 56 | 57 | #define NSEC_PER_SEC (1000000000L) 58 | #define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) 59 | 60 | #define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \ 61 | (B).tv_nsec - (A).tv_nsec) 62 | 63 | #define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec) 64 | 65 | /****************************************************************************/ 66 | 67 | // EtherCAT 68 | static ec_master_t *master = NULL; 69 | static ec_master_state_t master_state = {}; 70 | 71 | static ec_domain_t *domain1 = NULL; 72 | static ec_domain_state_t domain1_state = {}; 73 | 74 | /****************************************************************************/ 75 | 76 | // process data 77 | static uint8_t *domain1_pd = NULL; 78 | 79 | #define BusCouplerPos 0, 0 80 | #define DigInSlavePos 0, 1 81 | #define DigOutSlavePos 0, 2 82 | 83 | #define Beckhoff_EK1100 0x00000002, 0x044c2c52 84 | #define Beckhoff_EL1008 0x00000002, 0x03f03052 85 | #define Beckhoff_EL2008 0x00000002, 0x07d83052 86 | 87 | // offsets for PDO entries 88 | static int off_dig_out; 89 | static int off_dig_in; 90 | //static int off_counter_in; 91 | //static int off_counter_out; 92 | 93 | static unsigned int counter = 0; 94 | static unsigned int blink = 0; 95 | static unsigned int sync_ref_counter = 0; 96 | const struct timespec cycletime = {0, PERIOD_NS}; 97 | 98 | /*****************************************************************************/ 99 | static unsigned int servooff=1; 100 | static signed long temp[8]={}; 101 | uint32_t target_position = 0xfffff; 102 | 103 | #define IS620NSlavePos 0,3 /*EtherCAT address on the bus*/ 104 | #define INOVANCE_IS620N 0x00100000, 0x000C0108 /*IS620N Vendor ID, product code*/ 105 | 106 | /*x86_64: char-1byte,int-4bytes,long-8bytes*/ 107 | struct IS620N_offset { 108 | //RxPDO 109 | unsigned int control_word_6040_0; //0x6040:控制字,subindex:0,bitlen:16 110 | unsigned int target_position_607a_0;//0x607A:目标位置,subindex:0,bitlen:32 111 | unsigned int touch_probe_60b8_0; //0x60B8:探针,subindex:0,bitlen:16 112 | unsigned int pysical_outputs_60fe_1;//0x60FE:pysical_outputs,subindex:1,bitlen:32 113 | unsigned int target_velocity_60ff_0;//0x60FF:target_velocity,subindex:0,bitlen:32 114 | unsigned int target_torque_6071_0;//0x6071:int target_torque,subindex:0,bitlen:16 115 | unsigned int modes_operation_6060_0;//0x6060:Modes of operation,subindex:0,bitlen:8 116 | unsigned int max_profile_velocity_607f_0;//0x607F:max_profile_velocity,subindex:0,bitlen:32 117 | unsigned int positive_torque_limit_value_60e0_0;//0x60E0:positive_torque_limit_value,subindex:0,bitlen:16 118 | unsigned int negative_torque_limit_value_60e1_0;//0x60E1:negaitive_torque_limit_value,subindex:0,bitlen:16 119 | unsigned int torque_offset_60b2_0;//0x60B2:torque offset,subindex:0,bitlen:16 120 | unsigned int max_torque_6072_0;//0x6072:max torque,subindex:0,bitlen:16 121 | 122 | //TxPDo 123 | unsigned int status_word_6041_0;//0x6041:status_word,subindex:0,bitlen:16 124 | unsigned int position_actual_value_6064_0;//0x6064:position_actual_value,subindex:0,bitlen:32 125 | unsigned int touch_probe_status_60b9_0;//0x60B9,subindex:0,bitlen:16 126 | unsigned int touch_probe_pos1_pos_value_60ba_0;//0x60BA,subindex:0,bitlen:32 127 | unsigned int touch_probe_pos2_pos_value_60bc_0;//0x60BC ,subindex:0,bitlen:32 128 | unsigned int error_code_603f_0;//0x603F,subindex:0,bitlen:16 129 | unsigned int digital_inputs_60fd_0;//0x60FD,subindex:0,bitlen:32 130 | unsigned int torque_actual_value_6077_0;//0x6077,subindex:0,bitlen:16 131 | unsigned int following_error_actual_value_60f4_0;//0x60F4,subindex:0,bitlen:32 132 | unsigned int modes_of_operation_display_6061_0;//0x6061,subindex:0,bitlen:8 133 | unsigned int velocity_actual_value_606c_0;//0x606C,subindex:0,bitlen:32 134 | }; 135 | 136 | static struct IS620N_offset offset; 137 | 138 | const static ec_pdo_entry_reg_t domain1_regs[] = { 139 | //RxPDO 140 | {IS620NSlavePos, INOVANCE_IS620N, 0x6040, 0, &offset.control_word_6040_0}, 141 | {IS620NSlavePos, INOVANCE_IS620N, 0x607A, 0, &offset.target_position_607a_0}, 142 | {IS620NSlavePos, INOVANCE_IS620N, 0x60FF, 0, &offset.target_velocity_60ff_0}, 143 | {IS620NSlavePos, INOVANCE_IS620N, 0x6071, 0, &offset.target_torque_6071_0}, 144 | {IS620NSlavePos, INOVANCE_IS620N, 0x6060, 0, &offset.modes_operation_6060_0}, 145 | {IS620NSlavePos, INOVANCE_IS620N, 0x60B8, 0, &offset.touch_probe_60b8_0}, 146 | {IS620NSlavePos, INOVANCE_IS620N, 0x607F, 0, &offset.max_profile_velocity_607f_0}, 147 | //TxPDO 148 | {IS620NSlavePos, INOVANCE_IS620N, 0x603F, 0, &offset.error_code_603f_0}, 149 | {IS620NSlavePos, INOVANCE_IS620N, 0x6041, 0, &offset.status_word_6041_0}, 150 | {IS620NSlavePos, INOVANCE_IS620N, 0x6064, 0, &offset.position_actual_value_6064_0}, 151 | {IS620NSlavePos, INOVANCE_IS620N, 0x6077, 0, &offset.torque_actual_value_6077_0}, 152 | {IS620NSlavePos, INOVANCE_IS620N, 0x6061, 0, &offset.modes_of_operation_display_6061_0}, 153 | {IS620NSlavePos, INOVANCE_IS620N, 0x60B9, 0, &offset.touch_probe_status_60b9_0}, 154 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BA, 0, &offset.touch_probe_pos1_pos_value_60ba_0}, 155 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BC, 0, &offset.touch_probe_pos2_pos_value_60bc_0}, 156 | {IS620NSlavePos, INOVANCE_IS620N, 0x60FD, 0, &offset.digital_inputs_60fd_0}, 157 | {} 158 | }; 159 | 160 | /*重新映射PDO才需要定义下面这3个数组,如果使用servo的固定映射则不需要*/ 161 | 162 | /*Config PDOs*/ 163 | static ec_pdo_entry_info_t IS620N_pdo_entries[] = { 164 | //RxPdo 0x1702 165 | {0x6040, 0x00, 16}, 166 | {0x607A, 0x00, 32}, 167 | {0x60FF, 0x00, 32}, 168 | {0x6071, 0x00, 16}, 169 | {0x6060, 0x00, 8}, 170 | {0x60B8, 0x00, 16}, 171 | {0x607F, 0x00, 32}, 172 | //TxPdo 0x1B02 173 | {0x603F, 0x00, 16}, 174 | {0x6041, 0x00, 16}, 175 | {0x6064, 0x00, 32}, 176 | {0x6077, 0x00, 16}, 177 | {0x6061, 0x00, 8}, 178 | {0x60B9, 0x00, 16}, 179 | {0x60BA, 0x00, 32}, 180 | {0x60BC, 0x00, 32}, 181 | {0x60FD, 0x00, 32}, 182 | }; 183 | 184 | static ec_pdo_info_t IS620N_pdos[] = { 185 | //RxPdo 186 | {0x1600, 7, IS620N_pdo_entries + 0 }, 187 | //TxPdo 188 | {0x1A00, 9, IS620N_pdo_entries + 7 } 189 | }; 190 | 191 | static ec_sync_info_t IS620N_syncs[] = { 192 | /*{ 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE }, 193 | { 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE },*/ 194 | { 2, EC_DIR_OUTPUT, 1, IS620N_pdos + 0/*,EC_WD_ENABLE*//*, EC_WD_DISABLE*/ }, 195 | { 3, EC_DIR_INPUT, 1, IS620N_pdos + 1/*,EC_WD_DISABLE*//*, EC_WD_DISABLE*/ }, 196 | { 0xFF} 197 | }; 198 | 199 | /*****************************************************************************/ 200 | 201 | struct timespec timespec_add(struct timespec time1, struct timespec time2) 202 | { 203 | struct timespec result; 204 | 205 | if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) { 206 | result.tv_sec = time1.tv_sec + time2.tv_sec + 1; 207 | result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC; 208 | } else { 209 | result.tv_sec = time1.tv_sec + time2.tv_sec; 210 | result.tv_nsec = time1.tv_nsec + time2.tv_nsec; 211 | } 212 | 213 | return result; 214 | } 215 | 216 | /*****************************************************************************/ 217 | 218 | void check_domain1_state(void) 219 | { 220 | ec_domain_state_t ds; 221 | 222 | ecrt_domain_state(domain1, &ds); 223 | 224 | if (ds.working_counter != domain1_state.working_counter) 225 | printf("Domain1: WC %u.\n", ds.working_counter); 226 | if (ds.wc_state != domain1_state.wc_state) 227 | printf("Domain1: State %u.\n", ds.wc_state); 228 | 229 | domain1_state = ds; 230 | } 231 | 232 | /*****************************************************************************/ 233 | 234 | void check_master_state(void) 235 | { 236 | ec_master_state_t ms; 237 | 238 | ecrt_master_state(master, &ms); 239 | 240 | if (ms.slaves_responding != master_state.slaves_responding) 241 | printf("%u slave(s).\n", ms.slaves_responding); 242 | if (ms.al_states != master_state.al_states) 243 | printf("AL states: 0x%02X.\n", ms.al_states); 244 | if (ms.link_up != master_state.link_up) 245 | printf("Link is %s.\n", ms.link_up ? "up" : "down"); 246 | 247 | master_state = ms; 248 | } 249 | 250 | /****************************************************************************/ 251 | 252 | void cyclic_task() 253 | { 254 | struct timespec wakeupTime, time; 255 | #ifdef MEASURE_TIMING 256 | struct timespec startTime, endTime, lastStartTime = {}; 257 | uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0, 258 | latency_min_ns = 0, latency_max_ns = 0, 259 | period_min_ns = 0, period_max_ns = 0, 260 | exec_min_ns = 0, exec_max_ns = 0; 261 | #endif 262 | 263 | // get current time 264 | clock_gettime(CLOCK_TO_USE, &wakeupTime); 265 | 266 | while(1) { 267 | wakeupTime = timespec_add(wakeupTime, cycletime); 268 | clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); 269 | 270 | // Write application time to master 271 | // 272 | // It is a good idea to use the target time (not the measured time) as 273 | // application time, because it is more stable. 274 | // 275 | ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime)); 276 | 277 | #ifdef MEASURE_TIMING 278 | clock_gettime(CLOCK_TO_USE, &startTime); 279 | latency_ns = DIFF_NS(wakeupTime, startTime); 280 | period_ns = DIFF_NS(lastStartTime, startTime); 281 | exec_ns = DIFF_NS(lastStartTime, endTime); 282 | lastStartTime = startTime; 283 | 284 | if (latency_ns > latency_max_ns) { 285 | latency_max_ns = latency_ns; 286 | } 287 | if (latency_ns < latency_min_ns) { 288 | latency_min_ns = latency_ns; 289 | } 290 | if (period_ns > period_max_ns) { 291 | period_max_ns = period_ns; 292 | } 293 | if (period_ns < period_min_ns) { 294 | period_min_ns = period_ns; 295 | } 296 | if (exec_ns > exec_max_ns) { 297 | exec_max_ns = exec_ns; 298 | } 299 | if (exec_ns < exec_min_ns) { 300 | exec_min_ns = exec_ns; 301 | } 302 | #endif 303 | 304 | // receive process data 305 | ecrt_master_receive(master); 306 | ecrt_domain_process(domain1); 307 | 308 | // check process data state (optional) 309 | check_domain1_state(); 310 | 311 | if (counter) { 312 | counter--; 313 | } else { // do this at 1 Hz 314 | counter = FREQUENCY; 315 | 316 | // check for master state (optional) 317 | check_master_state(); 318 | 319 | #ifdef MEASURE_TIMING 320 | // output timing stats 321 | printf("period %10u ... %10u\n", 322 | period_min_ns, period_max_ns); 323 | printf("exec %10u ... %10u\n", 324 | exec_min_ns, exec_max_ns); 325 | printf("latency %10u ... %10u\n", 326 | latency_min_ns, latency_max_ns); 327 | period_max_ns = 0; 328 | period_min_ns = 0xffffffff; 329 | exec_max_ns = 0; 330 | exec_min_ns = 0xffffffff; 331 | latency_max_ns = 0; 332 | latency_min_ns = 0xffffffff; 333 | #endif 334 | 335 | // calculate new process data 336 | blink = !blink; 337 | } 338 | 339 | // write process data 340 | EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x66 : 0x99); 341 | //EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02); 342 | 343 | //IS620n control 344 | temp[0]=EC_READ_U16(domain1_pd + offset.status_word_6041_0); 345 | temp[1]=EC_READ_U32(domain1_pd + offset.position_actual_value_6064_0); 346 | 347 | //printf("\r%6f \t ",((float)temp[1]/1000) ); 348 | 349 | //printf("after value = %x\n",temp[0]); 350 | // write process data 351 | /*if(servooff==1){//servo off 352 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0006 ); 353 | //deactive++; 354 | } 355 | else*/ if( (temp[0]&0x004f) == 0x0040 ){ 356 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0006 ); 357 | //printf("1.state = %x\n",temp[0]); 358 | } 359 | else if( (temp[0]&0x006f) == 0x0021){ 360 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0007 ); 361 | //printf("2.state = %x\n",temp[0]); 362 | } 363 | else if( (temp[0]&0x027f) == 0x0233){ 364 | //EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x000f); 365 | //EC_WRITE_S32(domain1_pd+interpolateddata, 0); 366 | //EC_WRITE_S32(domain1_pd+tar_velo, 0xfffff); 367 | //EC_WRITE_S32(domain1_pd+max_torq, 0xf00); 368 | 369 | EC_WRITE_S8(domain1_pd+offset.modes_operation_6060_0, 9);//csv mode 370 | //EC_WRITE_U16(domain1_pd+offset.target_position_607a_0,target_position); 371 | //EC_WRITE_U32(domain1_pd+offset.target_velocity_60ff_0, 0x1000000); 372 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x000f); 373 | //printf("3.state = %x\n",temp[0]); 374 | } 375 | else if( (temp[0]&0x027f) == 0x0237){//600 800 376 | //EC_WRITE_S32(domain1_pd+interpolateddata,( value+=1000 )); 377 | //EC_WRITE_U32(domain1_pd+offset.target_velocity_60ff_0, 0x1000000); 378 | 379 | //EC_WRITE_U16(domain1_pd+offset.target_position_607a_0,(target_position+=0xfffff)); /////// 380 | EC_WRITE_U32(domain1_pd+offset.target_velocity_60ff_0, 0x1000000); 381 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x001f); 382 | //printf("4.state = %x\n",temp[0]); 383 | } 384 | 385 | //IS620n control end 386 | 387 | if (sync_ref_counter) { 388 | sync_ref_counter--; 389 | } else { 390 | sync_ref_counter = 1; // sync every cycle 391 | 392 | clock_gettime(CLOCK_TO_USE, &time); 393 | ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); 394 | } 395 | ecrt_master_sync_slave_clocks(master); 396 | 397 | // send process data 398 | ecrt_domain_queue(domain1); 399 | ecrt_master_send(master); 400 | 401 | #ifdef MEASURE_TIMING 402 | clock_gettime(CLOCK_TO_USE, &endTime); 403 | #endif 404 | } 405 | } 406 | 407 | /****************************************************************************/ 408 | 409 | int main(int argc, char **argv) 410 | { 411 | ec_slave_config_t *sc; 412 | 413 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { 414 | perror("mlockall failed"); 415 | return -1; 416 | } 417 | 418 | master = ecrt_request_master(0); 419 | if (!master) 420 | return -1; 421 | 422 | domain1 = ecrt_master_create_domain(master); 423 | if (!domain1) 424 | return -1; 425 | 426 | // Create configuration for bus coupler 427 | sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100); 428 | if (!sc) 429 | return -1; 430 | 431 | if (!(sc = ecrt_master_slave_config(master, 432 | DigOutSlavePos, Beckhoff_EL2008))) { 433 | fprintf(stderr, "Failed to get slave configuration.\n"); 434 | return -1; 435 | } 436 | 437 | off_dig_out = ecrt_slave_config_reg_pdo_entry(sc, 438 | 0x7000, 1, domain1, NULL); 439 | if (off_dig_out < 0) 440 | return -1; 441 | 442 | if (!(sc = ecrt_master_slave_config(master, 443 | DigInSlavePos, Beckhoff_EL1008))) { 444 | fprintf(stderr, "Failed to get slave configuration.\n"); 445 | return -1; 446 | } 447 | 448 | off_dig_in = ecrt_slave_config_reg_pdo_entry(sc, 449 | 0x6000, 1, domain1, NULL); 450 | if (off_dig_in < 0) 451 | return -1; 452 | 453 | /*off_counter_out = ecrt_slave_config_reg_pdo_entry(sc, 454 | 0x7020, 1, domain1, NULL); 455 | if (off_counter_out < 0) 456 | return -1;*/ 457 | 458 | //配置IS620 459 | if (!(sc = ecrt_master_slave_config( 460 | master, IS620NSlavePos, INOVANCE_IS620N))) { 461 | fprintf(stderr, "Failed to get slave3 configuration.\n"); 462 | return -1; 463 | } 464 | 465 | //printf("Configuring PDOs...\n"); 466 | 467 | if (ecrt_slave_config_pdos(sc, EC_END, IS620N_syncs)) { 468 | fprintf(stderr, "Failed to configure 1st PDOs.\n"); 469 | return -1; 470 | } 471 | 472 | /****************motor1馬達domain註冊到domain_process data *******************/ 473 | if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { 474 | fprintf(stderr, "1st motor RX_PDO entry registration failed!\n"); 475 | return -1; 476 | } 477 | //配置IS620结束 478 | 479 | // configure SYNC signals for this slave 480 | //ecrt_slave_config_dc(sc, 0x0700, PERIOD_NS, 4400000, 0, 0); 481 | ecrt_slave_config_dc(sc,0x0300, 4000000, 125000,0,0); //added by liuzq 482 | 483 | printf("Activating master...\n"); 484 | if (ecrt_master_activate(master)) 485 | return -1; 486 | 487 | if (!(domain1_pd = ecrt_domain_data(domain1))) { 488 | return -1; 489 | } 490 | 491 | /* Set priority */ 492 | 493 | struct sched_param param = {}; 494 | param.sched_priority = sched_get_priority_max(SCHED_FIFO); 495 | 496 | printf("Using priority %i.", param.sched_priority); 497 | if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { 498 | perror("sched_setscheduler failed"); 499 | } 500 | 501 | printf("Starting cyclic function.\n"); 502 | cyclic_task(); 503 | 504 | return 0; 505 | } 506 | 507 | /****************************************************************************/ 508 | -------------------------------------------------------------------------------- /main_dc_el2008.c: -------------------------------------------------------------------------------- 1 | /***************************************************************************** 2 | * 3 | * $Id$ 4 | * 5 | * Copyright (C) 2007-2009 Florian Pose, Ingenieurgemeinschaft IgH 6 | * 7 | * This file is part of the IgH EtherCAT Master. 8 | * 9 | * The IgH EtherCAT Master is free software; you can redistribute it and/or 10 | * modify it under the terms of the GNU General Public License version 2, as 11 | * published by the Free Software Foundation. 12 | * 13 | * The IgH EtherCAT Master is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 16 | * Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License along 19 | * with the IgH EtherCAT Master; if not, write to the Free Software 20 | * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 | * 22 | * --- 23 | * 24 | * The license mentioned above concerns the source code only. Using the 25 | * EtherCAT technology and brand is only permitted in compliance with the 26 | * industrial property and similar rights of Beckhoff Automation GmbH. 27 | * 28 | * $gcc -o main_dc_el2008 main_dc_el2008.c -I/opt/etherlab/include -I/opt/etherlab/lib -lethercat 29 | ****************************************************************************/ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include /* sched_setscheduler() */ 43 | 44 | /****************************************************************************/ 45 | 46 | #include "ecrt.h" 47 | 48 | /****************************************************************************/ 49 | 50 | // Application parameters 51 | #define FREQUENCY 1000 52 | #define CLOCK_TO_USE CLOCK_MONOTONIC 53 | #define MEASURE_TIMING 54 | 55 | /****************************************************************************/ 56 | 57 | #define NSEC_PER_SEC (1000000000L) 58 | #define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) 59 | 60 | #define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \ 61 | (B).tv_nsec - (A).tv_nsec) 62 | 63 | #define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec) 64 | 65 | /****************************************************************************/ 66 | 67 | // EtherCAT 68 | static ec_master_t *master = NULL; 69 | static ec_master_state_t master_state = {}; 70 | 71 | static ec_domain_t *domain1 = NULL; 72 | static ec_domain_state_t domain1_state = {}; 73 | 74 | /****************************************************************************/ 75 | 76 | // process data 77 | static uint8_t *domain1_pd = NULL; 78 | 79 | #define BusCouplerPos 0, 0 80 | #define DigInSlavePos 0, 1 81 | #define DigOutSlavePos 0, 2 82 | 83 | #define Beckhoff_EK1100 0x00000002, 0x044c2c52 84 | #define Beckhoff_EL1008 0x00000002, 0x03f03052 85 | #define Beckhoff_EL2008 0x00000002, 0x07d83052 86 | 87 | // offsets for PDO entries 88 | static int off_dig_out; 89 | static int off_dig_in; 90 | //static int off_counter_in; 91 | //static int off_counter_out; 92 | 93 | static unsigned int counter = 0; 94 | static unsigned int blink = 0; 95 | static unsigned int sync_ref_counter = 0; 96 | const struct timespec cycletime = {0, PERIOD_NS}; 97 | 98 | /*****************************************************************************/ 99 | 100 | struct timespec timespec_add(struct timespec time1, struct timespec time2) 101 | { 102 | struct timespec result; 103 | 104 | if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) { 105 | result.tv_sec = time1.tv_sec + time2.tv_sec + 1; 106 | result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC; 107 | } else { 108 | result.tv_sec = time1.tv_sec + time2.tv_sec; 109 | result.tv_nsec = time1.tv_nsec + time2.tv_nsec; 110 | } 111 | 112 | return result; 113 | } 114 | 115 | /*****************************************************************************/ 116 | 117 | void check_domain1_state(void) 118 | { 119 | ec_domain_state_t ds; 120 | 121 | ecrt_domain_state(domain1, &ds); 122 | 123 | if (ds.working_counter != domain1_state.working_counter) 124 | printf("Domain1: WC %u.\n", ds.working_counter); 125 | if (ds.wc_state != domain1_state.wc_state) 126 | printf("Domain1: State %u.\n", ds.wc_state); 127 | 128 | domain1_state = ds; 129 | } 130 | 131 | /*****************************************************************************/ 132 | 133 | void check_master_state(void) 134 | { 135 | ec_master_state_t ms; 136 | 137 | ecrt_master_state(master, &ms); 138 | 139 | if (ms.slaves_responding != master_state.slaves_responding) 140 | printf("%u slave(s).\n", ms.slaves_responding); 141 | if (ms.al_states != master_state.al_states) 142 | printf("AL states: 0x%02X.\n", ms.al_states); 143 | if (ms.link_up != master_state.link_up) 144 | printf("Link is %s.\n", ms.link_up ? "up" : "down"); 145 | 146 | master_state = ms; 147 | } 148 | 149 | /****************************************************************************/ 150 | 151 | void cyclic_task() 152 | { 153 | struct timespec wakeupTime, time; 154 | #ifdef MEASURE_TIMING 155 | struct timespec startTime, endTime, lastStartTime = {}; 156 | uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0, 157 | latency_min_ns = 0, latency_max_ns = 0, 158 | period_min_ns = 0, period_max_ns = 0, 159 | exec_min_ns = 0, exec_max_ns = 0; 160 | #endif 161 | 162 | // get current time 163 | clock_gettime(CLOCK_TO_USE, &wakeupTime); 164 | 165 | while(1) { 166 | wakeupTime = timespec_add(wakeupTime, cycletime); 167 | clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); 168 | 169 | // Write application time to master 170 | // 171 | // It is a good idea to use the target time (not the measured time) as 172 | // application time, because it is more stable. 173 | // 174 | ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime)); 175 | 176 | #ifdef MEASURE_TIMING 177 | clock_gettime(CLOCK_TO_USE, &startTime); 178 | latency_ns = DIFF_NS(wakeupTime, startTime); 179 | period_ns = DIFF_NS(lastStartTime, startTime); 180 | exec_ns = DIFF_NS(lastStartTime, endTime); 181 | lastStartTime = startTime; 182 | 183 | if (latency_ns > latency_max_ns) { 184 | latency_max_ns = latency_ns; 185 | } 186 | if (latency_ns < latency_min_ns) { 187 | latency_min_ns = latency_ns; 188 | } 189 | if (period_ns > period_max_ns) { 190 | period_max_ns = period_ns; 191 | } 192 | if (period_ns < period_min_ns) { 193 | period_min_ns = period_ns; 194 | } 195 | if (exec_ns > exec_max_ns) { 196 | exec_max_ns = exec_ns; 197 | } 198 | if (exec_ns < exec_min_ns) { 199 | exec_min_ns = exec_ns; 200 | } 201 | #endif 202 | 203 | // receive process data 204 | ecrt_master_receive(master); 205 | ecrt_domain_process(domain1); 206 | 207 | // check process data state (optional) 208 | check_domain1_state(); 209 | 210 | if (counter) { 211 | counter--; 212 | } else { // do this at 1 Hz 213 | counter = FREQUENCY; 214 | 215 | // check for master state (optional) 216 | check_master_state(); 217 | 218 | #ifdef MEASURE_TIMING 219 | // output timing stats 220 | printf("period %10u ... %10u\n", 221 | period_min_ns, period_max_ns); 222 | printf("exec %10u ... %10u\n", 223 | exec_min_ns, exec_max_ns); 224 | printf("latency %10u ... %10u\n", 225 | latency_min_ns, latency_max_ns); 226 | period_max_ns = 0; 227 | period_min_ns = 0xffffffff; 228 | exec_max_ns = 0; 229 | exec_min_ns = 0xffffffff; 230 | latency_max_ns = 0; 231 | latency_min_ns = 0xffffffff; 232 | #endif 233 | 234 | // calculate new process data 235 | blink = !blink; 236 | } 237 | 238 | // write process data 239 | EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x66 : 0x99); 240 | //EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02); 241 | 242 | if (sync_ref_counter) { 243 | sync_ref_counter--; 244 | } else { 245 | sync_ref_counter = 1; // sync every cycle 246 | 247 | clock_gettime(CLOCK_TO_USE, &time); 248 | ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); 249 | } 250 | ecrt_master_sync_slave_clocks(master); 251 | 252 | // send process data 253 | ecrt_domain_queue(domain1); 254 | ecrt_master_send(master); 255 | 256 | #ifdef MEASURE_TIMING 257 | clock_gettime(CLOCK_TO_USE, &endTime); 258 | #endif 259 | } 260 | } 261 | 262 | /****************************************************************************/ 263 | 264 | int main(int argc, char **argv) 265 | { 266 | ec_slave_config_t *sc; 267 | 268 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { 269 | perror("mlockall failed"); 270 | return -1; 271 | } 272 | 273 | master = ecrt_request_master(0); 274 | if (!master) 275 | return -1; 276 | 277 | domain1 = ecrt_master_create_domain(master); 278 | if (!domain1) 279 | return -1; 280 | 281 | // Create configuration for bus coupler 282 | sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100); 283 | if (!sc) 284 | return -1; 285 | 286 | if (!(sc = ecrt_master_slave_config(master, 287 | DigOutSlavePos, Beckhoff_EL2008))) { 288 | fprintf(stderr, "Failed to get slave configuration.\n"); 289 | return -1; 290 | } 291 | 292 | off_dig_out = ecrt_slave_config_reg_pdo_entry(sc, 293 | 0x7000, 1, domain1, NULL); 294 | if (off_dig_out < 0) 295 | return -1; 296 | 297 | if (!(sc = ecrt_master_slave_config(master, 298 | DigInSlavePos, Beckhoff_EL1008))) { 299 | fprintf(stderr, "Failed to get slave configuration.\n"); 300 | return -1; 301 | } 302 | 303 | off_dig_in = ecrt_slave_config_reg_pdo_entry(sc, 304 | 0x6000, 1, domain1, NULL); 305 | if (off_dig_in < 0) 306 | return -1; 307 | 308 | /*off_counter_out = ecrt_slave_config_reg_pdo_entry(sc, 309 | 0x7020, 1, domain1, NULL); 310 | if (off_counter_out < 0) 311 | return -1;*/ 312 | 313 | // configure SYNC signals for this slave 314 | ecrt_slave_config_dc(sc, 0x0700, PERIOD_NS, 4400000, 0, 0); 315 | 316 | printf("Activating master...\n"); 317 | if (ecrt_master_activate(master)) 318 | return -1; 319 | 320 | if (!(domain1_pd = ecrt_domain_data(domain1))) { 321 | return -1; 322 | } 323 | 324 | /* Set priority */ 325 | 326 | struct sched_param param = {}; 327 | param.sched_priority = sched_get_priority_max(SCHED_FIFO); 328 | 329 | printf("Using priority %i.", param.sched_priority); 330 | if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { 331 | perror("sched_setscheduler failed"); 332 | } 333 | 334 | printf("Starting cyclic function.\n"); 335 | cyclic_task(); 336 | 337 | return 0; 338 | } 339 | 340 | /****************************************************************************/ 341 | -------------------------------------------------------------------------------- /rtai_rtdm_dc/is620n_dc_csp.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * $Id$ 4 | * 5 | * Copyright (C) 2011 IgH Andreas Stewering-Bone 6 | * 2012 Florian Pose 7 | * 8 | * This file is part of the IgH EtherCAT master 9 | * 10 | * The IgH EtherCAT Master is free software; you can redistribute it and/or 11 | * modify it under the terms of the GNU General Public License version 2, as 12 | * published by the Free Software Foundation. 13 | * 14 | * The IgH EtherCAT master is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 17 | * Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License along 20 | * with the IgH EtherCAT master. If not, see . 21 | * 22 | * --- 23 | * 24 | * The license mentioned above concerns the source code only. Using the 25 | * EtherCAT technology and brand is only permitted in compliance with the 26 | * industrial property and similar rights of Beckhoff Automation GmbH. 27 | * 本例演示正转3圈反转3圈,IS620N 20位增量型转1圈绝对位置值增加1048576 28 | * $gcc -o is620n_dc_csp is620n_dc_csp.c -I/opt/etherlab/include -I/opt/rtai-5.1_v4.9.80/include -L/opt/rtai-5.1_v4.9.80/lib -L/opt/etherlab/lib -llxrt -lrtdm -lethercat -lm -lpthread -Wl,--rpath=/opt/rtai-5.1_v4.9.80/lib -Wl,--rpath=/opt/etherlab/lib 29 | *****************************************************************************/ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include "ecrt.h" 41 | 42 | #define pi 3.14159265 43 | #define N 1500 44 | #define M 3 45 | #define rt_printf(X, Y) 46 | 47 | //#define CLOCK_TO_USE CLOCK_MONOTONIC //CLOCK_REALTIME 48 | #define NSEC_PER_SEC 1000000000 49 | 50 | RT_TASK *task; 51 | 52 | static unsigned int cycle_ns = 1000000; /* 1 ms */ 53 | 54 | static int run = 1; 55 | 56 | /****************************************************************************/ 57 | 58 | // EtherCAT 59 | static ec_master_t *master = NULL; 60 | static ec_master_state_t master_state = {}; 61 | 62 | static ec_domain_t *domain1 = NULL; 63 | static ec_domain_state_t domain1_state = {}; 64 | 65 | static uint8_t *domain1_pd = NULL; 66 | 67 | static ec_slave_config_t *sc_dig_out_01 = NULL; 68 | 69 | /****************************************************************************/ 70 | 71 | // EtherCAT distributed clock variables 72 | 73 | #define DC_FILTER_CNT 1024 74 | #define SYNC_MASTER_TO_REF 1 75 | 76 | static uint64_t dc_start_time_ns = 0LL; 77 | static uint64_t dc_time_ns = 0; 78 | #if SYNC_MASTER_TO_REF 79 | static uint8_t dc_started = 0; 80 | static int32_t dc_diff_ns = 0; 81 | static int32_t prev_dc_diff_ns = 0; 82 | static int64_t dc_diff_total_ns = 0LL; 83 | static int64_t dc_delta_total_ns = 0LL; 84 | static int dc_filter_idx = 0; 85 | static int64_t dc_adjust_ns; 86 | #endif 87 | static int64_t system_time_base = 0LL; 88 | static uint64_t wakeup_time = 0LL; 89 | static uint64_t overruns = 0LL; 90 | 91 | /****************************************************************************/ 92 | 93 | // process data 94 | 95 | #define BusCoupler01_Pos 0, 0 96 | #define DigOutSlave01_Pos 0, 1 97 | 98 | #define Beckhoff_EK1100 0x00000002, 0x044c2c52 99 | //#define Beckhoff_EL2004 0x00000002, 0x07d43052 100 | #define Beckhoff_EL2008 0x00000002, 0x07d83052 101 | 102 | // offsets for PDO entries 103 | static unsigned int off_dig_out0 = 0; 104 | 105 | // process data 106 | #if 0 107 | const static ec_pdo_entry_reg_t domain1_regs[] = { 108 | //{DigOutSlave01_Pos, Beckhoff_EL2004, 0x7000, 0x01, &off_dig_out0, NULL}, 109 | {DigOutSlave01_Pos, Beckhoff_EL2008, 0x7000, 0x01, &off_dig_out0, NULL}, 110 | {} 111 | }; 112 | 113 | /****************************************************************************/ 114 | 115 | /* Slave 1, "EL2004" 116 | * Vendor ID: 0x00000002 117 | * Product code: 0x07d43052 118 | * Revision number: 0x00100000 119 | */ 120 | 121 | /* Slave 1, "EL2008" 122 | * Vendor ID: 0x00000002 123 | * Product code: 0x07d83052 124 | * Revision number: 0x00100000 125 | */ 126 | 127 | ec_pdo_entry_info_t slave_1_pdo_entries[] = { 128 | {0x7000, 0x01, 1}, /* Output */ 129 | {0x7010, 0x01, 1}, /* Output */ 130 | {0x7020, 0x01, 1}, /* Output */ 131 | {0x7030, 0x01, 1}, /* Output */ 132 | }; 133 | 134 | ec_pdo_info_t slave_1_pdos[] = { 135 | {0x1600, 1, slave_1_pdo_entries + 0}, /* Channel 1 */ 136 | {0x1601, 1, slave_1_pdo_entries + 1}, /* Channel 2 */ 137 | {0x1602, 1, slave_1_pdo_entries + 2}, /* Channel 3 */ 138 | {0x1603, 1, slave_1_pdo_entries + 3}, /* Channel 4 */ 139 | }; 140 | 141 | ec_sync_info_t slave_1_syncs[] = { 142 | {0, EC_DIR_OUTPUT, 4, slave_1_pdos + 0, EC_WD_ENABLE}, 143 | {0xff} 144 | }; 145 | #endif 146 | 147 | #define IS620NSlavePos 0,0 /*EtherCAT address on the bus*/ 148 | #define INOVANCE_IS620N 0x00100000, 0x000C0108 /*IS620N Vendor ID, product code*/ 149 | 150 | /*x86_64: char-1byte,int-4bytes,long-8bytes*/ 151 | struct IS620N_offset { 152 | //RxPDO 153 | unsigned int control_word_6040_0; //0x6040:控制字,subindex:0,bitlen:16 154 | unsigned int target_position_607a_0;//0x607A:目标位置,subindex:0,bitlen:32 155 | unsigned int touch_probe_60b8_0; //0x60B8:探针,subindex:0,bitlen:16 156 | unsigned int pysical_outputs_60fe_1;//0x60FE:pysical_outputs,subindex:1,bitlen:32 157 | unsigned int target_velocity_60ff_0;//0x60FF:target_velocity,subindex:0,bitlen:32 158 | unsigned int target_torque_6071_0;//0x6071:int target_torque,subindex:0,bitlen:16 159 | unsigned int modes_operation_6060_0;//0x6060:Modes of operation,subindex:0,bitlen:8 160 | unsigned int max_profile_velocity_607f_0;//0x607F:max_profile_velocity,subindex:0,bitlen:32 161 | unsigned int positive_torque_limit_value_60e0_0;//0x60E0:positive_torque_limit_value,subindex:0,bitlen:16 162 | unsigned int negative_torque_limit_value_60e1_0;//0x60E1:negaitive_torque_limit_value,subindex:0,bitlen:16 163 | unsigned int torque_offset_60b2_0;//0x60B2:torque offset,subindex:0,bitlen:16 164 | unsigned int max_torque_6072_0;//0x6072:max torque,subindex:0,bitlen:16 165 | 166 | //TxPDo 167 | unsigned int status_word_6041_0;//0x6041:status_word,subindex:0,bitlen:16 168 | unsigned int position_actual_value_6064_0;//0x6064:position_actual_value,subindex:0,bitlen:32 169 | unsigned int touch_probe_status_60b9_0;//0x60B9,subindex:0,bitlen:16 170 | unsigned int touch_probe_pos1_pos_value_60ba_0;//0x60BA,subindex:0,bitlen:32 171 | unsigned int touch_probe_pos2_pos_value_60bc_0;//0x60BC ,subindex:0,bitlen:32 172 | unsigned int error_code_603f_0;//0x603F,subindex:0,bitlen:16 173 | unsigned int digital_inputs_60fd_0;//0x60FD,subindex:0,bitlen:32 174 | unsigned int torque_actual_value_6077_0;//0x6077,subindex:0,bitlen:16 175 | unsigned int following_error_actual_value_60f4_0;//0x60F4,subindex:0,bitlen:32 176 | unsigned int modes_of_operation_display_6061_0;//0x6061,subindex:0,bitlen:8 177 | unsigned int velocity_actual_value_606c_0;//0x606C,subindex:0,bitlen:32 178 | }; 179 | 180 | static struct IS620N_offset offset; 181 | 182 | const static ec_pdo_entry_reg_t domain1_regs[] = { 183 | //RxPDO 184 | {IS620NSlavePos, INOVANCE_IS620N, 0x6040, 0, &offset.control_word_6040_0}, 185 | {IS620NSlavePos, INOVANCE_IS620N, 0x607A, 0, &offset.target_position_607a_0}, 186 | //{IS620NSlavePos, INOVANCE_IS620N, 0x60FF, 0, &offset.target_velocity_60ff_0}, 187 | //{IS620NSlavePos, INOVANCE_IS620N, 0x6071, 0, &offset.target_torque_6071_0}, 188 | {IS620NSlavePos, INOVANCE_IS620N, 0x6060, 0, &offset.modes_operation_6060_0}, 189 | //{IS620NSlavePos, INOVANCE_IS620N, 0x60B8, 0, &offset.touch_probe_60b8_0}, 190 | //{IS620NSlavePos, INOVANCE_IS620N, 0x607F, 0, &offset.max_profile_velocity_607f_0}, 191 | //TxPDO 192 | {IS620NSlavePos, INOVANCE_IS620N, 0x603F, 0, &offset.error_code_603f_0}, 193 | {IS620NSlavePos, INOVANCE_IS620N, 0x6041, 0, &offset.status_word_6041_0}, 194 | {IS620NSlavePos, INOVANCE_IS620N, 0x6064, 0, &offset.position_actual_value_6064_0}, 195 | {IS620NSlavePos, INOVANCE_IS620N, 0x6077, 0, &offset.torque_actual_value_6077_0}, 196 | {IS620NSlavePos, INOVANCE_IS620N, 0x6061, 0, &offset.modes_of_operation_display_6061_0}, 197 | {IS620NSlavePos, INOVANCE_IS620N, 0x60B9, 0, &offset.touch_probe_status_60b9_0}, 198 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BA, 0, &offset.touch_probe_pos1_pos_value_60ba_0}, 199 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BC, 0, &offset.touch_probe_pos2_pos_value_60bc_0}, 200 | {IS620NSlavePos, INOVANCE_IS620N, 0x60FD, 0, &offset.digital_inputs_60fd_0}, 201 | {} 202 | }; 203 | 204 | /*重新映射PDO才需要定义下面这3个数组,如果使用servo的固定映射则不需要*/ 205 | 206 | /*Config PDOs*/ 207 | static ec_pdo_entry_info_t IS620N_pdo_entries[] = { 208 | //RxPdo 0x1702 209 | {0x6040, 0x00, 16}, 210 | {0x607A, 0x00, 32}, 211 | //{0x60FF, 0x00, 32}, 212 | //{0x6071, 0x00, 16}, 213 | {0x6060, 0x00, 8}, 214 | //{0x60B8, 0x00, 16}, 215 | //{0x607F, 0x00, 32}, 216 | //TxPdo 0x1B02 217 | {0x603F, 0x00, 16}, 218 | {0x6041, 0x00, 16}, 219 | {0x6064, 0x00, 32}, 220 | {0x6077, 0x00, 16}, 221 | {0x6061, 0x00, 8}, 222 | {0x60B9, 0x00, 16}, 223 | {0x60BA, 0x00, 32}, 224 | {0x60BC, 0x00, 32}, 225 | {0x60FD, 0x00, 32}, 226 | }; 227 | 228 | static ec_pdo_info_t IS620N_pdos[] = { 229 | //RxPdo 230 | {0x1600, 3, IS620N_pdo_entries + 0 }, 231 | //TxPdo 232 | {0x1A00, 9, IS620N_pdo_entries + 3 } 233 | }; 234 | 235 | static ec_sync_info_t IS620N_syncs[] = { 236 | /*{ 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE }, 237 | { 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE },*/ 238 | { 2, EC_DIR_OUTPUT, 1, IS620N_pdos + 0/*,EC_WD_ENABLE*//*, EC_WD_DISABLE*/ }, 239 | { 3, EC_DIR_INPUT, 1, IS620N_pdos + 1/*,EC_WD_DISABLE*//*, EC_WD_DISABLE*/ }, 240 | { 0xFF} 241 | }; 242 | 243 | /***************************************************************************** 244 | * Realtime task 245 | ****************************************************************************/ 246 | 247 | /** Get the time in ns for the current cpu, adjusted by system_time_base. 248 | * 249 | * \attention Rather than calling rt_get_time_ns() directly, all application 250 | * time calls should use this method instead. 251 | * 252 | * \ret The time in ns. 253 | */ 254 | uint64_t system_time_ns(void) 255 | { 256 | RTIME time = rt_get_time_ns(); 257 | 258 | if (system_time_base > time) { 259 | rt_printk("%s() error: system_time_base greater than" 260 | " system time (system_time_base: %lld, time: %llu\n", 261 | __func__, system_time_base, time); 262 | return time; 263 | } 264 | else { 265 | return time - system_time_base; 266 | } 267 | } 268 | 269 | /****************************************************************************/ 270 | 271 | /** Convert system time to RTAI time in counts (via the system_time_base). 272 | */ 273 | RTIME system2count( 274 | uint64_t time 275 | ) 276 | { 277 | RTIME ret; 278 | 279 | if ((system_time_base < 0) && 280 | ((uint64_t) (-system_time_base) > time)) { 281 | rt_printk("%s() error: system_time_base less than" 282 | " system time (system_time_base: %lld, time: %llu\n", 283 | __func__, system_time_base, time); 284 | ret = time; 285 | } 286 | else { 287 | ret = time + system_time_base; 288 | } 289 | 290 | return nano2count(ret); 291 | } 292 | 293 | /*****************************************************************************/ 294 | 295 | /** Synchronise the distributed clocks 296 | */ 297 | void sync_distributed_clocks(void) 298 | { 299 | #if SYNC_MASTER_TO_REF 300 | uint32_t ref_time = 0; 301 | uint64_t prev_app_time = dc_time_ns; 302 | #endif 303 | 304 | dc_time_ns = system_time_ns(); 305 | 306 | #if SYNC_MASTER_TO_REF 307 | // get reference clock time to synchronize master cycle 308 | ecrt_master_reference_clock_time(master, &ref_time); 309 | dc_diff_ns = (uint32_t) prev_app_time - ref_time; 310 | #else 311 | // sync reference clock to master 312 | ecrt_master_sync_reference_clock_to(master, dc_time_ns); 313 | #endif 314 | 315 | // call to sync slaves to ref slave 316 | ecrt_master_sync_slave_clocks(master); 317 | } 318 | 319 | /*****************************************************************************/ 320 | 321 | /** Return the sign of a number 322 | * 323 | * ie -1 for -ve value, 0 for 0, +1 for +ve value 324 | * 325 | * \retval the sign of the value 326 | */ 327 | #define sign(val) \ 328 | ({ typeof (val) _val = (val); \ 329 | ((_val > 0) - (_val < 0)); }) 330 | 331 | /*****************************************************************************/ 332 | 333 | /** Update the master time based on ref slaves time diff 334 | * 335 | * called after the ethercat frame is sent to avoid time jitter in 336 | * sync_distributed_clocks() 337 | */ 338 | void update_master_clock(void) 339 | { 340 | #if SYNC_MASTER_TO_REF 341 | // calc drift (via un-normalised time diff) 342 | int32_t delta = dc_diff_ns - prev_dc_diff_ns; 343 | prev_dc_diff_ns = dc_diff_ns; 344 | 345 | // normalise the time diff 346 | dc_diff_ns = 347 | ((dc_diff_ns + (cycle_ns / 2)) % cycle_ns) - (cycle_ns / 2); 348 | 349 | // only update if primary master 350 | if (dc_started) { 351 | 352 | // add to totals 353 | dc_diff_total_ns += dc_diff_ns; 354 | dc_delta_total_ns += delta; 355 | dc_filter_idx++; 356 | 357 | if (dc_filter_idx >= DC_FILTER_CNT) { 358 | // add rounded delta average 359 | dc_adjust_ns += 360 | ((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT); 361 | 362 | // and add adjustment for general diff (to pull in drift) 363 | dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT); 364 | 365 | // limit crazy numbers (0.1% of std cycle time) 366 | if (dc_adjust_ns < -1000) { 367 | dc_adjust_ns = -1000; 368 | } 369 | if (dc_adjust_ns > 1000) { 370 | dc_adjust_ns = 1000; 371 | } 372 | 373 | // reset 374 | dc_diff_total_ns = 0LL; 375 | dc_delta_total_ns = 0LL; 376 | dc_filter_idx = 0; 377 | } 378 | 379 | // add cycles adjustment to time base (including a spot adjustment) 380 | system_time_base += dc_adjust_ns + sign(dc_diff_ns); 381 | } 382 | else { 383 | dc_started = (dc_diff_ns != 0); 384 | 385 | if (dc_started) { 386 | // output first diff 387 | rt_printk("First master diff: %d.\n", dc_diff_ns); 388 | 389 | // record the time of this initial cycle 390 | dc_start_time_ns = dc_time_ns; 391 | } 392 | } 393 | #endif 394 | } 395 | 396 | /****************************************************************************/ 397 | 398 | void rt_check_domain_state(void) 399 | { 400 | ec_domain_state_t ds = {}; 401 | 402 | ecrt_domain_state(domain1, &ds); 403 | 404 | if (ds.working_counter != domain1_state.working_counter) { 405 | rt_printf("Domain1: WC %u.\n", ds.working_counter); 406 | } 407 | 408 | if (ds.wc_state != domain1_state.wc_state) { 409 | rt_printf("Domain1: State %u.\n", ds.wc_state); 410 | } 411 | 412 | domain1_state = ds; 413 | } 414 | 415 | /****************************************************************************/ 416 | 417 | void rt_check_master_state(void) 418 | { 419 | ec_master_state_t ms; 420 | 421 | ecrt_master_state(master, &ms); 422 | 423 | if (ms.slaves_responding != master_state.slaves_responding) { 424 | rt_printf("%u slave(s).\n", ms.slaves_responding); 425 | } 426 | 427 | if (ms.al_states != master_state.al_states) { 428 | rt_printf("AL states: 0x%02X.\n", ms.al_states); 429 | } 430 | 431 | if (ms.link_up != master_state.link_up) { 432 | rt_printf("Link is %s.\n", ms.link_up ? "up" : "down"); 433 | } 434 | 435 | master_state = ms; 436 | } 437 | 438 | /****************************************************************************/ 439 | 440 | /** Wait for the next period 441 | */ 442 | 443 | void wait_period(void) 444 | { 445 | while (1) 446 | { 447 | RTIME wakeup_count = system2count(wakeup_time); 448 | RTIME current_count = rt_get_time(); 449 | 450 | if ((wakeup_count < current_count) 451 | || (wakeup_count > current_count + (50 * cycle_ns))) { 452 | rt_printk("%s(): unexpected wake time!\n", __func__); 453 | } 454 | 455 | switch (rt_sleep_until(wakeup_count)) { 456 | case RTE_UNBLKD: 457 | rt_printk("rt_sleep_until(): RTE_UNBLKD\n"); 458 | continue; 459 | 460 | case RTE_TMROVRN: 461 | rt_printk("rt_sleep_until(): RTE_TMROVRN\n"); 462 | overruns++; 463 | 464 | if (overruns % 100 == 0) { 465 | // in case wake time is broken ensure other processes get 466 | // some time slice (and error messages can get displayed) 467 | rt_sleep(cycle_ns / 100); 468 | } 469 | break; 470 | 471 | default: 472 | break; 473 | } 474 | 475 | // done if we got to here 476 | break; 477 | } 478 | 479 | // set master time in nano-seconds 480 | ecrt_master_application_time(master, wakeup_time); 481 | 482 | // calc next wake time (in sys time) 483 | wakeup_time += cycle_ns; 484 | } 485 | 486 | /****************************************************************************/ 487 | 488 | void my_cyclic(void) 489 | { 490 | int cycle_counter = 0; 491 | unsigned int blink = 0; 492 | 493 | signed long temp[8]={}; 494 | 495 | uint32_t target_position = 0;int once=0; 496 | uint32_t positions[N]; 497 | int i,dir=0; 498 | 499 | // oneshot mode to allow adjustable wake time 500 | rt_set_oneshot_mode(); 501 | 502 | // set first wake time in a few cycles 503 | wakeup_time = system_time_ns() + 10 * cycle_ns; 504 | 505 | // start the timer 506 | start_rt_timer(nano2count(cycle_ns)); 507 | 508 | rt_make_hard_real_time(); 509 | 510 | while (run) { 511 | // wait for next period (using adjustable system time) 512 | wait_period(); 513 | 514 | cycle_counter++; 515 | 516 | if (!run) { 517 | break; 518 | } 519 | 520 | // receive EtherCAT 521 | ecrt_master_receive(master); 522 | ecrt_domain_process(domain1); 523 | 524 | rt_check_domain_state(); 525 | 526 | if (!(cycle_counter % 1000)) { 527 | rt_check_master_state(); 528 | } 529 | 530 | if (!(cycle_counter % 200)) { 531 | blink = !blink; 532 | } 533 | 534 | //EC_WRITE_U8(domain1_pd + off_dig_out0, blink ? 0x00 : 0x0F); 535 | 536 | //is620n control 537 | temp[0]=EC_READ_U16(domain1_pd + offset.status_word_6041_0); 538 | temp[1]=EC_READ_U32(domain1_pd + offset.position_actual_value_6064_0); 539 | if(!once++){ 540 | for(i=0;i 7 | * 8 | * This file is part of the IgH EtherCAT master 9 | * 10 | * The IgH EtherCAT Master is free software; you can redistribute it and/or 11 | * modify it under the terms of the GNU General Public License version 2, as 12 | * published by the Free Software Foundation. 13 | * 14 | * The IgH EtherCAT master is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 17 | * Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License along 20 | * with the IgH EtherCAT master. If not, see . 21 | * 22 | * --- 23 | * 24 | * The license mentioned above concerns the source code only. Using the 25 | * EtherCAT technology and brand is only permitted in compliance with the 26 | * industrial property and similar rights of Beckhoff Automation GmbH. 27 | * 这个例子在rtai下使用 28 | *build: 29 | $gcc -o is620n_dc_rt_csv is620n_dc_rt_csv.c -I/opt/etherlab/include -I/opt/rtai-5.1/include -L/opt/rtai-5.1/lib -L/opt/etherlab/lib -llxrt -lrtdm -lethercat -lpthread -Wl,--rpath=/opt/rtai-5.1/lib -Wl,--rpath=/opt/etherlab/lib 30 | *run: 31 | * $sudo insmod /opt/rtai-5.1/modules/rtai_hal.ko 32 | * $sudo insmod /opt/rtai-5.1/modules/rtai_sched.ko 33 | * $sudo insmod /opt/rtai-5.1/modules/rtai_fifos.ko 34 | * $sudo insmod /opt/rtai-5.1/modules/rtai_sem.ko 35 | * $sudo insmod /opt/rtai-5.1/modules/rtai_shm.ko 36 | * $sudo insmod /opt/rtai-5.1/modules/rtai_rtdm.ko 37 | * $sudo ./main 38 | *****************************************************************************/ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | 50 | #include "ecrt.h" 51 | 52 | #define rt_printf(X, Y) 53 | 54 | //#define CLOCK_TO_USE CLOCK_MONOTONIC //CLOCK_REALTIME 55 | #define NSEC_PER_SEC 1000000000 56 | 57 | RT_TASK *task; 58 | 59 | static unsigned int cycle_ns = 1000000; /* 1 ms */ 60 | 61 | static int run = 1; 62 | 63 | /****************************************************************************/ 64 | 65 | // EtherCAT 66 | static ec_master_t *master = NULL; 67 | static ec_master_state_t master_state = {}; 68 | 69 | static ec_domain_t *domain1 = NULL; 70 | static ec_domain_state_t domain1_state = {}; 71 | 72 | static uint8_t *domain1_pd = NULL; 73 | 74 | static ec_slave_config_t *sc_dig_out_01 = NULL; 75 | 76 | /****************************************************************************/ 77 | 78 | // EtherCAT distributed clock variables 79 | 80 | #define DC_FILTER_CNT 1024 81 | #define SYNC_MASTER_TO_REF 1 82 | 83 | static uint64_t dc_start_time_ns = 0LL; 84 | static uint64_t dc_time_ns = 0; 85 | #if SYNC_MASTER_TO_REF 86 | static uint8_t dc_started = 0; 87 | static int32_t dc_diff_ns = 0; 88 | static int32_t prev_dc_diff_ns = 0; 89 | static int64_t dc_diff_total_ns = 0LL; 90 | static int64_t dc_delta_total_ns = 0LL; 91 | static int dc_filter_idx = 0; 92 | static int64_t dc_adjust_ns; 93 | #endif 94 | static int64_t system_time_base = 0LL; 95 | static uint64_t wakeup_time = 0LL; 96 | static uint64_t overruns = 0LL; 97 | 98 | /****************************************************************************/ 99 | 100 | // process data 101 | 102 | #define BusCoupler01_Pos 0, 0 103 | #define DigOutSlave01_Pos 0, 1 104 | 105 | #define Beckhoff_EK1100 0x00000002, 0x044c2c52 106 | //#define Beckhoff_EL2004 0x00000002, 0x07d43052 107 | #define Beckhoff_EL2008 0x00000002, 0x07d83052 108 | 109 | // offsets for PDO entries 110 | static unsigned int off_dig_out0 = 0; 111 | 112 | // process data 113 | #if 0 114 | const static ec_pdo_entry_reg_t domain1_regs[] = { 115 | //{DigOutSlave01_Pos, Beckhoff_EL2004, 0x7000, 0x01, &off_dig_out0, NULL}, 116 | {DigOutSlave01_Pos, Beckhoff_EL2008, 0x7000, 0x01, &off_dig_out0, NULL}, 117 | {} 118 | }; 119 | 120 | /****************************************************************************/ 121 | 122 | /* Slave 1, "EL2004" 123 | * Vendor ID: 0x00000002 124 | * Product code: 0x07d43052 125 | * Revision number: 0x00100000 126 | */ 127 | 128 | /* Slave 1, "EL2008" 129 | * Vendor ID: 0x00000002 130 | * Product code: 0x07d83052 131 | * Revision number: 0x00100000 132 | */ 133 | 134 | ec_pdo_entry_info_t slave_1_pdo_entries[] = { 135 | {0x7000, 0x01, 1}, /* Output */ 136 | {0x7010, 0x01, 1}, /* Output */ 137 | {0x7020, 0x01, 1}, /* Output */ 138 | {0x7030, 0x01, 1}, /* Output */ 139 | }; 140 | 141 | ec_pdo_info_t slave_1_pdos[] = { 142 | {0x1600, 1, slave_1_pdo_entries + 0}, /* Channel 1 */ 143 | {0x1601, 1, slave_1_pdo_entries + 1}, /* Channel 2 */ 144 | {0x1602, 1, slave_1_pdo_entries + 2}, /* Channel 3 */ 145 | {0x1603, 1, slave_1_pdo_entries + 3}, /* Channel 4 */ 146 | }; 147 | 148 | ec_sync_info_t slave_1_syncs[] = { 149 | {0, EC_DIR_OUTPUT, 4, slave_1_pdos + 0, EC_WD_ENABLE}, 150 | {0xff} 151 | }; 152 | #endif 153 | 154 | #define IS620NSlavePos 0,3 /*EtherCAT address on the bus*/ 155 | #define INOVANCE_IS620N 0x00100000, 0x000C0108 /*IS620N Vendor ID, product code*/ 156 | 157 | /*x86_64: char-1byte,int-4bytes,long-8bytes*/ 158 | struct IS620N_offset { 159 | //RxPDO 160 | unsigned int control_word_6040_0; //0x6040:控制字,subindex:0,bitlen:16 161 | unsigned int target_position_607a_0;//0x607A:目标位置,subindex:0,bitlen:32 162 | unsigned int touch_probe_60b8_0; //0x60B8:探针,subindex:0,bitlen:16 163 | unsigned int pysical_outputs_60fe_1;//0x60FE:pysical_outputs,subindex:1,bitlen:32 164 | unsigned int target_velocity_60ff_0;//0x60FF:target_velocity,subindex:0,bitlen:32 165 | unsigned int target_torque_6071_0;//0x6071:int target_torque,subindex:0,bitlen:16 166 | unsigned int modes_operation_6060_0;//0x6060:Modes of operation,subindex:0,bitlen:8 167 | unsigned int max_profile_velocity_607f_0;//0x607F:max_profile_velocity,subindex:0,bitlen:32 168 | unsigned int positive_torque_limit_value_60e0_0;//0x60E0:positive_torque_limit_value,subindex:0,bitlen:16 169 | unsigned int negative_torque_limit_value_60e1_0;//0x60E1:negaitive_torque_limit_value,subindex:0,bitlen:16 170 | unsigned int torque_offset_60b2_0;//0x60B2:torque offset,subindex:0,bitlen:16 171 | unsigned int max_torque_6072_0;//0x6072:max torque,subindex:0,bitlen:16 172 | 173 | //TxPDo 174 | unsigned int status_word_6041_0;//0x6041:status_word,subindex:0,bitlen:16 175 | unsigned int position_actual_value_6064_0;//0x6064:position_actual_value,subindex:0,bitlen:32 176 | unsigned int touch_probe_status_60b9_0;//0x60B9,subindex:0,bitlen:16 177 | unsigned int touch_probe_pos1_pos_value_60ba_0;//0x60BA,subindex:0,bitlen:32 178 | unsigned int touch_probe_pos2_pos_value_60bc_0;//0x60BC ,subindex:0,bitlen:32 179 | unsigned int error_code_603f_0;//0x603F,subindex:0,bitlen:16 180 | unsigned int digital_inputs_60fd_0;//0x60FD,subindex:0,bitlen:32 181 | unsigned int torque_actual_value_6077_0;//0x6077,subindex:0,bitlen:16 182 | unsigned int following_error_actual_value_60f4_0;//0x60F4,subindex:0,bitlen:32 183 | unsigned int modes_of_operation_display_6061_0;//0x6061,subindex:0,bitlen:8 184 | unsigned int velocity_actual_value_606c_0;//0x606C,subindex:0,bitlen:32 185 | }; 186 | 187 | static struct IS620N_offset offset; 188 | 189 | const static ec_pdo_entry_reg_t domain1_regs[] = { 190 | //RxPDO 191 | {IS620NSlavePos, INOVANCE_IS620N, 0x6040, 0, &offset.control_word_6040_0}, 192 | {IS620NSlavePos, INOVANCE_IS620N, 0x607A, 0, &offset.target_position_607a_0}, 193 | {IS620NSlavePos, INOVANCE_IS620N, 0x60FF, 0, &offset.target_velocity_60ff_0}, 194 | {IS620NSlavePos, INOVANCE_IS620N, 0x6071, 0, &offset.target_torque_6071_0}, 195 | {IS620NSlavePos, INOVANCE_IS620N, 0x6060, 0, &offset.modes_operation_6060_0}, 196 | {IS620NSlavePos, INOVANCE_IS620N, 0x60B8, 0, &offset.touch_probe_60b8_0}, 197 | {IS620NSlavePos, INOVANCE_IS620N, 0x607F, 0, &offset.max_profile_velocity_607f_0}, 198 | //TxPDO 199 | {IS620NSlavePos, INOVANCE_IS620N, 0x603F, 0, &offset.error_code_603f_0}, 200 | {IS620NSlavePos, INOVANCE_IS620N, 0x6041, 0, &offset.status_word_6041_0}, 201 | {IS620NSlavePos, INOVANCE_IS620N, 0x6064, 0, &offset.position_actual_value_6064_0}, 202 | {IS620NSlavePos, INOVANCE_IS620N, 0x6077, 0, &offset.torque_actual_value_6077_0}, 203 | {IS620NSlavePos, INOVANCE_IS620N, 0x6061, 0, &offset.modes_of_operation_display_6061_0}, 204 | {IS620NSlavePos, INOVANCE_IS620N, 0x60B9, 0, &offset.touch_probe_status_60b9_0}, 205 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BA, 0, &offset.touch_probe_pos1_pos_value_60ba_0}, 206 | {IS620NSlavePos, INOVANCE_IS620N, 0x60BC, 0, &offset.touch_probe_pos2_pos_value_60bc_0}, 207 | {IS620NSlavePos, INOVANCE_IS620N, 0x60FD, 0, &offset.digital_inputs_60fd_0}, 208 | {} 209 | }; 210 | 211 | /*重新映射PDO才需要定义下面这3个数组,如果使用servo的固定映射则不需要*/ 212 | 213 | /*Config PDOs*/ 214 | static ec_pdo_entry_info_t IS620N_pdo_entries[] = { 215 | //RxPdo 0x1702 216 | {0x6040, 0x00, 16}, 217 | {0x607A, 0x00, 32}, 218 | {0x60FF, 0x00, 32}, 219 | {0x6071, 0x00, 16}, 220 | {0x6060, 0x00, 8}, 221 | {0x60B8, 0x00, 16}, 222 | {0x607F, 0x00, 32}, 223 | //TxPdo 0x1B02 224 | {0x603F, 0x00, 16}, 225 | {0x6041, 0x00, 16}, 226 | {0x6064, 0x00, 32}, 227 | {0x6077, 0x00, 16}, 228 | {0x6061, 0x00, 8}, 229 | {0x60B9, 0x00, 16}, 230 | {0x60BA, 0x00, 32}, 231 | {0x60BC, 0x00, 32}, 232 | {0x60FD, 0x00, 32}, 233 | }; 234 | 235 | static ec_pdo_info_t IS620N_pdos[] = { 236 | //RxPdo 237 | {0x1600, 7, IS620N_pdo_entries + 0 }, 238 | //TxPdo 239 | {0x1A00, 9, IS620N_pdo_entries + 7 } 240 | }; 241 | 242 | static ec_sync_info_t IS620N_syncs[] = { 243 | /*{ 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE }, 244 | { 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE },*/ 245 | { 2, EC_DIR_OUTPUT, 1, IS620N_pdos + 0/*,EC_WD_ENABLE*//*, EC_WD_DISABLE*/ }, 246 | { 3, EC_DIR_INPUT, 1, IS620N_pdos + 1/*,EC_WD_DISABLE*//*, EC_WD_DISABLE*/ }, 247 | { 0xFF} 248 | }; 249 | 250 | /***************************************************************************** 251 | * Realtime task 252 | ****************************************************************************/ 253 | 254 | /** Get the time in ns for the current cpu, adjusted by system_time_base. 255 | * 256 | * \attention Rather than calling rt_get_time_ns() directly, all application 257 | * time calls should use this method instead. 258 | * 259 | * \ret The time in ns. 260 | */ 261 | uint64_t system_time_ns(void) 262 | { 263 | RTIME time = rt_get_time_ns(); 264 | 265 | if (system_time_base > time) { 266 | rt_printk("%s() error: system_time_base greater than" 267 | " system time (system_time_base: %lld, time: %llu\n", 268 | __func__, system_time_base, time); 269 | return time; 270 | } 271 | else { 272 | return time - system_time_base; 273 | } 274 | } 275 | 276 | /****************************************************************************/ 277 | 278 | /** Convert system time to RTAI time in counts (via the system_time_base). 279 | */ 280 | RTIME system2count( 281 | uint64_t time 282 | ) 283 | { 284 | RTIME ret; 285 | 286 | if ((system_time_base < 0) && 287 | ((uint64_t) (-system_time_base) > time)) { 288 | rt_printk("%s() error: system_time_base less than" 289 | " system time (system_time_base: %lld, time: %llu\n", 290 | __func__, system_time_base, time); 291 | ret = time; 292 | } 293 | else { 294 | ret = time + system_time_base; 295 | } 296 | 297 | return nano2count(ret); 298 | } 299 | 300 | /*****************************************************************************/ 301 | 302 | /** Synchronise the distributed clocks 303 | */ 304 | void sync_distributed_clocks(void) 305 | { 306 | #if SYNC_MASTER_TO_REF 307 | uint32_t ref_time = 0; 308 | uint64_t prev_app_time = dc_time_ns; 309 | #endif 310 | 311 | dc_time_ns = system_time_ns(); 312 | 313 | #if SYNC_MASTER_TO_REF 314 | // get reference clock time to synchronize master cycle 315 | ecrt_master_reference_clock_time(master, &ref_time); 316 | dc_diff_ns = (uint32_t) prev_app_time - ref_time; 317 | #else 318 | // sync reference clock to master 319 | ecrt_master_sync_reference_clock_to(master, dc_time_ns); 320 | #endif 321 | 322 | // call to sync slaves to ref slave 323 | ecrt_master_sync_slave_clocks(master); 324 | } 325 | 326 | /*****************************************************************************/ 327 | 328 | /** Return the sign of a number 329 | * 330 | * ie -1 for -ve value, 0 for 0, +1 for +ve value 331 | * 332 | * \retval the sign of the value 333 | */ 334 | #define sign(val) \ 335 | ({ typeof (val) _val = (val); \ 336 | ((_val > 0) - (_val < 0)); }) 337 | 338 | /*****************************************************************************/ 339 | 340 | /** Update the master time based on ref slaves time diff 341 | * 342 | * called after the ethercat frame is sent to avoid time jitter in 343 | * sync_distributed_clocks() 344 | */ 345 | void update_master_clock(void) 346 | { 347 | #if SYNC_MASTER_TO_REF 348 | // calc drift (via un-normalised time diff) 349 | int32_t delta = dc_diff_ns - prev_dc_diff_ns; 350 | prev_dc_diff_ns = dc_diff_ns; 351 | 352 | // normalise the time diff 353 | dc_diff_ns = 354 | ((dc_diff_ns + (cycle_ns / 2)) % cycle_ns) - (cycle_ns / 2); 355 | 356 | // only update if primary master 357 | if (dc_started) { 358 | 359 | // add to totals 360 | dc_diff_total_ns += dc_diff_ns; 361 | dc_delta_total_ns += delta; 362 | dc_filter_idx++; 363 | 364 | if (dc_filter_idx >= DC_FILTER_CNT) { 365 | // add rounded delta average 366 | dc_adjust_ns += 367 | ((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT); 368 | 369 | // and add adjustment for general diff (to pull in drift) 370 | dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT); 371 | 372 | // limit crazy numbers (0.1% of std cycle time) 373 | if (dc_adjust_ns < -1000) { 374 | dc_adjust_ns = -1000; 375 | } 376 | if (dc_adjust_ns > 1000) { 377 | dc_adjust_ns = 1000; 378 | } 379 | 380 | // reset 381 | dc_diff_total_ns = 0LL; 382 | dc_delta_total_ns = 0LL; 383 | dc_filter_idx = 0; 384 | } 385 | 386 | // add cycles adjustment to time base (including a spot adjustment) 387 | system_time_base += dc_adjust_ns + sign(dc_diff_ns); 388 | } 389 | else { 390 | dc_started = (dc_diff_ns != 0); 391 | 392 | if (dc_started) { 393 | // output first diff 394 | rt_printk("First master diff: %d.\n", dc_diff_ns); 395 | 396 | // record the time of this initial cycle 397 | dc_start_time_ns = dc_time_ns; 398 | } 399 | } 400 | #endif 401 | } 402 | 403 | /****************************************************************************/ 404 | 405 | void rt_check_domain_state(void) 406 | { 407 | ec_domain_state_t ds = {}; 408 | 409 | ecrt_domain_state(domain1, &ds); 410 | 411 | if (ds.working_counter != domain1_state.working_counter) { 412 | rt_printf("Domain1: WC %u.\n", ds.working_counter); 413 | } 414 | 415 | if (ds.wc_state != domain1_state.wc_state) { 416 | rt_printf("Domain1: State %u.\n", ds.wc_state); 417 | } 418 | 419 | domain1_state = ds; 420 | } 421 | 422 | /****************************************************************************/ 423 | 424 | void rt_check_master_state(void) 425 | { 426 | ec_master_state_t ms; 427 | 428 | ecrt_master_state(master, &ms); 429 | 430 | if (ms.slaves_responding != master_state.slaves_responding) { 431 | rt_printf("%u slave(s).\n", ms.slaves_responding); 432 | } 433 | 434 | if (ms.al_states != master_state.al_states) { 435 | rt_printf("AL states: 0x%02X.\n", ms.al_states); 436 | } 437 | 438 | if (ms.link_up != master_state.link_up) { 439 | rt_printf("Link is %s.\n", ms.link_up ? "up" : "down"); 440 | } 441 | 442 | master_state = ms; 443 | } 444 | 445 | /****************************************************************************/ 446 | 447 | /** Wait for the next period 448 | */ 449 | 450 | void wait_period(void) 451 | { 452 | while (1) 453 | { 454 | RTIME wakeup_count = system2count(wakeup_time); 455 | RTIME current_count = rt_get_time(); 456 | 457 | if ((wakeup_count < current_count) 458 | || (wakeup_count > current_count + (50 * cycle_ns))) { 459 | rt_printk("%s(): unexpected wake time!\n", __func__); 460 | } 461 | 462 | switch (rt_sleep_until(wakeup_count)) { 463 | case RTE_UNBLKD: 464 | rt_printk("rt_sleep_until(): RTE_UNBLKD\n"); 465 | continue; 466 | 467 | case RTE_TMROVRN: 468 | rt_printk("rt_sleep_until(): RTE_TMROVRN\n"); 469 | overruns++; 470 | 471 | if (overruns % 100 == 0) { 472 | // in case wake time is broken ensure other processes get 473 | // some time slice (and error messages can get displayed) 474 | rt_sleep(cycle_ns / 100); 475 | } 476 | break; 477 | 478 | default: 479 | break; 480 | } 481 | 482 | // done if we got to here 483 | break; 484 | } 485 | 486 | // set master time in nano-seconds 487 | ecrt_master_application_time(master, wakeup_time); 488 | 489 | // calc next wake time (in sys time) 490 | wakeup_time += cycle_ns; 491 | } 492 | 493 | /****************************************************************************/ 494 | 495 | void my_cyclic(void) 496 | { 497 | int cycle_counter = 0; 498 | unsigned int blink = 0; 499 | 500 | signed long temp[8]={}; 501 | 502 | // oneshot mode to allow adjustable wake time 503 | rt_set_oneshot_mode(); 504 | 505 | // set first wake time in a few cycles 506 | wakeup_time = system_time_ns() + 10 * cycle_ns; 507 | 508 | // start the timer 509 | start_rt_timer(nano2count(cycle_ns)); 510 | 511 | rt_make_hard_real_time(); 512 | 513 | while (run) { 514 | // wait for next period (using adjustable system time) 515 | wait_period(); 516 | 517 | cycle_counter++; 518 | 519 | if (!run) { 520 | break; 521 | } 522 | 523 | // receive EtherCAT 524 | ecrt_master_receive(master); 525 | ecrt_domain_process(domain1); 526 | 527 | rt_check_domain_state(); 528 | 529 | if (!(cycle_counter % 1000)) { 530 | rt_check_master_state(); 531 | } 532 | 533 | if (!(cycle_counter % 200)) { 534 | blink = !blink; 535 | } 536 | 537 | //EC_WRITE_U8(domain1_pd + off_dig_out0, blink ? 0x00 : 0x0F); 538 | 539 | //is620n control 540 | temp[0]=EC_READ_U16(domain1_pd + offset.status_word_6041_0); 541 | temp[1]=EC_READ_U32(domain1_pd + offset.position_actual_value_6064_0); 542 | //printf("\r%6f \t ",((float)temp[1]/1000) ); 543 | 544 | // write process data 545 | /*if(servooff==1){//servo off 546 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0006 ); 547 | //deactive++; 548 | } 549 | else*/if(temp[0] == 0x218){ 550 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0080 ); 551 | } 552 | else if( (temp[0]&0x04f) == 0x0040 ){ //temp[0]==0x250 553 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0006 ); 554 | //printf("1.state = %x\n",temp[0]); 555 | } 556 | else if( (temp[0]&0x06f) == 0x0021){ //temp[0]==0x231 557 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x0007 ); 558 | //printf("2.state = %x\n",temp[0]); 559 | } 560 | else if( (temp[0]&0x06f) == 0x023){ //temp[0]==0x233 561 | EC_WRITE_S8(domain1_pd+offset.modes_operation_6060_0, 9);//csv mode 562 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x000f); 563 | //printf("3.state = %x\n",temp[0]); 564 | } 565 | else if( (temp[0]&0x06f) == 0x027){//600 800 //temp[0]=0x237 566 | EC_WRITE_U32(domain1_pd+offset.target_velocity_60ff_0, 0x1000000); 567 | EC_WRITE_U16(domain1_pd+offset.control_word_6040_0, 0x001f); 568 | //printf("4.state = %x\n",temp[0]); 569 | } 570 | //is620n control end 571 | 572 | // queue process data 573 | ecrt_domain_queue(domain1); 574 | 575 | // sync distributed clock just before master_send to set 576 | // most accurate master clock time 577 | sync_distributed_clocks(); 578 | 579 | // send EtherCAT data 580 | ecrt_master_send(master); 581 | 582 | // update the master clock 583 | // Note: called after ecrt_master_send() to reduce time 584 | // jitter in the sync_distributed_clocks() call 585 | update_master_clock(); 586 | } 587 | 588 | rt_make_soft_real_time(); 589 | stop_rt_timer(); 590 | } 591 | 592 | /**************************************************************************** 593 | * Signal handler 594 | ***************************************************************************/ 595 | 596 | void signal_handler(int sig) 597 | { 598 | run = 0; 599 | } 600 | 601 | /**************************************************************************** 602 | * Main function 603 | ***************************************************************************/ 604 | 605 | int main(int argc, char *argv[]) 606 | { 607 | //ec_slave_config_t *sc_ek1100; 608 | ec_slave_config_t *sc_is620n; 609 | int ret; 610 | 611 | signal(SIGTERM, signal_handler); 612 | signal(SIGINT, signal_handler); 613 | 614 | if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { 615 | perror("mlockall failed"); 616 | return -1; 617 | } 618 | 619 | printf("Requesting master...\n"); 620 | master = ecrt_request_master(0); 621 | if (!master) { 622 | return -1; 623 | } 624 | 625 | domain1 = ecrt_master_create_domain(master); 626 | if (!domain1) { 627 | return -1; 628 | } 629 | 630 | printf("Creating slave configurations...\n"); 631 | 632 | #if 0 633 | // Create configuration for bus coupler 634 | sc_ek1100 = 635 | ecrt_master_slave_config(master, BusCoupler01_Pos, Beckhoff_EK1100); 636 | if (!sc_ek1100) { 637 | return -1; 638 | } 639 | 640 | /*sc_dig_out_01 = 641 | ecrt_master_slave_config(master, DigOutSlave01_Pos, Beckhoff_EL2004); 642 | if (!sc_dig_out_01) { 643 | fprintf(stderr, "Failed to get slave configuration.\n"); 644 | return -1; 645 | }*/ 646 | 647 | sc_dig_out_01 = 648 | ecrt_master_slave_config(master, DigOutSlave01_Pos, Beckhoff_EL2008); 649 | if (!sc_dig_out_01) { 650 | fprintf(stderr, "Failed to get slave configuration.\n"); 651 | return -1; 652 | } 653 | //设置EL2008　eeprom 654 | if (ecrt_slave_config_pdos(sc_dig_out_01, EC_END, slave_1_syncs)) { 655 | fprintf(stderr, "Failed to configure PDOs.\n"); 656 | return -1; 657 | } 658 | //设置PDO映射 659 | if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { 660 | fprintf(stderr, "PDO entry registration failed!\n"); 661 | return -1; 662 | } 663 | #endif 664 | 665 | //配置IS620 666 | if (!(sc_is620n = ecrt_master_slave_config( 667 | master, IS620NSlavePos, INOVANCE_IS620N))) { 668 | fprintf(stderr, "Failed to get slave1 configuration.\n"); 669 | return -1; 670 | } 671 | 672 | printf("Configuring PDOs...\n"); 673 | 674 | if (ecrt_slave_config_pdos(sc_is620n, EC_END, IS620N_syncs)) { 675 | fprintf(stderr, "Failed to configure is620n PDOs.\n"); 676 | return -1; 677 | } 678 | 679 | if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { 680 | fprintf(stderr, "is620n PDO entry registration failed!\n"); 681 | return -1; 682 | } 683 | //配置IS620结束 684 | 685 | /* Set the initial master time and select a slave to use as the DC 686 | * reference clock, otherwise pass NULL to auto select the first capable 687 | * slave. Note: This can be used whether the master or the ref slave will 688 | * be used as the systems master DC clock. 689 | */ 690 | dc_start_time_ns = system_time_ns(); 691 | dc_time_ns = dc_start_time_ns; 692 | 693 | #if 0 694 | ret = ecrt_master_select_reference_clock(master, sc_ek1100); 695 | if (ret < 0) { 696 | fprintf(stderr, "Failed to select reference clock: %s\n", 697 | strerror(-ret)); 698 | return ret; 699 | } 700 | #endif 701 | 702 | ret = ecrt_master_select_reference_clock(master, sc_is620n); 703 | if (ret < 0) { 704 | fprintf(stderr, "Failed to select reference clock: %s\n", 705 | strerror(-ret)); 706 | return ret; 707 | } 708 | 709 | ecrt_slave_config_dc(sc_is620n, 0x0300, 1000000, 1000000/2, 0, 0);//1ms 710 | 711 | ret = ecrt_master_select_reference_clock(master, sc_is620n); 712 | if (ret < 0) { 713 | fprintf(stderr, "Failed to select reference clock: %s\n", 714 | strerror(-ret)); 715 | return ret; 716 | } 717 | 718 | printf("Activating master...\n"); 719 | if (ecrt_master_activate(master)) { 720 | return -1; 721 | } 722 | 723 | if (!(domain1_pd = ecrt_domain_data(domain1))) { 724 | fprintf(stderr, "Failed to get domain data pointer.\n"); 725 | return -1; 726 | } 727 | 728 | /* Create cyclic RT-thread */ 729 | struct sched_param param; 730 | param.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1; 731 | if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { 732 | puts("ERROR IN SETTING THE SCHEDULER"); 733 | perror("errno"); 734 | return -1; 735 | } 736 | 737 | task = rt_task_init(nam2num("ec_rtai_rtdm_example"), 738 | 0 /* priority */, 0 /* stack size */, 0 /* msg size */); 739 | 740 | my_cyclic(); 741 | 742 | rt_task_delete(task); 743 | 744 | printf("End of Program\n"); 745 | ecrt_release_master(master); 746 | 747 | return 0; 748 | } 749 | 750 | /****************************************************************************/ -------------------------------------------------------------------------------- /rtai_rtdm_dc/main.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * $Id$ 4 | * 5 | * Copyright (C) 2011 IgH Andreas Stewering-Bone 6 | * 2012 Florian Pose 7 | * 8 | * This file is part of the IgH EtherCAT master 9 | * 10 | * The IgH EtherCAT Master is free software; you can redistribute it and/or 11 | * modify it under the terms of the GNU General Public License version 2, as 12 | * published by the Free Software Foundation. 13 | * 14 | * The IgH EtherCAT master is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 17 | * Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License along 20 | * with the IgH EtherCAT master. If not, see . 21 | * 22 | * --- 23 | * 24 | * The license mentioned above concerns the source code only. Using the 25 | * EtherCAT technology and brand is only permitted in compliance with the 26 | * industrial property and similar rights of Beckhoff Automation GmbH. 27 | * 28 | *build: 29 | $gcc -o main main.c -I/opt/etherlab/include -I/opt/rtai-5.1/include -L/opt/rtai-5.1/lib -L/opt/etherlab/lib -llxrt -lrtdm -lethercat -lpthread -Wl,--rpath=/opt/rtai-5.1/lib -Wl,--rpath=/opt/etherlab/lib 30 | *run: 31 | * $sudo insmod /opt/rtai-5.1/modules/rtai_hal.ko 32 | * $sudo insmod /opt/rtai-5.1/modules/rtai_sched.ko 33 | * $sudo insmod /opt/rtai-5.1/modules/rtai_fifos.ko 34 | * $sudo insmod /opt/rtai-5.1/modules/rtai_sem.ko 35 | * $sudo insmod /opt/rtai-5.1/modules/rtai_shm.ko 36 | * $sudo insmod /opt/rtai-5.1/modules/rtai_rtdm.ko 37 | * $sudo ./main 38 | *****************************************************************************/ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | 49 | #include "ecrt.h" 50 | 51 | #define rt_printf(X, Y) 52 | 53 | #define NSEC_PER_SEC 1000000000 54 | 55 | RT_TASK *task; 56 | 57 | static unsigned int cycle_ns = 1000000; /* 1 ms */ 58 | 59 | static int run = 1; 60 | 61 | /****************************************************************************/ 62 | 63 | // EtherCAT 64 | static ec_master_t *master = NULL; 65 | static ec_master_state_t master_state = {}; 66 | 67 | static ec_domain_t *domain1 = NULL; 68 | static ec_domain_state_t domain1_state = {}; 69 | 70 | static uint8_t *domain1_pd = NULL; 71 | 72 | static ec_slave_config_t *sc_dig_out_01 = NULL; 73 | 74 | /****************************************************************************/ 75 | 76 | // EtherCAT distributed clock variables 77 | 78 | #define DC_FILTER_CNT 1024 79 | #define SYNC_MASTER_TO_REF 1 80 | 81 | static uint64_t dc_start_time_ns = 0LL; 82 | static uint64_t dc_time_ns = 0; 83 | #if SYNC_MASTER_TO_REF 84 | static uint8_t dc_started = 0; 85 | static int32_t dc_diff_ns = 0; 86 | static int32_t prev_dc_diff_ns = 0; 87 | static int64_t dc_diff_total_ns = 0LL; 88 | static int64_t dc_delta_total_ns = 0LL; 89 | static int dc_filter_idx = 0; 90 | static int64_t dc_adjust_ns; 91 | #endif 92 | static int64_t system_time_base = 0LL; 93 | static uint64_t wakeup_time = 0LL; 94 | static uint64_t overruns = 0LL; 95 | 96 | /****************************************************************************/ 97 | 98 | // process data 99 | 100 | #define BusCoupler01_Pos 0, 0 101 | #define DigOutSlave01_Pos 0, 2 102 | 103 | #define Beckhoff_EK1100 0x00000002, 0x044c2c52 104 | //#define Beckhoff_EL2004 0x00000002, 0x07d43052 105 | #define Beckhoff_EL2008 0x00000002, 0x07d83052 106 | 107 | // offsets for PDO entries 108 | static unsigned int off_dig_out0 = 0; 109 | 110 | // process data 111 | 112 | const static ec_pdo_entry_reg_t domain1_regs[] = { 113 | //{DigOutSlave01_Pos, Beckhoff_EL2004, 0x7000, 0x01, &off_dig_out0, NULL}, 114 | {DigOutSlave01_Pos, Beckhoff_EL2008, 0x7000, 0x01, &off_dig_out0, NULL}, 115 | {} 116 | }; 117 | 118 | /****************************************************************************/ 119 | 120 | /* Slave 1, "EL2004" 121 | * Vendor ID: 0x00000002 122 | * Product code: 0x07d43052 123 | * Revision number: 0x00100000 124 | */ 125 | 126 | /* Slave 1, "EL2008" 127 | * Vendor ID: 0x00000002 128 | * Product code: 0x07d83052 129 | * Revision number: 0x00100000 130 | */ 131 | ec_pdo_entry_info_t slave_1_pdo_entries[] = { 132 | {0x7000, 0x01, 1}, /* Output */ 133 | {0x7010, 0x01, 1}, /* Output */ 134 | {0x7020, 0x01, 1}, /* Output */ 135 | {0x7030, 0x01, 1}, /* Output */ 136 | }; 137 | 138 | ec_pdo_info_t slave_1_pdos[] = { 139 | {0x1600, 1, slave_1_pdo_entries + 0}, /* Channel 1 */ 140 | {0x1601, 1, slave_1_pdo_entries + 1}, /* Channel 2 */ 141 | {0x1602, 1, slave_1_pdo_entries + 2}, /* Channel 3 */ 142 | {0x1603, 1, slave_1_pdo_entries + 3}, /* Channel 4 */ 143 | }; 144 | 145 | ec_sync_info_t slave_1_syncs[] = { 146 | {0, EC_DIR_OUTPUT, 4, slave_1_pdos + 0, EC_WD_ENABLE}, 147 | {0xff} 148 | }; 149 | 150 | /***************************************************************************** 151 | * Realtime task 152 | ****************************************************************************/ 153 | 154 | /** Get the time in ns for the current cpu, adjusted by system_time_base. 155 | * 156 | * \attention Rather than calling rt_get_time_ns() directly, all application 157 | * time calls should use this method instead. 158 | * 159 | * \ret The time in ns. 160 | */ 161 | uint64_t system_time_ns(void) 162 | { 163 | RTIME time = rt_get_time_ns(); 164 | 165 | if (system_time_base > time) { 166 | rt_printk("%s() error: system_time_base greater than" 167 | " system time (system_time_base: %lld, time: %llu\n", 168 | __func__, system_time_base, time); 169 | return time; 170 | } 171 | else { 172 | return time - system_time_base; 173 | } 174 | } 175 | 176 | /****************************************************************************/ 177 | 178 | /** Convert system time to RTAI time in counts (via the system_time_base). 179 | */ 180 | RTIME system2count( 181 | uint64_t time 182 | ) 183 | { 184 | RTIME ret; 185 | 186 | if ((system_time_base < 0) && 187 | ((uint64_t) (-system_time_base) > time)) { 188 | rt_printk("%s() error: system_time_base less than" 189 | " system time (system_time_base: %lld, time: %llu\n", 190 | __func__, system_time_base, time); 191 | ret = time; 192 | } 193 | else { 194 | ret = time + system_time_base; 195 | } 196 | 197 | return nano2count(ret); 198 | } 199 | 200 | /*****************************************************************************/ 201 | 202 | /** Synchronise the distributed clocks 203 | */ 204 | void sync_distributed_clocks(void) 205 | { 206 | #if SYNC_MASTER_TO_REF 207 | uint32_t ref_time = 0; 208 | uint64_t prev_app_time = dc_time_ns; 209 | #endif 210 | 211 | dc_time_ns = system_time_ns(); 212 | 213 | // set master time in nano-seconds 214 | ecrt_master_application_time(master, dc_time_ns); 215 | 216 | #if SYNC_MASTER_TO_REF 217 | // get reference clock time to synchronize master cycle 218 | ecrt_master_reference_clock_time(master, &ref_time); 219 | dc_diff_ns = (uint32_t) prev_app_time - ref_time; 220 | #else 221 | // sync reference clock to master 222 | ecrt_master_sync_reference_clock(master); 223 | #endif 224 | 225 | // call to sync slaves to ref slave 226 | ecrt_master_sync_slave_clocks(master); 227 | } 228 | 229 | /*****************************************************************************/ 230 | 231 | /** Return the sign of a number 232 | * 233 | * ie -1 for -ve value, 0 for 0, +1 for +ve value 234 | * 235 | * \retval the sign of the value 236 | */ 237 | #define sign(val) \ 238 | ({ typeof (val) _val = (val); \ 239 | ((_val > 0) - (_val < 0)); }) 240 | 241 | /*****************************************************************************/ 242 | 243 | /** Update the master time based on ref slaves time diff 244 | * 245 | * called after the ethercat frame is sent to avoid time jitter in 246 | * sync_distributed_clocks() 247 | */ 248 | void update_master_clock(void) 249 | { 250 | #if SYNC_MASTER_TO_REF 251 | // calc drift (via un-normalised time diff) 252 | int32_t delta = dc_diff_ns - prev_dc_diff_ns; 253 | prev_dc_diff_ns = dc_diff_ns; 254 | 255 | // normalise the time diff 256 | dc_diff_ns = 257 | ((dc_diff_ns + (cycle_ns / 2)) % cycle_ns) - (cycle_ns / 2); 258 | 259 | // only update if primary master 260 | if (dc_started) { 261 | 262 | // add to totals 263 | dc_diff_total_ns += dc_diff_ns; 264 | dc_delta_total_ns += delta; 265 | dc_filter_idx++; 266 | 267 | if (dc_filter_idx >= DC_FILTER_CNT) { 268 | // add rounded delta average 269 | dc_adjust_ns += 270 | ((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT); 271 | 272 | // and add adjustment for general diff (to pull in drift) 273 | dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT); 274 | 275 | // limit crazy numbers (0.1% of std cycle time) 276 | if (dc_adjust_ns < -1000) { 277 | dc_adjust_ns = -1000; 278 | } 279 | if (dc_adjust_ns > 1000) { 280 | dc_adjust_ns = 1000; 281 | } 282 | 283 | // reset 284 | dc_diff_total_ns = 0LL; 285 | dc_delta_total_ns = 0LL; 286 | dc_filter_idx = 0; 287 | } 288 | 289 | // add cycles adjustment to time base (including a spot adjustment) 290 | system_time_base += dc_adjust_ns + sign(dc_diff_ns); 291 | } 292 | else { 293 | dc_started = (dc_diff_ns != 0); 294 | 295 | if (dc_started) { 296 | // output first diff 297 | rt_printk("First master diff: %d.\n", dc_diff_ns); 298 | 299 | // record the time of this initial cycle 300 | dc_start_time_ns = dc_time_ns; 301 | } 302 | } 303 | #endif 304 | } 305 | 306 | /****************************************************************************/ 307 | 308 | void rt_check_domain_state(void) 309 | { 310 | ec_domain_state_t ds = {}; 311 | 312 | ecrt_domain_state(domain1, &ds); 313 | 314 | if (ds.working_counter != domain1_state.working_counter) { 315 | rt_printf("Domain1: WC %u.\n", ds.working_counter); 316 | } 317 | 318 | if (ds.wc_state != domain1_state.wc_state) { 319 | rt_printf("Domain1: State %u.\n", ds.wc_state); 320 | } 321 | 322 | domain1_state = ds; 323 | } 324 | 325 | /****************************************************************************/ 326 | 327 | void rt_check_master_state(void) 328 | { 329 | ec_master_state_t ms; 330 | 331 | ecrt_master_state(master, &ms); 332 | 333 | if (ms.slaves_responding != master_state.slaves_responding) { 334 | rt_printf("%u slave(s).\n", ms.slaves_responding); 335 | } 336 | 337 | if (ms.al_states != master_state.al_states) { 338 | rt_printf("AL states: 0x%02X.\n", ms.al_states); 339 | } 340 | 341 | if (ms.link_up != master_state.link_up) { 342 | rt_printf("Link is %s.\n", ms.link_up ? "up" : "down"); 343 | } 344 | 345 | master_state = ms; 346 | } 347 | 348 | /****************************************************************************/ 349 | 350 | /** Wait for the next period 351 | */ 352 | void wait_period(void) 353 | { 354 | while (1) 355 | { 356 | RTIME wakeup_count = system2count(wakeup_time); 357 | RTIME current_count = rt_get_time(); 358 | 359 | if ((wakeup_count < current_count) 360 | || (wakeup_count > current_count + (50 * cycle_ns))) { 361 | rt_printk("%s(): unexpected wake time!\n", __func__); 362 | } 363 | 364 | switch (rt_sleep_until(wakeup_count)) { 365 | case RTE_UNBLKD: 366 | rt_printk("rt_sleep_until(): RTE_UNBLKD\n"); 367 | continue; 368 | 369 | case RTE_TMROVRN: 370 | rt_printk("rt_sleep_until(): RTE_TMROVRN\n"); 371 | overruns++; 372 | 373 | if (overruns % 100 == 0) { 374 | // in case wake time is broken ensure other processes get 375 | // some time slice (and error messages can get displayed) 376 | rt_sleep(cycle_ns / 100); 377 | } 378 | break; 379 | 380 | default: 381 | break; 382 | } 383 | 384 | // done if we got to here 385 | break; 386 | } 387 | 388 | // calc next wake time (in sys time) 389 | wakeup_time += cycle_ns; 390 | } 391 | 392 | /****************************************************************************/ 393 | 394 | void my_cyclic(void) 395 | { 396 | int cycle_counter = 0; 397 | unsigned int blink = 0; 398 | 399 | // oneshot mode to allow adjustable wake time 400 | rt_set_oneshot_mode(); 401 | 402 | // set first wake time in a few cycles 403 | wakeup_time = system_time_ns() + 10 * cycle_ns; 404 | 405 | // start the timer 406 | start_rt_timer(nano2count(cycle_ns)); 407 | 408 | rt_make_hard_real_time(); 409 | 410 | while (run) { 411 | // wait for next period (using adjustable system time) 412 | wait_period(); 413 | 414 | cycle_counter++; 415 | 416 | if (!run) { 417 | break; 418 | } 419 | 420 | // receive EtherCAT 421 | ecrt_master_receive(master); 422 | ecrt_domain_process(domain1); 423 | 424 | rt_check_domain_state(); 425 | 426 | if (!(cycle_counter % 1000)) { 427 | rt_check_master_state(); 428 | } 429 | 430 | if (!(cycle_counter % 200)) { 431 | blink = !blink; 432 | } 433 | 434 | EC_WRITE_U8(domain1_pd + off_dig_out0, blink ? 0x00 : 0x0F); 435 | 436 | // queue process data 437 | ecrt_domain_queue(domain1); 438 | 439 | // sync distributed clock just before master_send to set 440 | // most accurate master clock time 441 | sync_distributed_clocks(); 442 | 443 | // send EtherCAT data 444 | ecrt_master_send(master); 445 | 446 | // update the master clock 447 | // Note: called after ecrt_master_send() to reduce time 448 | // jitter in the sync_distributed_clocks() call 449 | update_master_clock(); 450 | } 451 | 452 | rt_make_soft_real_time(); 453 | stop_rt_timer(); 454 | } 455 | 456 | /**************************************************************************** 457 | * Signal handler 458 | ***************************************************************************/ 459 | 460 | void signal_handler(int sig) 461 | { 462 | run = 0; 463 | } 464 | 465 | /**************************************************************************** 466 | * Main function 467 | ***************************************************************************/ 468 | 469 | int main(int argc, char *argv[]) 470 | { 471 | ec_slave_config_t *sc_ek1100; 472 | int ret; 473 | 474 | signal(SIGTERM, signal_handler); 475 | signal(SIGINT, signal_handler); 476 | 477 | mlockall(MCL_CURRENT | MCL_FUTURE); 478 | 479 | printf("Requesting master...\n"); 480 | master = ecrt_request_master(0); 481 | if (!master) { 482 | return -1; 483 | } 484 | 485 | domain1 = ecrt_master_create_domain(master); 486 | if (!domain1) { 487 | return -1; 488 | } 489 | 490 | printf("Creating slave configurations...\n"); 491 | 492 | // Create configuration for bus coupler 493 | sc_ek1100 = 494 | ecrt_master_slave_config(master, BusCoupler01_Pos, Beckhoff_EK1100); 495 | if (!sc_ek1100) { 496 | return -1; 497 | } 498 | 499 | /*sc_dig_out_01 = 500 | ecrt_master_slave_config(master, DigOutSlave01_Pos, Beckhoff_EL2004); 501 | if (!sc_dig_out_01) { 502 | fprintf(stderr, "Failed to get slave configuration.\n"); 503 | return -1; 504 | }*/ 505 | 506 | sc_dig_out_01 = 507 | ecrt_master_slave_config(master, DigOutSlave01_Pos, Beckhoff_EL2008); 508 | if (!sc_dig_out_01) { 509 | fprintf(stderr, "Failed to get slave configuration.\n"); 510 | return -1; 511 | } 512 | //设置EL2008　eeprom 513 | if (ecrt_slave_config_pdos(sc_dig_out_01, EC_END, slave_1_syncs)) { 514 | fprintf(stderr, "Failed to configure PDOs.\n"); 515 | return -1; 516 | } 517 | //设置PDO映射 518 | if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { 519 | fprintf(stderr, "PDO entry registration failed!\n"); 520 | return -1; 521 | } 522 | 523 | /* Set the initial master time and select a slave to use as the DC 524 | * reference clock, otherwise pass NULL to auto select the first capable 525 | * slave. Note: This can be used whether the master or the ref slave will 526 | * be used as the systems master DC clock. 527 | */ 528 | dc_start_time_ns = system_time_ns(); 529 | dc_time_ns = dc_start_time_ns; 530 | 531 | /* Attention: The initial application time is also used for phase 532 | * calculation for the SYNC0/1 interrupts. Please be sure to call it at 533 | * the correct phase to the realtime cycle. 534 | */ 535 | ecrt_master_application_time(master, dc_start_time_ns); 536 | 537 | ret = ecrt_master_select_reference_clock(master, sc_ek1100); 538 | if (ret < 0) { 539 | fprintf(stderr, "Failed to select reference clock: %s\n", 540 | strerror(-ret)); 541 | return ret; 542 | } 543 | 544 | printf("Activating master...\n"); 545 | if (ecrt_master_activate(master)) { 546 | return -1; 547 | } 548 | 549 | if (!(domain1_pd = ecrt_domain_data(domain1))) { 550 | fprintf(stderr, "Failed to get domain data pointer.\n"); 551 | return -1; 552 | } 553 | 554 | /* Create cyclic RT-thread */ 555 | struct sched_param param; 556 | param.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1; 557 | if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { 558 | puts("ERROR IN SETTING THE SCHEDULER"); 559 | perror("errno"); 560 | return -1; 561 | } 562 | 563 | task = rt_task_init(nam2num("ec_rtai_rtdm_example"), 564 | 0 /* priority */, 0 /* stack size */, 0 /* msg size */); 565 | 566 | my_cyclic(); 567 | 568 | rt_task_delete(task); 569 | 570 | printf("End of Program\n"); 571 | ecrt_release_master(master); 572 | 573 | return 0; 574 | } 575 | 576 | /****************************************************************************/ 577 | -------------------------------------------------------------------------------- /rtai_rtdm_dc/xmc4800_relax.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * 3 | * $Id$ 4 | * 5 | * Copyright (C) 2011 IgH Andreas Stewering-Bone 6 | * 2012 Florian Pose 7 | * 8 | * This file is part of the IgH EtherCAT master 9 | * 10 | * The IgH EtherCAT Master is free software; you can redistribute it and/or 11 | * modify it under the terms of the GNU General Public License version 2, as 12 | * published by the Free Software Foundation. 13 | * 14 | * The IgH EtherCAT master is distributed in the hope that it will be useful, 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General 17 | * Public License for more details. 18 | * 19 | * You should have received a copy of the GNU General Public License along 20 | * with the IgH EtherCAT master. If not, see . 21 | * 22 | * --- 23 | * 24 | * The license mentioned above concerns the source code only. Using the 25 | * EtherCAT technology and brand is only permitted in compliance with the 26 | * industrial property and similar rights of Beckhoff Automation GmbH. 27 | * 28 | *build: 29 | $g++ -o xmc4800_relax xmc4800_relax.cpp -I/opt/etherlab/include -I/opt/rtai-5.1_v4.9.80/include -L/opt/rtai-5.1_v4.9.80/lib -L/opt/etherlab/lib -llxrt -lrtdm -lethercat -lpthread -Wl,--rpath=/opt/rtai-5.1_v4.9.80/lib -Wl,--rpath=/opt/etherlab/lib 30 | *run: 31 | * $sudo insmod /opt/rtai-5.1_v4.9.80/modules/rtai_hal.ko 32 | * $sudo insmod /opt/rtai-5.1_v4.9.80/modules/rtai_sched.ko 33 | * $sudo insmod /opt/rtai-5.1_v4.9.80/modules/rtai_fifos.ko 34 | * $sudo insmod /opt/rtai-5.1_v4.9.80/modules/rtai_sem.ko 35 | * $sudo insmod /opt/rtai-5.1_v4.9.80/modules/rtai_shm.ko 36 | * $sudo insmod /opt/rtai-5.1_v4.9.80/modules/rtai_rtdm.ko 37 | * $sudo ./xmc4800_relax 38 | *****************************************************************************/ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | 49 | #include "ecrt.h" 50 | 51 | #include 52 | #include 53 | using namespace std; 54 | 55 | #define rt_printf(X, Y) 56 | 57 | #define NSEC_PER_SEC 1000000000 58 | 59 | RT_TASK *task; 60 | 61 | static unsigned int cycle_ns = 1000000; /* 1 ms */ 62 | 63 | static int run = 1; 64 | 65 | /****************************************************************************/ 66 | 67 | // EtherCAT 68 | static ec_master_t *master = NULL; 69 | static ec_master_state_t master_state = {}; 70 | 71 | static ec_domain_t *domain1 = NULL; 72 | static ec_domain_state_t domain1_state = {}; 73 | 74 | static uint8_t *domain1_pd = NULL; 75 | 76 | static ec_slave_config_t *sc_dig_out_01 = NULL; 77 | 78 | /****************************************************************************/ 79 | 80 | // EtherCAT distributed clock variables 81 | 82 | #define DC_FILTER_CNT 1024 83 | #define SYNC_MASTER_TO_REF 1 84 | 85 | static uint64_t dc_start_time_ns = 0LL; 86 | static uint64_t dc_time_ns = 0; 87 | #if SYNC_MASTER_TO_REF 88 | static uint8_t dc_started = 0; 89 | static int32_t dc_diff_ns = 0; 90 | static int32_t prev_dc_diff_ns = 0; 91 | static int64_t dc_diff_total_ns = 0LL; 92 | static int64_t dc_delta_total_ns = 0LL; 93 | static int dc_filter_idx = 0; 94 | static int64_t dc_adjust_ns; 95 | #endif 96 | static int64_t system_time_base = 0LL; 97 | static uint64_t wakeup_time = 0LL; 98 | static uint64_t overruns = 0LL; 99 | 100 | /****************************************************************************/ 101 | 102 | // process data 103 | 104 | #define BusCoupler01_Pos 0, 0 105 | #define DigOutSlave01_Pos 0, 2 106 | #define Xmc4800_relax_Pos 0, 0 107 | 108 | #define SLAVE_POSITION(Alias,Position) Alias,Position 109 | 110 | #define Beckhoff_EK1100 0x00000002, 0x044c2c52 111 | //#define Beckhoff_EL2004 0x00000002, 0x07d43052 112 | #define Beckhoff_EL2008 0x00000002, 0x07d83052 113 | 114 | #define XMC4800_RELAX 0x0000034e, 0x00000000 115 | #define SLAVE_VENDORID(VendorId,ProductCode) VendorId,ProductCode 116 | 117 | #define INFINEON_ID 0x34E 118 | #define XMC4800_RELAX_PRODUCT_CODE1 0x00 119 | 120 | // offsets for PDO entries 121 | static unsigned int off_dig_out0 = 0; 122 | 123 | /*x86_64: char-1byte,int-4bytes,long-8bytes*/ 124 | struct XMC4800 { 125 | //RxPdo 0x1600 126 | unsigned int out_gen_int1; //0x7000,subindex:1,bitlen:16 127 | unsigned int out_gen_int2; //0x7000,subindex:2,bitlen:16 128 | unsigned int out_gen_int3; //0x7000,subindex:3,bitlen:16 129 | unsigned int out_gen_int4; //0x7000,subindex:4,bitlen:16 130 | unsigned int out_gen_bit1; //0x7000,subindex:5,bitlen:1 131 | 132 | //TxPdo 0x1a00 133 | unsigned int in_gen_int1; //0x6000,subindex:1,bitlen:16 134 | unsigned int in_gen_int2; //0x6000,subindex:2,bitlen:16 135 | unsigned int in_gen_int3; //0x6000,subindex:3,bitlen:16 136 | unsigned int in_gen_int4; //0x6000,subindex:4,bitlen:16 137 | unsigned int in_gen_bit1; //0x6000,subindex:5,bitlen:1 138 | }; 139 | 140 | // process data 141 | 142 | static struct XMC4800 xmc4800; 143 | 144 | #if 0 145 | const static ec_pdo_entry_reg_t domain1_regs[] = { 146 | //{DigOutSlave01_Pos, Beckhoff_EL2004, 0x7000, 0x01, &off_dig_out0, NULL}, 147 | /*{DigOutSlave01_Pos, Beckhoff_EL2008, 0x7000, 0x01, &off_dig_out0, NULL},*/ 148 | //RXPDO,0x1600 - 对于主站来说是Tx,对于本从站来说是Rx 149 | {Xmc4800_relax_Pos, XMC4800_RELAX, 0x7000, 0x01, &xmc4800.out_gen_int1,NULL}, 150 | {Xmc4800_relax_Pos, XMC4800_RELAX, 0x7000, 0x02, &xmc4800.out_gen_int2,NULL}, 151 | {Xmc4800_relax_Pos, XMC4800_RELAX, 0x7000, 0x03, &xmc4800.out_gen_int3,NULL}, 152 | {Xmc4800_relax_Pos, XMC4800_RELAX, 0x7000, 0x04, &xmc4800.out_gen_int4,NULL}, 153 | {Xmc4800_relax_Pos, XMC4800_RELAX, 0x7000, 0x05, &xmc4800.out_gen_bit1,NULL}, 154 | //TXPDO,0x1A00 - 对于主站来说是Rx,对于本从站来说是Tx 155 | {Xmc4800_relax_Pos, XMC4800_RELAX, 0x6000, 0x01, &xmc4800.in_gen_int1,NULL}, 156 | {Xmc4800_relax_Pos, XMC4800_RELAX, 0x6000, 0x02, &xmc4800.in_gen_int2,NULL}, 157 | {Xmc4800_relax_Pos, XMC4800_RELAX, 0x6000, 0x03, &xmc4800.in_gen_int3,NULL}, 158 | {Xmc4800_relax_Pos, XMC4800_RELAX, 0x6000, 0x04, &xmc4800.in_gen_int4,NULL}, 159 | {Xmc4800_relax_Pos, XMC4800_RELAX, 0x6000, 0x05, &xmc4800.in_gen_bit1,NULL}, 160 | {} 161 | }; 162 | #endif 163 | 164 | static ec_pdo_entry_reg_t *domain1_regs; 165 | 166 | //重新设置eeprom内的PDO entry,如果使用Fixed PDOs则不需要这3个数组 167 | static ec_pdo_entry_info_t XMC4800_pdo_entries[] = { 168 | //XMC4800 RxPdo 0x1600, output to slaves 169 | {0x7000, 0x01, 16}, 170 | {0x7000, 0x02, 16}, 171 | {0x7000, 0x03, 16}, 172 | {0x7000, 0x04, 16}, 173 | {0x7000, 0x05, 1}, 174 | {0x7000, 0x06, 1}, 175 | {0x7000, 0x07, 1}, 176 | {0x7000, 0x08, 1}, 177 | {0x7000, 0x09, 1}, 178 | {0x7000, 0x0a, 1}, 179 | {0x7000, 0x0b, 1}, 180 | {0x7000, 0x0c, 1}, 181 | //XMC4800 TxPdo 0x1a00, input from slaves 182 | {0x6000, 0x01, 16}, 183 | {0x6000, 0x02, 16}, 184 | {0x6000, 0x03, 16}, 185 | {0x6000, 0x04, 16}, 186 | {0x6000, 0x05, 1}, 187 | {0x6000, 0x06, 1}, 188 | {0x6000, 0x07, 1}, 189 | {0x6000, 0x08, 1}, 190 | {0x6000, 0x09, 1}, 191 | {0x6000, 0x0a, 1}, 192 | {0x6000, 0x0b, 1}, 193 | {0x6000, 0x0c, 1}, 194 | }; 195 | 196 | static ec_pdo_info_t XMC4800_pdos[] = { 197 | //RxPdo 198 | {0x1600, 12, XMC4800_pdo_entries + 0}, 199 | //TxPdo 200 | {0x1a00, 12, XMC4800_pdo_entries + 12} 201 | }; 202 | 203 | /* 204 | {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, 205 | {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, 206 | {2, EC_DIR_OUTPUT, 1, slave_0_pdos + 0, EC_WD_ENABLE}, 207 | {3, EC_DIR_INPUT, 1, slave_0_pdos + 1, EC_WD_DISABLE}, 208 | {0xff} 209 | */ 210 | 211 | static ec_sync_info_t XMC4800_syncs[] = { 212 | { 0, EC_DIR_OUTPUT, 0 ,NULL, EC_WD_DISABLE }, 213 | { 1, EC_DIR_INPUT, 0 ,NULL, EC_WD_DISABLE }, 214 | { 2, EC_DIR_OUTPUT, 1 ,XMC4800_pdos + 0 ,EC_WD_ENABLE },//主站是output 215 | { 3, EC_DIR_INPUT, 1 ,XMC4800_pdos + 1 ,EC_WD_DISABLE },//主站是intput 216 | {0xFF} 217 | }; 218 | /****************************************************************************/ 219 | 220 | /* Slave 1, "EL2004" 221 | * Vendor ID: 0x00000002 222 | * Product code: 0x07d43052 223 | * Revision number: 0x00100000 224 | */ 225 | 226 | /* Slave 1, "EL2008" 227 | * Vendor ID: 0x00000002 228 | * Product code: 0x07d83052 229 | * Revision number: 0x00100000 230 | */ 231 | ec_pdo_entry_info_t slave_1_pdo_entries[] = { 232 | {0x7000, 0x01, 1}, /* Output */ 233 | {0x7010, 0x01, 1}, /* Output */ 234 | {0x7020, 0x01, 1}, /* Output */ 235 | {0x7030, 0x01, 1}, /* Output */ 236 | {0x7040, 0x01, 1}, /* Output */ 237 | {0x7050, 0x01, 1}, /* Output */ 238 | {0x7060, 0x01, 1}, /* Output */ 239 | {0x7070, 0x01, 1}, /* Output */ 240 | }; 241 | 242 | ec_pdo_info_t slave_1_pdos[] = { 243 | {0x1600, 1, slave_1_pdo_entries + 0}, /* Channel 1 */ 244 | {0x1601, 1, slave_1_pdo_entries + 1}, /* Channel 2 */ 245 | {0x1602, 1, slave_1_pdo_entries + 2}, /* Channel 3 */ 246 | {0x1603, 1, slave_1_pdo_entries + 3}, /* Channel 4 */ 247 | {0x1604, 1, slave_1_pdo_entries + 4}, /* Channel 5 */ 248 | {0x1605, 1, slave_1_pdo_entries + 5}, /* Channel 6 */ 249 | {0x1606, 1, slave_1_pdo_entries + 6}, /* Channel 7 */ 250 | {0x1607, 1, slave_1_pdo_entries + 7}, /* Channel 8 */ 251 | }; 252 | 253 | ec_sync_info_t slave_1_syncs[] = { 254 | {0, EC_DIR_OUTPUT, 8, slave_1_pdos + 0, EC_WD_ENABLE}, 255 | {0xff} 256 | }; 257 | 258 | //ec_pdo_entry_reg_t *aa; 259 | 260 | /***************************************************************************** 261 | * Realtime task 262 | ****************************************************************************/ 263 | 264 | /** Get the time in ns for the current cpu, adjusted by system_time_base. 265 | * 266 | * \attention Rather than calling rt_get_time_ns() directly, all application 267 | * time calls should use this method instead. 268 | * 269 | * \ret The time in ns. 270 | */ 271 | uint64_t system_time_ns(void) 272 | { 273 | RTIME time = rt_get_time_ns(); 274 | 275 | if (system_time_base > time) { 276 | rt_printk("%s() error: system_time_base greater than" 277 | " system time (system_time_base: %lld, time: %llu\n", 278 | __func__, system_time_base, time); 279 | return time; 280 | } 281 | else { 282 | return time - system_time_base; 283 | } 284 | } 285 | 286 | /****************************************************************************/ 287 | 288 | /** Convert system time to RTAI time in counts (via the system_time_base). 289 | */ 290 | RTIME system2count( 291 | uint64_t time 292 | ) 293 | { 294 | RTIME ret; 295 | 296 | if ((system_time_base < 0) && 297 | ((uint64_t) (-system_time_base) > time)) { 298 | rt_printk("%s() error: system_time_base less than" 299 | " system time (system_time_base: %lld, time: %llu\n", 300 | __func__, system_time_base, time); 301 | ret = time; 302 | } 303 | else { 304 | ret = time + system_time_base; 305 | } 306 | 307 | return nano2count(ret); 308 | } 309 | 310 | /*****************************************************************************/ 311 | 312 | /** Synchronise the distributed clocks 313 | */ 314 | void sync_distributed_clocks(void) 315 | { 316 | #if SYNC_MASTER_TO_REF 317 | uint32_t ref_time = 0; 318 | uint64_t prev_app_time = dc_time_ns; 319 | #endif 320 | 321 | dc_time_ns = system_time_ns(); 322 | 323 | // set master time in nano-seconds 324 | ecrt_master_application_time(master, dc_time_ns); 325 | 326 | #if SYNC_MASTER_TO_REF 327 | // get reference clock time to synchronize master cycle 328 | ecrt_master_reference_clock_time(master, &ref_time); 329 | dc_diff_ns = (uint32_t) prev_app_time - ref_time; 330 | #else 331 | // sync reference clock to master 332 | ecrt_master_sync_reference_clock(master); 333 | #endif 334 | 335 | // call to sync slaves to ref slave 336 | ecrt_master_sync_slave_clocks(master); 337 | } 338 | 339 | /*****************************************************************************/ 340 | 341 | /** Return the sign of a number 342 | * 343 | * ie -1 for -ve value, 0 for 0, +1 for +ve value 344 | * 345 | * \retval the sign of the value 346 | */ 347 | #define sign(val) \ 348 | ({ typeof (val) _val = (val); \ 349 | ((_val > 0) - (_val < 0)); }) 350 | 351 | /*****************************************************************************/ 352 | 353 | /** Update the master time based on ref slaves time diff 354 | * 355 | * called after the ethercat frame is sent to avoid time jitter in 356 | * sync_distributed_clocks() 357 | */ 358 | void update_master_clock(void) 359 | { 360 | #if SYNC_MASTER_TO_REF 361 | // calc drift (via un-normalised time diff) 362 | int32_t delta = dc_diff_ns - prev_dc_diff_ns; 363 | prev_dc_diff_ns = dc_diff_ns; 364 | 365 | // normalise the time diff 366 | dc_diff_ns = 367 | ((dc_diff_ns + (cycle_ns / 2)) % cycle_ns) - (cycle_ns / 2); 368 | 369 | // only update if primary master 370 | if (dc_started) { 371 | 372 | // add to totals 373 | dc_diff_total_ns += dc_diff_ns; 374 | dc_delta_total_ns += delta; 375 | dc_filter_idx++; 376 | 377 | if (dc_filter_idx >= DC_FILTER_CNT) { 378 | // add rounded delta average 379 | dc_adjust_ns += 380 | ((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT); 381 | 382 | // and add adjustment for general diff (to pull in drift) 383 | dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT); 384 | 385 | // limit crazy numbers (0.1% of std cycle time) 386 | if (dc_adjust_ns < -1000) { 387 | dc_adjust_ns = -1000; 388 | } 389 | if (dc_adjust_ns > 1000) { 390 | dc_adjust_ns = 1000; 391 | } 392 | 393 | // reset 394 | dc_diff_total_ns = 0LL; 395 | dc_delta_total_ns = 0LL; 396 | dc_filter_idx = 0; 397 | } 398 | 399 | // add cycles adjustment to time base (including a spot adjustment) 400 | system_time_base += dc_adjust_ns + sign(dc_diff_ns); 401 | } 402 | else { 403 | dc_started = (dc_diff_ns != 0); 404 | 405 | if (dc_started) { 406 | // output first diff 407 | rt_printk("First master diff: %d.\n", dc_diff_ns); 408 | 409 | // record the time of this initial cycle 410 | dc_start_time_ns = dc_time_ns; 411 | } 412 | } 413 | #endif 414 | } 415 | 416 | /****************************************************************************/ 417 | 418 | void rt_check_domain_state(void) 419 | { 420 | ec_domain_state_t ds = {}; 421 | 422 | ecrt_domain_state(domain1, &ds); 423 | 424 | if (ds.working_counter != domain1_state.working_counter) { 425 | rt_printf("Domain1: WC %u.\n", ds.working_counter); 426 | } 427 | 428 | if (ds.wc_state != domain1_state.wc_state) { 429 | rt_printf("Domain1: State %u.\n", ds.wc_state); 430 | } 431 | 432 | domain1_state = ds; 433 | } 434 | 435 | /****************************************************************************/ 436 | 437 | void rt_check_master_state(void) 438 | { 439 | ec_master_state_t ms; 440 | 441 | ecrt_master_state(master, &ms); 442 | 443 | if (ms.slaves_responding != master_state.slaves_responding) { 444 | rt_printf("%u slave(s).\n", ms.slaves_responding); 445 | } 446 | 447 | if (ms.al_states != master_state.al_states) { 448 | rt_printf("AL states: 0x%02X.\n", ms.al_states); 449 | } 450 | 451 | if (ms.link_up != master_state.link_up) { 452 | rt_printf("Link is %s.\n", ms.link_up ? "up" : "down"); 453 | } 454 | 455 | master_state = ms; 456 | } 457 | 458 | /****************************************************************************/ 459 | 460 | /** Wait for the next period 461 | */ 462 | void wait_period(void) 463 | { 464 | while (1) 465 | { 466 | RTIME wakeup_count = system2count(wakeup_time); 467 | RTIME current_count = rt_get_time(); 468 | 469 | if ((wakeup_count < current_count) 470 | || (wakeup_count > current_count + (50 * cycle_ns))) { 471 | rt_printk("%s(): unexpected wake time!\n", __func__); 472 | } 473 | 474 | switch (rt_sleep_until(wakeup_count)) { 475 | case RTE_UNBLKD: 476 | rt_printk("rt_sleep_until(): RTE_UNBLKD\n"); 477 | continue; 478 | 479 | case RTE_TMROVRN: 480 | rt_printk("rt_sleep_until(): RTE_TMROVRN\n"); 481 | overruns++; 482 | 483 | if (overruns % 100 == 0) { 484 | // in case wake time is broken ensure other processes get 485 | // some time slice (and error messages can get displayed) 486 | rt_sleep(cycle_ns / 100); 487 | } 488 | break; 489 | 490 | default: 491 | break; 492 | } 493 | 494 | // done if we got to here 495 | break; 496 | } 497 | 498 | // calc next wake time (in sys time) 499 | wakeup_time += cycle_ns; 500 | } 501 | 502 | /****************************************************************************/ 503 | 504 | void my_cyclic(void) 505 | { 506 | int cycle_counter = 0; 507 | unsigned int blink = 0; 508 | 509 | // oneshot mode to allow adjustable wake time 510 | rt_set_oneshot_mode(); 511 | 512 | // set first wake time in a few cycles 513 | wakeup_time = system_time_ns() + 10 * cycle_ns; 514 | 515 | // start the timer 516 | start_rt_timer(nano2count(cycle_ns)); 517 | 518 | rt_make_hard_real_time(); 519 | 520 | while (run) { 521 | // wait for next period (using adjustable system time) 522 | wait_period(); 523 | 524 | cycle_counter++; 525 | 526 | if (!run) { 527 | break; 528 | } 529 | 530 | // receive EtherCAT 531 | ecrt_master_receive(master); 532 | ecrt_domain_process(domain1); 533 | 534 | rt_check_domain_state(); 535 | 536 | if (!(cycle_counter % 1000)) { 537 | rt_check_master_state(); 538 | } 539 | 540 | if (!(cycle_counter % 200)) { 541 | blink = !blink; 542 | } 543 | 544 | //EC_WRITE_U8(domain1_pd + off_dig_out0, blink ? 0x00 : 0x0FF); //输出到El2008 545 | EC_WRITE_U32(domain1_pd + xmc4800.out_gen_bit1, blink ? 0x55 : 0xaa); //输出到XMC4800_Relax开发板 546 | //EC_WRITE_U16(domain1_pd + off_dig_out0,1); 547 | //fprintf(stdout,"%d\n",EC_READ_U32(domain1_pd+xmc4800.out_gen_bit1)); 548 | // queue process data 549 | ecrt_domain_queue(domain1); 550 | 551 | // sync distributed clock just before master_send to set 552 | // most accurate master clock time 553 | sync_distributed_clocks(); 554 | 555 | // send EtherCAT data 556 | ecrt_master_send(master); 557 | 558 | // update the master clock 559 | // Note: called after ecrt_master_send() to reduce time 560 | // jitter in the sync_distributed_clocks() call 561 | update_master_clock(); 562 | } 563 | 564 | rt_make_soft_real_time(); 565 | stop_rt_timer(); 566 | } 567 | 568 | /**************************************************************************** 569 | * Signal handler 570 | ***************************************************************************/ 571 | 572 | void signal_handler(int sig) 573 | { 574 | run = 0; 575 | } 576 | 577 | /**************************************************************************** 578 | * Main function 579 | ***************************************************************************/ 580 | 581 | int main(int argc, char *argv[]) 582 | { 583 | ec_slave_config_t *sc_ek1100; 584 | ec_slave_config_t *sc_xmc4800_relax; 585 | int ret; 586 | 587 | signal(SIGTERM, signal_handler); 588 | signal(SIGINT, signal_handler); 589 | 590 | mlockall(MCL_CURRENT | MCL_FUTURE); 591 | 592 | printf("Requesting master...\n"); 593 | master = ecrt_request_master(0); 594 | if (!master) { 595 | return -1; 596 | } 597 | 598 | domain1 = ecrt_master_create_domain(master); 599 | if (!domain1) { 600 | return -1; 601 | } 602 | 603 | printf("Creating slave configurations...\n"); 604 | 605 | /* // Create configuration for bus coupler 606 | sc_ek1100 = 607 | ecrt_master_slave_config(master, BusCoupler01_Pos, Beckhoff_EK1100); 608 | if (!sc_ek1100) { 609 | return -1; 610 | }*/ 611 | 612 | /*sc_dig_out_01 = 613 | ecrt_master_slave_config(master, DigOutSlave01_Pos, Beckhoff_EL2004); 614 | if (!sc_dig_out_01) { 615 | fprintf(stderr, "Failed to get slave configuration.\n"); 616 | return -1; 617 | }*/ 618 | 619 | /* sc_dig_out_01 = 620 | ecrt_master_slave_config(master, DigOutSlave01_Pos, Beckhoff_EL2008); 621 | if (!sc_dig_out_01) { 622 | fprintf(stderr, "Failed to get slave configuration.\n"); 623 | return -1; 624 | }*/ 625 | 626 | /*//设置EL2008　eeprom 627 | if (ecrt_slave_config_pdos(sc_dig_out_01, EC_END, slave_1_syncs)) { 628 | fprintf(stderr, "Failed to configure PDOs.\n"); 629 | return -1; 630 | }*/ 631 | 632 | /* //设置PDO映射 633 | if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { 634 | fprintf(stderr, "PDO entry registration failed!\n"); 635 | return -1; 636 | }*/ 637 | 638 | //配置xmc4800_relax 639 | if (!(sc_xmc4800_relax = ecrt_master_slave_config( 640 | master, Xmc4800_relax_Pos, XMC4800_RELAX))) { 641 | fprintf(stderr, "Failed to get slave1 configuration.\n"); 642 | return -1; 643 | } 644 | 645 | printf("Configuring PDOs...\n"); 646 | 647 | if (ecrt_slave_config_pdos(sc_xmc4800_relax, EC_END, XMC4800_syncs)) { 648 | fprintf(stderr, "Failed to configure xmc4800_relax PDOs.\n"); 649 | return -1; 650 | } 651 | 652 | /*a.push_back({Xmc4800_relax_Pos, XMC4800_RELAX, 0x7000, 0x01, &xmc4800.out_gen_int1,NULL}); 653 | a.push_back({Xmc4800_relax_Pos, XMC4800_RELAX, 0x7000, 0x02, &xmc4800.out_gen_int2,NULL}); 654 | a.push_back({Xmc4800_relax_Pos, XMC4800_RELAX, 0x7000, 0x03, &xmc4800.out_gen_int3,NULL}); 655 | a.push_back({Xmc4800_relax_Pos, XMC4800_RELAX, 0x7000, 0x04, &xmc4800.out_gen_int4,NULL}); 656 | a.push_back({Xmc4800_relax_Pos, XMC4800_RELAX, 0x7000, 0x05, &xmc4800.out_gen_bit1,NULL}); 657 | 658 | a.push_back({Xmc4800_relax_Pos, XMC4800_RELAX, 0x6000, 0x01, &xmc4800.in_gen_int1,NULL}); 659 | a.push_back({Xmc4800_relax_Pos, XMC4800_RELAX, 0x6000, 0x02, &xmc4800.in_gen_int2,NULL}); 660 | a.push_back({Xmc4800_relax_Pos, XMC4800_RELAX, 0x6000, 0x03, &xmc4800.in_gen_int3,NULL}); 661 | a.push_back({Xmc4800_relax_Pos, XMC4800_RELAX, 0x6000, 0x04, &xmc4800.in_gen_int4,NULL}); 662 | a.push_back({Xmc4800_relax_Pos, XMC4800_RELAX, 0x6000, 0x05, &xmc4800.in_gen_bit1,NULL}); 663 | a.push_back({});*/ 664 | 665 | //动态生成domain1_regs数组 666 | domain1_regs = new ec_pdo_entry_reg_t[11](); 667 | 668 | domain1_regs[0]={SLAVE_POSITION(0,0),SLAVE_VENDORID(INFINEON_ID,XMC4800_RELAX_PRODUCT_CODE1), 0x7000, 0x01, &xmc4800.out_gen_int1,NULL}; 669 | domain1_regs[1]={SLAVE_POSITION(0,0),SLAVE_VENDORID(INFINEON_ID,XMC4800_RELAX_PRODUCT_CODE1), 0x7000, 0x02, &xmc4800.out_gen_int2,NULL}; 670 | domain1_regs[2]={SLAVE_POSITION(0,0),SLAVE_VENDORID(INFINEON_ID,XMC4800_RELAX_PRODUCT_CODE1), 0x7000, 0x03, &xmc4800.out_gen_int3,NULL}; 671 | domain1_regs[3]={SLAVE_POSITION(0,0),SLAVE_VENDORID(INFINEON_ID,XMC4800_RELAX_PRODUCT_CODE1), 0x7000, 0x04, &xmc4800.out_gen_int4,NULL}; 672 | domain1_regs[4]={SLAVE_POSITION(0,0),SLAVE_VENDORID(INFINEON_ID,XMC4800_RELAX_PRODUCT_CODE1), 0x7000, 0x05, &xmc4800.out_gen_bit1,NULL}; 673 | 674 | domain1_regs[5]={SLAVE_POSITION(0,0),SLAVE_VENDORID(INFINEON_ID,XMC4800_RELAX_PRODUCT_CODE1), 0x6000, 0x01, &xmc4800.in_gen_int1,NULL}; 675 | domain1_regs[6]={SLAVE_POSITION(0,0),SLAVE_VENDORID(INFINEON_ID,XMC4800_RELAX_PRODUCT_CODE1), 0x6000, 0x02, &xmc4800.in_gen_int2,NULL}; 676 | domain1_regs[7]={SLAVE_POSITION(0,0),SLAVE_VENDORID(INFINEON_ID,XMC4800_RELAX_PRODUCT_CODE1), 0x6000, 0x03, &xmc4800.in_gen_int3,NULL}; 677 | domain1_regs[8]={SLAVE_POSITION(0,0),SLAVE_VENDORID(INFINEON_ID,XMC4800_RELAX_PRODUCT_CODE1), 0x6000, 0x04, &xmc4800.in_gen_int4,NULL}; 678 | domain1_regs[9]={SLAVE_POSITION(0,0),SLAVE_VENDORID(INFINEON_ID,XMC4800_RELAX_PRODUCT_CODE1), 0x6000, 0x05, &xmc4800.in_gen_bit1,NULL}; 679 | domain1_regs[10]={}; 680 | 681 | if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { 682 | fprintf(stderr, "xmc4800_relax PDO entry registration failed!\n"); 683 | return -1; 684 | } 685 | 686 | /*Registers a PDO entry for process data exchange in domain*/ 687 | /* if(ecrt_slave_config_reg_pdo_entry(sc_xmc4800_relax, 0x7000, 0x05, domain1, &xmc4800.out_gen_bit1) < 0) { 688 | fprintf(stderr,"PDO entry (0x7000:0x05) registration failed!\n"); 689 | return -1; 690 | }*/ 691 | 692 | //配置xmc4800_relax结束 693 | /* Set the initial master time and select a slave to use as the DC 694 | * reference clock, otherwise pass NULL to auto select the first capable 695 | * slave. Note: This can be used whether the master or the ref slave will 696 | * be used as the systems master DC clock. 697 | */ 698 | dc_start_time_ns = system_time_ns(); 699 | dc_time_ns = dc_start_time_ns; 700 | 701 | /* Attention: The initial application time is also used for phase 702 | * calculation for the SYNC0/1 interrupts. Please be sure to call it at 703 | * the correct phase to the realtime cycle. 704 | */ 705 | ecrt_master_application_time(master, dc_start_time_ns); 706 | 707 | ret = ecrt_master_select_reference_clock(master, sc_xmc4800_relax/*sc_ek1100*/); 708 | if (ret < 0) { 709 | fprintf(stderr, "Failed to select reference clock: %s\n", 710 | strerror(-ret)); 711 | return ret; 712 | } 713 | 714 | printf("Activating master...\n"); 715 | if (ecrt_master_activate(master)) { 716 | return -1; 717 | } 718 | 719 | if (!(domain1_pd = ecrt_domain_data(domain1))) { 720 | fprintf(stderr, "Failed to get domain data pointer.\n"); 721 | return -1; 722 | } 723 | 724 | /*ecrt_slave_config_sdo8(sc_xmc4800_relax,0x7000,0x0c,1); 725 | ecrt_slave_config_sdo32(sc_xmc4800_relax,0x7000,1,1); 726 | ecrt_slave_config_sdo8(sc_xmc4800_relax,0x7000,0x5,1); 727 | ecrt_slave_config_sdo8(sc_xmc4800_relax,0x7000,0x6,1);*/ 728 | 729 | /* Create cyclic RT-thread */ 730 | struct sched_param param; 731 | param.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1; 732 | if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { 733 | puts("ERROR IN SETTING THE SCHEDULER"); 734 | perror("errno"); 735 | return -1; 736 | } 737 | 738 | task = rt_task_init(nam2num("ec_rtai_rtdm_example"), 739 | 0 /* priority */, 0 /* stack size */, 0 /* msg size */); 740 | 741 | my_cyclic(); 742 | 743 | rt_task_delete(task); 744 | 745 | printf("End of Program\n"); 746 | ecrt_release_master(master); 747 | 748 | delete[] domain1_regs; 749 | 750 | return 0; 751 | } 752 | 753 | /***********************************NO MORE**********************************/ 754 | 755 | --------------------------------------------------------------------------------