int EtherCatServer::configServer(std::string configFile) { // 1. Configure the system master = ecrt_request_master(0); if (!master) return -1; // load up the configuration slaves = configLoader.loadConfiguration(master, configFile); domain1 = ecrt_master_create_domain(master); if (!domain1) return -1; // load and apply configurations configLoader.applyConfiguration(master, domain1, &slaves); printf("Activating master...\n"); if (ecrt_master_activate(master)) return -1; if (!(domain1_pd = ecrt_domain_data(domain1))) { return -1; } return 0; };
int __init init_mini_module(void) { int ret = -1; ec_slave_config_t *sc; printk(KERN_INFO PFX "Starting...\n"); master = ecrt_request_master(0); if (!master) { printk(KERN_ERR PFX "Requesting master 0 failed.\n"); ret = -EBUSY; goto out_return; } sema_init(&master_sem, 1); ecrt_master_callbacks(master, send_callback, receive_callback, master); printk(KERN_INFO PFX "Registering domain...\n"); if (!(domain1 = ecrt_master_create_domain(master))) { printk(KERN_ERR PFX "Domain creation failed!\n"); goto out_release_master; } // Create configuration for bus coupler sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100); if (!sc) { printk(KERN_ERR PFX "Failed to create slave config.\n"); ret = -ENOMEM; goto out_release_master; } create_serial_devices(master, domain1); printk(KERN_INFO PFX "Activating master...\n"); if (ecrt_master_activate(master)) { printk(KERN_ERR PFX "Failed to activate master!\n"); goto out_free_serial; } // Get internal process data for domain domain1_pd = ecrt_domain_data(domain1); printk(KERN_INFO PFX "Starting cyclic sample thread.\n"); init_timer(&timer); timer.function = cyclic_task; timer.expires = jiffies + 10; add_timer(&timer); printk(KERN_INFO PFX "Started.\n"); return 0; out_free_serial: free_serial_devices(); out_release_master: printk(KERN_ERR PFX "Releasing master...\n"); ecrt_release_master(master); out_return: printk(KERN_ERR PFX "Failed to load. Aborting.\n"); return ret; }
int main(int argc, char **argv) { ec_slave_config_t *sc; struct sigaction sa; struct itimerval tv; master = ecrt_request_master(0); if (!master) return -1; domain1 = ecrt_master_create_domain(master); if (!domain1) return -1; if (!(sc_ana_in = ecrt_master_slave_config( master, AnaInSlavePos, Beckhoff_EL3102))) { fprintf(stderr, "Failed to get slave configuration.\n"); return -1; } #if SDO_ACCESS fprintf(stderr, "Creating SDO requests...\n"); if (!(sdo = ecrt_slave_config_create_sdo_request(sc_ana_in, 0x3102, 2, 2))) { fprintf(stderr, "Failed to create SDO request.\n"); return -1; } ecrt_sdo_request_timeout(sdo, 500); // ms #endif #if CONFIGURE_PDOS printf("Configuring PDOs...\n"); if (ecrt_slave_config_pdos(sc_ana_in, EC_END, el3102_syncs)) { fprintf(stderr, "Failed to configure PDOs.\n"); return -1; } if (!(sc = ecrt_master_slave_config( master, AnaOutSlavePos, Beckhoff_EL4102))) { fprintf(stderr, "Failed to get slave configuration.\n"); return -1; } if (ecrt_slave_config_pdos(sc, EC_END, el4102_syncs)) { fprintf(stderr, "Failed to configure PDOs.\n"); return -1; } if (!(sc = ecrt_master_slave_config( master, DigOutSlavePos, Beckhoff_EL2032))) { fprintf(stderr, "Failed to get slave configuration.\n"); return -1; } if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) { fprintf(stderr, "Failed to configure PDOs.\n"); return -1; } #endif // Create configuration for bus coupler sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100); if (!sc) return -1; if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { fprintf(stderr, "PDO entry registration failed!\n"); return -1; } printf("Activating master...\n"); if (ecrt_master_activate(master)) return -1; if (!(domain1_pd = ecrt_domain_data(domain1))) { return -1; } #if PRIORITY pid_t pid = getpid(); if (setpriority(PRIO_PROCESS, pid, -19)) fprintf(stderr, "Warning: Failed to set priority: %s\n", strerror(errno)); #endif sa.sa_handler = signal_handler; sigemptyset(&sa.sa_mask); sa.sa_flags = 0; if (sigaction(SIGALRM, &sa, 0)) { fprintf(stderr, "Failed to install signal handler!\n"); return -1; } printf("Starting timer...\n"); tv.it_interval.tv_sec = 0; tv.it_interval.tv_usec = 1000000 / FREQUENCY; tv.it_value.tv_sec = 0; tv.it_value.tv_usec = 1000; if (setitimer(ITIMER_REAL, &tv, NULL)) { fprintf(stderr, "Failed to start timer: %s\n", strerror(errno)); return 1; } printf("Started.\n"); while (1) { pause(); #if 0 struct timeval t; gettimeofday(&t, NULL); printf("%u.%06u\n", t.tv_sec, t.tv_usec); #endif while (sig_alarms != user_alarms) { cyclic_task(); user_alarms++; } } return 0; }
int main(int argc, char **argv) { // ec_slave_config_t *sc; struct sigaction sa; struct itimerval tv; master = ecrt_request_master(0); if (!master) return -1; domain1 = ecrt_master_create_domain(master); if (!domain1) { return -1; } if (!(sc_ana_in = ecrt_master_slave_config(master, AliasAndPositon, VendorID_ProductCode))) { fprintf(stderr, "Failed to get slave configuration.\n"); return -1; } #if SDO_ACCESS fprintf(stderr, "Creating operation mode read SDO requests...\n"); if (!(sdo_operation_mode_display = ecrt_slave_config_create_sdo_request(sc_ana_in, MODES_OF_OPERATION_DISPLAY, 0, 1))) // uint8 data size 1 { fprintf(stderr, "Failed to create SDO modes_of_operation_display 0x6061 request.\n"); return -1; } fprintf(stderr, "Creating operation mode write SDO requests...\n"); if (!(sdo_operation_mode_write = ecrt_slave_config_create_sdo_request(sc_ana_in, MODES_OF_OPERATION, 0, 1))) // uint8 data size 1 { fprintf(stderr, "Failed to create SDO MODES_OF_OPERATION request.\n"); return -1; } fprintf(stderr, "Creating controlword write SDO requests...\n"); if (!(sdo_controlword_write = ecrt_slave_config_create_sdo_request(sc_ana_in, CONTROLWORD, 0, 2))) // uint16 data size 2 { fprintf(stderr, "Failed to create SDO CONTROLWORD request.\n"); return -1; } fprintf(stderr, "Creating statusword read SDO requests...\n"); if (!(sdo_statusword_read = ecrt_slave_config_create_sdo_request(sc_ana_in, STATUSWORD, 0, 2))) // uint16 data size 2 { fprintf(stderr, "Failed to create SDO STATUSWORD request.\n"); return -1; } //EC_WRITE_U16(ecrt_sdo_request_data(sdo), 0xFFFF); ecrt_sdo_request_timeout(sdo_operation_mode_display, 500); // ms ecrt_sdo_request_timeout(sdo_operation_mode_write, 500); // ms ecrt_sdo_request_timeout(sdo_controlword_write, 500); // ms ecrt_sdo_request_timeout(sdo_statusword_read, 500); // ms #endif #if CONFIGURE_PDOS printf("Configuring PDOs...\n"); if (ecrt_slave_config_pdos(sc_ana_in, EC_END, duetfl80_syncs)) { fprintf(stderr, "Failed to configure PDOs.\n"); return -1; } // if (!(sc = ecrt_master_slave_config( // master, AnaOutSlavePos, Beckhoff_EL4102))) { // fprintf(stderr, "Failed to get slave configuration.\n"); // return -1; // } // if (ecrt_slave_config_pdos(sc, EC_END, el4102_syncs)) { // fprintf(stderr, "Failed to configure PDOs.\n"); // return -1; // } // if (!(sc = ecrt_master_slave_config( // master, DigOutSlavePos, Beckhoff_EL2032))) { // fprintf(stderr, "Failed to get slave configuration.\n"); // return -1; // } // if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) { // fprintf(stderr, "Failed to configure PDOs.\n"); // return -1; // } // // Create configuration for bus coupler // sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100); // if (!sc) // return -1; if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { fprintf(stderr, "PDO entry registration failed!\n"); return -1; } #endif printf("Activating master...\n"); if (ecrt_master_activate(master)) return -1; #if 1 if (!(domain1_pd = ecrt_domain_data(domain1))) { return -1; } #endif #if PRIORITY pid_t pid = getpid(); if (setpriority(PRIO_PROCESS, pid, -19)) fprintf(stderr, "Warning: Failed to set priority: %s\n", strerror(errno)); #endif printf("Starting timer...\n"); tv.it_interval.tv_sec = 0; tv.it_interval.tv_usec = 1000000 / FREQUENCY; tv.it_value.tv_sec = 0; tv.it_value.tv_usec = 1000; if (setitimer(ITIMER_REAL, &tv, NULL)) { fprintf(stderr, "Failed to start timer: %s\n", strerror(errno)); return 1; } // handle ctrl+c ,important , do not remove sa.sa_handler = signal_handler; sigemptyset(&sa.sa_mask); sa.sa_flags = 0; if (signal(SIGINT, my_sig_handler) == SIG_ERR) { printf("\ncan't catch SIGUSR1\n"); return -1; } if (sigaction(SIGALRM, &sa, 0)) { fprintf(stderr, "Failed to install signal handler!\n"); return -1; } // 1. check operation mode bool getModeOk=false; uint8_t mode_value; int i=0; while(1) { // printf("i=%d\n",i); i++; // receive process data ecrt_master_receive(master); // ecrt_domain_process(domain1); // check process data state (optional) // check_domain1_state(); // check for master state (optional) check_master_state(); // check for islave configuration state(s) (optional) check_slave_config_states(); // read_sdo(); if(i==1) { ecrt_sdo_request_read(sdo_operation_mode_display); // trigger read } switch (ecrt_sdo_request_state(sdo_operation_mode_display)) { case EC_REQUEST_UNUSED: // request was not used yet printf("request was not used yet\n"); break; case EC_REQUEST_BUSY: // printf( "Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo_operation_mode_display value: 0x%02X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display))); getModeOk = true; mode_value = EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display)); break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(getModeOk) { printf("get mode value <%02x>\n",mode_value); break; } // ecrt_domain_queue(domain1); ecrt_master_send(master); // pause(); // sleep(1); // cyclic_task(); } printf("check mode done\n"); if(getModeOk == false) { exit(-1); } // exit(0); // 2. set operation mode to velocity mode printf("setting mode to velocity_mode...\n"); bool isWriteModeOk=false; while(1) { ecrt_master_receive(master); EC_WRITE_U8(ecrt_sdo_request_data(sdo_operation_mode_write), 0x03); ecrt_sdo_request_write(sdo_operation_mode_write); switch (ecrt_sdo_request_state(sdo_operation_mode_write)) { case EC_REQUEST_UNUSED: // request was not used yet // ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // printf("Request to Write,But Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo_operation_mode_write write value 0x03 ok\n"); // ecrt_sdo_request_write(sdo_operation_mode_write); isWriteModeOk = true; break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO state!\n"); break; } if(isWriteModeOk) { break; } ecrt_master_send(master); } // 3. check operation mode, after write getModeOk=false; // for(int i=0;i<10;++i) i=0; while(1) { i++; ecrt_master_receive(master); if(i==1) { ecrt_sdo_request_read(sdo_operation_mode_display); // trigger read } switch (ecrt_sdo_request_state(sdo_operation_mode_display)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // fprintf(stderr, "Still busy...\n"); break; case EC_REQUEST_SUCCESS: printf("sdo_operation_mode_display value: 0x%02X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display))); if(EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display)) ^ 0x03 == 0) { printf("mode is in velocity_mode \n"); getModeOk = true; } else { printf("mode not in velocity_mode \n"); exit(-1); } break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(getModeOk) { break; } // if(i<10) { ecrt_master_send(master); } // sleep(1); } // 4. read target velocity while(1) { i++; ecrt_master_receive(master); if(i==1) { ecrt_sdo_request_read(sdo_operation_mode_display); // trigger read } switch (ecrt_sdo_request_state(sdo_operation_mode_display)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // fprintf(stderr, "Still busy...\n"); break; case EC_REQUEST_SUCCESS: printf("sdo_operation_mode_display value: 0x%02X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display))); if(EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display)) ^ 0x03 == 0) { printf("mode is in velocity_mode \n"); getModeOk = true; } else { printf("mode not in velocity_mode \n"); exit(-1); } break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(getModeOk) { break; } ecrt_master_send(master); } // 5. set target velocity to zero // 6. read statusword printf("6.read statusword...\n"); bool isOperationDisabled=false; // while(!isOperationDisabled) while(1) { ecrt_master_receive(master); EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x0080);// reset from fault ecrt_sdo_request_write(sdo_controlword_write); switch (ecrt_sdo_request_state(sdo_controlword_write)) { case EC_REQUEST_UNUSED: // request was not used yet // ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: printf("Request to Write,But Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo_controlword_write write value 0x0080\n"); ecrt_sdo_request_write(sdo_controlword_write); break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO state!\n"); break; } ecrt_master_send(master); // read state ,need switch_on_disabled ecrt_sdo_request_read(sdo_statusword_read); // trigger read switch (ecrt_sdo_request_state(sdo_statusword_read)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(sdo_statusword_read); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: printf("Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "statusword value: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read))); if(EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x004F ^ 0x0040 == 0) // p91 { isOperationDisabled = true; printf("motor state is in switch_on_disabled \n"); } break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(isOperationDisabled) { printf("good \n"); break; } ecrt_master_send(master); } // 7. set controlword to ready_to_switch_on printf("7. set controlword , ready_to_switch_on...\n"); isOperationDisabled = false; while(1) { ecrt_master_receive(master); EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x0006);// reset from fault ecrt_sdo_request_write(sdo_controlword_write); switch (ecrt_sdo_request_state(sdo_controlword_write)) { case EC_REQUEST_UNUSED: // request was not used yet // ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // printf("Request to Write,But Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo_controlword_write write value 0x0006\n"); ecrt_sdo_request_write(sdo_controlword_write); break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO state!\n"); break; } ecrt_master_send(master); // read state ,need switch_on_disabled ecrt_sdo_request_read(sdo_statusword_read); // trigger read switch (ecrt_sdo_request_state(sdo_statusword_read)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(sdo_statusword_read); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // printf("Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "statusword value: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read))); if(EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x006F ^ 0x0021 == 0) // p91 { isOperationDisabled = true; printf("motor state is in ready_to_switch_on \n"); } break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(isOperationDisabled) { printf("good2 \n"); break; } ecrt_master_send(master); } // 8. set controlword , enable operation printf("8. set controlword , enable operation...\n"); isOperationDisabled = false; while(1) { ecrt_master_receive(master); EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x000F);// reset from fault ecrt_sdo_request_write(sdo_controlword_write); switch (ecrt_sdo_request_state(sdo_controlword_write)) { case EC_REQUEST_UNUSED: // request was not used yet // ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // printf("Request to Write,But Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo_controlword_write write value 0x000F\n"); ecrt_sdo_request_write(sdo_controlword_write); break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO state!\n"); break; } ecrt_master_send(master); // read state ,need switch_on_disabled ecrt_sdo_request_read(sdo_statusword_read); // trigger read switch (ecrt_sdo_request_state(sdo_statusword_read)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(sdo_statusword_read); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // printf("Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "statusword value: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read))); fprintf(stderr, "statusword value aa: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x006F); fprintf(stderr, "statusword value aaa: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x006F^ 0x0027); if(EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x006F == 0x0027) // p91 { isOperationDisabled = true; printf("motor state is in operation_on \n"); } break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(isOperationDisabled) { printf("good2 \n"); break; } ecrt_master_send(master); } // 8. set target velocity 100r/min // sa.sa_handler = signal_handler; // sigemptyset(&sa.sa_mask); // sa.sa_flags = 0; // if (sigaction(SIGALRM, &sa, 0)) { // fprintf(stderr, "Failed to install signal handler!\n"); // return -1; // } // printf("Started.\n"); // while (1) { // pause(); //#if 0 // struct timeval t; // gettimeofday(&t, NULL); // printf("%u.%06u\n", t.tv_sec, t.tv_usec); //#endif // while (sig_alarms != user_alarms) { // cyclic_task(); // user_alarms++; // } // } return 0; }
int __init init_mod(void) { int ret = -1, i; RTIME tick_period, requested_ticks, now; ec_slave_config_t *sc; printk(KERN_INFO PFX "Starting...\n"); rt_sem_init(&master_sem, 1); t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000; master = ecrt_request_master(0); if (!master) { ret = -EBUSY; printk(KERN_ERR PFX "Requesting master 0 failed!\n"); goto out_return; } ecrt_master_callbacks(master, send_callback, receive_callback, master); printk(KERN_INFO PFX "Registering domain...\n"); if (!(domain1 = ecrt_master_create_domain(master))) { printk(KERN_ERR PFX "Domain creation failed!\n"); goto out_release_master; } // if (!(domain2 = ecrt_master_create_domain(master))) { // printk(KERN_ERR PFX "Domain2 creation failed!\n"); // goto out_release_master; // } printk(KERN_INFO PFX "Configuring PDOs...\n"); if (!(sc = ecrt_master_slave_config(master, yas, yaskawa))) { printk(KERN_ERR PFX "Failed to get slave configuration.\n"); goto out_release_master; } // ecrt_slave_config_dc(sc, 0x300, 1000000, 0, 0, 0); if (ecrt_slave_config_sdo8(sc, 0x6060, 0, 8)){ printk(KERN_ERR PFX "Failed to configure SDOs.\n"); goto out_release_master; } if (ecrt_slave_config_pdos(sc, EC_END, slave_0_syncs)) { printk(KERN_ERR PFX "Failed to configure PDOs.\n"); goto out_release_master; } if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { printk(KERN_ERR PFX "1st motor RX_PDO entry registration failed!\n"); goto out_release_master; } printk(KERN_INFO PFX "Activating master...\n"); if (ecrt_master_activate(master)) { printk(KERN_ERR PFX "Failed to activate master!\n"); goto out_release_master; } // Get internal process data for domain domain1_pd = ecrt_domain_data(domain1); // domain2_pd = ecrt_domain_data(domain2); printk(KERN_INFO PFX "Starting cyclic sample thread...\n"); requested_ticks = nano2count(TIMERTICKS); tick_period = start_rt_timer(requested_ticks); printk(KERN_INFO PFX "RT timer started with %i/%i ticks.\n", (int) tick_period, (int) requested_ticks); if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) { printk(KERN_ERR PFX "Failed to init RTAI task!\n"); goto out_stop_timer; } now = rt_get_time(); if (rt_task_make_periodic(&task, now + tick_period, tick_period)) { printk(KERN_ERR PFX "Failed to run RTAI task!\n"); goto out_stop_task; } printk(KERN_INFO PFX "Initialized.\n"); return 0; out_stop_task: rt_task_delete(&task); out_stop_timer: stop_rt_timer(); out_release_master: printk(KERN_ERR PFX "Releasing master...\n"); ecrt_release_master(master); out_return: rt_sem_delete(&master_sem); printk(KERN_ERR PFX "Failed to load. Aborting.\n"); return ret; }
int main(int argc, char **argv) { ec_slave_config_t *sc; if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { perror("mlockall failed"); return -1; } master = ecrt_request_master(0); if (!master) return -1; domain_r = ecrt_master_create_domain(master); if (!domain_r) return -1; domain_w = ecrt_master_create_domain(master); if (!domain_w) return -1; if (!(sc = ecrt_master_slave_config(master, pana, panasonic))) { fprintf(stderr, "Failed to get slave1 configuration.\n"); return -1; } #if SDO_ACCESS if (ecrt_slave_config_sdo8(sc, 0x6060, 0, 8)) { return -1; } #endif #if CONFIGURE_PDOS printf("Configuring PDOs...\n"); if (ecrt_slave_config_pdos(sc, EC_END, slave_0_syncs)) { fprintf(stderr, "Failed to configure 1st PDOs.\n"); return -1; } #endif if (ecrt_domain_reg_pdo_entry_list(domain_r, domain_r_regs)) { fprintf(stderr, "1st motor RX_PDO entry registration failed!\n"); return -1; } if (ecrt_domain_reg_pdo_entry_list(domain_w, domain_w_regs)) { fprintf(stderr, "1st motor TX_PDO entry registration failed!\n"); return -1; } ecrt_slave_config_dc(sc,0x0300,500000,0,0,0); printf("Activating master...\n"); if (ecrt_master_activate(master)) return -1; if (!(domain_r_pd = ecrt_domain_data(domain_r))) { return -1; } if (!(domain_w_pd = ecrt_domain_data(domain_w))) { return -1; } pid_t pid = getpid(); if (setpriority(PRIO_PROCESS, pid, -20)) fprintf(stderr, "Warning: Failed to set priority: %s\n", strerror(errno)); signal( SIGINT , endsignal ); printf("Starting cyclic function.\n"); cyclic_task(); ecrt_release_master(master); return 0; }
void init_master(master_setup_variables_t *master_setup, ctrlproto_slv_handle *slv_handles, unsigned int total_no_of_slaves) { int slv; struct sigaction sa; struct itimerval tv; master_setup->master = ecrt_request_master(0); if (!master_setup->master) exit(-1); master_setup->domain = ecrt_master_create_domain(master_setup->master); if (!master_setup->domain) exit(-1); for (slv = 0; slv < total_no_of_slaves; ++slv) { if (!( slv_handles[slv].slave_config= ecrt_master_slave_config( //sc_data_in master_setup->master, slv_handles[slv].slave_alias, slv_handles[slv].slave_pos , slv_handles[slv].slave_vendorid, slv_handles[slv].slave_productid))) { fprintf(stderr, "Failed to get slave configuration.\n"); exit(-1); } //logmsg(1, "Configuring PDOs...\n"); if (ecrt_slave_config_pdos(slv_handles[slv].slave_config, EC_END, slv_handles[slv].__sync_info)) { //slave_0_syncs fprintf(stderr, "Failed to configure PDOs.\n"); exit(-1); } //#if PARAMETER_UPDATE motor_config_request(slv_handles[slv].slave_config, slv_handles[slv].__request); //#endif } if (ecrt_domain_reg_pdo_entry_list(master_setup->domain, master_setup->domain_regs)) { fprintf(stderr, "PDO entry registration failed!\n"); exit(-1); } if (ecrt_master_set_send_interval(master_setup->master, FREQUENCY) != 0) { fprintf(stderr, "failed to set send interval\n"); exit(-1); } logmsg(1, "Activating master...\n"); if (ecrt_master_activate(master_setup->master)) exit(-1); if (!(master_setup->domain_pd = ecrt_domain_data(master_setup->domain))) { exit(-1); } pid_t pid = getpid(); if (setpriority(PRIO_PROCESS, pid, -19)) fprintf(stderr, "Warning: Failed to set priority: %s\n", strerror(errno)); sa.sa_handler = signal_handler; sigemptyset(&sa.sa_mask); sa.sa_flags = 0; if (sigaction(SIGALRM, &sa, 0)) { fprintf(stderr, "Failed to install signal handler!\n"); exit(-1); } //logmsg(1, "Starting timer...\n"); tv.it_interval.tv_sec = 0; tv.it_interval.tv_usec = 1000000 / FREQUENCY; tv.it_value.tv_sec = 0; tv.it_value.tv_usec = 1000; if (setitimer(ITIMER_REAL, &tv, NULL)) { fprintf(stderr, "Failed to start timer: %s\n", strerror(errno)); exit(-1); } logmsg(0, "Started.\n"); }
int main(int argc, char **argv) { // ec_slave_config_t *sc; struct sigaction sa; struct itimerval tv; master = ecrt_request_master(0); if (!master) return -1; domain1 = ecrt_master_create_domain(master); if (!domain1) { return -1; } if (!(sc_ana_in = ecrt_master_slave_config(master, AliasAndPositon, VendorID_ProductCode))) { fprintf(stderr, "Failed to get slave configuration.\n"); return -1; } // printf("sync mgr 0 config: %d \n",sc_ana_in->sync_configs[0].dir); // printf("sync mgr 1 config: %d \n",sc_ana_in->sync_configs[1].dir); #if SDO_ACCESS fprintf(stderr, "Creating SDO requests...\n"); if (!(sdo = ecrt_slave_config_create_sdo_request(sc_ana_in, 0x6061, 0, 1))) // data size 1 ? { fprintf(stderr, "Failed to create SDO modes_of_operation_display 0x6061 request.\n"); return -1; } //EC_WRITE_U16(ecrt_sdo_request_data(sdo), 0xFFFF); ecrt_sdo_request_timeout(sdo, 500); // ms #endif #if CONFIGURE_PDOS printf("Configuring PDOs...\n"); if (ecrt_slave_config_pdos(sc_ana_in, EC_END, duetfl80_syncs)) { fprintf(stderr, "Failed to configure PDOs.\n"); return -1; } // if (!(sc = ecrt_master_slave_config( // master, AnaOutSlavePos, Beckhoff_EL4102))) { // fprintf(stderr, "Failed to get slave configuration.\n"); // return -1; // } // if (ecrt_slave_config_pdos(sc, EC_END, el4102_syncs)) { // fprintf(stderr, "Failed to configure PDOs.\n"); // return -1; // } // if (!(sc = ecrt_master_slave_config( // master, DigOutSlavePos, Beckhoff_EL2032))) { // fprintf(stderr, "Failed to get slave configuration.\n"); // return -1; // } // if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) { // fprintf(stderr, "Failed to configure PDOs.\n"); // return -1; // } #endif // // Create configuration for bus coupler // sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100); // if (!sc) // return -1; if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { fprintf(stderr, "PDO entry registration failed!\n"); return -1; } printf("Activating master...\n"); if (ecrt_master_activate(master)) return -1; if (!(domain1_pd = ecrt_domain_data(domain1))) { return -1; } #if PRIORITY pid_t pid = getpid(); if (setpriority(PRIO_PROCESS, pid, -19)) fprintf(stderr, "Warning: Failed to set priority: %s\n", strerror(errno)); #endif sa.sa_handler = signal_handler; sigemptyset(&sa.sa_mask); sa.sa_flags = 0; if (sigaction(SIGALRM, &sa, 0)) { fprintf(stderr, "Failed to install signal handler!\n"); return -1; } printf("Starting timer...\n"); // tv.it_interval.tv_sec = 0; // tv.it_interval.tv_usec = 1000000 / FREQUENCY; // tv.it_value.tv_sec = 0; // tv.it_value.tv_usec = 1000; // if (setitimer(ITIMER_REAL, &tv, NULL)) { // fprintf(stderr, "Failed to start timer: %s\n", strerror(errno)); // return 1; // } printf("Started.\n"); while (1) { // pause(); #if 1 // for(int i=0;i<10;++i) // { // my_cyclic_task(); // receive process data ecrt_master_receive(master); // ecrt_domain_process(domain1); // check process data state (optional) // check_domain1_state(); // check for master state (optional) check_master_state(); // check for islave configuration state(s) (optional) check_slave_config_states(); // read process data SDO switch (ecrt_sdo_request_state(sdo)) { case EC_REQUEST_UNUSED: // request was not used yet printf("request was not used yet\n"); ecrt_sdo_request_read(sdo); // trigger first read // ecrt_sdo_request_write(sdo); break; case EC_REQUEST_BUSY: // fprintf(stderr, "Still busy...\n"); break; case EC_REQUEST_SUCCESS: printf( "SDO value: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo))); ecrt_sdo_request_read(sdo); // trigger next read break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); ecrt_sdo_request_read(sdo); // retry reading break; } // read_sdo(); // switch (ecrt_sdo_request_state(sdo)) { // case EC_REQUEST_UNUSED: // request was not used yet // printf("request was not used yet\n"); // ecrt_sdo_request_read(sdo); // trigger first read // break; // case EC_REQUEST_BUSY: // fprintf(stderr, "Still busy...\n"); // break; // case EC_REQUEST_SUCCESS: // fprintf(stderr, "sdo value: 0x%04X\n", // EC_READ_U8(ecrt_sdo_request_data(sdo))); //// getModeOk = true; // break; // case EC_REQUEST_ERROR: // fprintf(stderr, "Failed to read SDO!\n"); // break; // } // send process data // ecrt_domain_queue(domain1); // ecrt_sdo_request_read(sdo); // trigger next read ecrt_master_send(master); ecrt_master_receive(master); // sleep(1); // } #else // 1. check operation mode bool getModeOk=false; for(int i=0;i<10;++i) { printf("i=%d\n",i); // receive process data ecrt_master_receive(master); // ecrt_domain_process(domain1); // check process data state (optional) // check_domain1_state(); // check for master state (optional) check_master_state(); // check for islave configuration state(s) (optional) check_slave_config_states(); // read_sdo(); // ecrt_sdo_request_read(sdo); // trigger read switch (ecrt_sdo_request_state(sdo)) { case EC_REQUEST_UNUSED: // request was not used yet printf("request was not used yet\n"); ecrt_sdo_request_read(sdo); // trigger first read break; case EC_REQUEST_BUSY: fprintf(stderr, "Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo value: 0x%04X\n", EC_READ_U8(ecrt_sdo_request_data(sdo))); getModeOk = true; break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(getModeOk) { break; } // ecrt_domain_queue(domain1); ecrt_master_send(master); // sleep(1); // cyclic_task(); } if(getModeOk == false) { // exit(-1); } #endif } return 0; }
int main(int argc, char **argv) { cmdline(argc, argv); ec_slave_config_t *sc; struct sigaction sa; struct itimerval tv; master = ecrt_request_master(0); if (!master) return -1; domain1 = ecrt_master_create_domain(master); if (!domain1) return -1; if (!(sc_data_in = ecrt_master_slave_config( master, SomanetPos, SOMANET_ID))) { fprintf(stderr, "Failed to get slave configuration.\n"); return -1; } #if SDO_ACCESS fprintf(stderr, "Creating SDO requests...\n"); if (!(sdo = ecrt_slave_config_create_sdo_request(sc_data_in, 0x6041, 0, 1))) { fprintf(stderr, "Failed to create SDO request.\n"); return -1; } ecrt_sdo_request_timeout(sdo, 500); // ms if (!(request[0] = ecrt_slave_config_create_sdo_request(sc_data_in, CAN_OD_POS_VALUE, 0, 4))) { fprintf(stderr, "Failed to create SDO request for object 0x%4x\n", CAN_OD_POS_VALUE); return -1; } ecrt_sdo_request_timeout(request[0], 500); // ms if (!(request[1] = ecrt_slave_config_create_sdo_request(sc_data_in, CAN_OD_VEL_VALUE, 0, 4))) { fprintf(stderr, "Failed to create SDO request for object 0x%4x\n", CAN_OD_VEL_VALUE); return -1; } ecrt_sdo_request_timeout(request[1], 500); // ms if (!(request[2] = ecrt_slave_config_create_sdo_request(sc_data_in, CAN_OD_TOR_VALUE, 0, 2))) { fprintf(stderr, "Failed to create SDO request for object 0x%4x\n", CAN_OD_TOR_VALUE); return -1; } ecrt_sdo_request_timeout(request[2], 500); // ms /* register sdo download request */ if (!(sdo_download_requests[0] = ecrt_slave_config_create_sdo_request(sc_data_in, CAN_OD_MODES, 0, 4))) { fprintf(stderr, "Failed to create SDO download request for object 0x%4x\n", CAN_OD_MODES); return -1; } ecrt_sdo_request_timeout(sdo_download_requests[0], 500); // ms /* set the sdoexample to a specific bit muster */ sdoexample = 0x22442244; #endif #if CONFIGURE_PDOS logmsg(1, "Configuring PDOs...\n"); if (ecrt_slave_config_pdos(sc_data_in, EC_END, slave_0_syncs)) { fprintf(stderr, "Failed to configure PDOs.\n"); return -1; } if (!(sc = ecrt_master_slave_config( master, SomanetPos, SOMANET_ID))) { fprintf(stderr, "Failed to get slave configuration.\n"); return -1; } #if 0 if (ecrt_slave_config_pdos(sc, EC_END, el4102_syncs)) { fprintf(stderr, "Failed to configure PDOs.\n"); return -1; } if (!(sc = ecrt_master_slave_config( master, DigOutSlavePos, Beckhoff_EL2032))) { fprintf(stderr, "Failed to get slave configuration.\n"); return -1; } if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) { fprintf(stderr, "Failed to configure PDOs.\n"); return -1; } #endif #endif // Create configuration for bus coupler sc = ecrt_master_slave_config(master, SomanetPos /*BusCouplerPos*/, SOMANET_ID/*Beckhoff_EK1100*/); if (!sc) return -1; if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { fprintf(stderr, "PDO entry registration failed!\n"); return -1; } logmsg(1, "Activating master...\n"); if (ecrt_master_activate(master)) return -1; if (!(domain1_pd = ecrt_domain_data(domain1))) { return -1; } #if PRIORITY pid_t pid = getpid(); if (setpriority(PRIO_PROCESS, pid, -19)) fprintf(stderr, "Warning: Failed to set priority: %s\n", strerror(errno)); #endif sa.sa_handler = signal_handler; sigemptyset(&sa.sa_mask); sa.sa_flags = 0; if (sigaction(SIGALRM, &sa, 0)) { fprintf(stderr, "Failed to install signal handler!\n"); return -1; } logmsg(1, "Starting timer...\n"); tv.it_interval.tv_sec = 0; tv.it_interval.tv_usec = 1000000 / FREQUENCY; tv.it_value.tv_sec = 0; tv.it_value.tv_usec = 1000; if (setitimer(ITIMER_REAL, &tv, NULL)) { fprintf(stderr, "Failed to start timer: %s\n", strerror(errno)); return 1; } logmsg(0, "Started.\n"); while (1) { pause(); #if 0 struct timeval t; gettimeofday(&t, NULL); printf("%u.%06u\n", (unsigned)t.tv_sec, (unsigned)t.tv_usec); #endif while (sig_alarms != user_alarms) { cyclic_task(); user_alarms++; } } return 0; }
int __init init_mod(void) { int ret = -1, i; RTIME tick_period, requested_ticks, now; ec_slave_config_t *sc; printk(KERN_INFO PFX "Starting...\n"); rt_sem_init(&master_sem, 1); t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000; master = ecrt_request_master(0); if (!master) { ret = -EBUSY; printk(KERN_ERR PFX "Requesting master 0 failed!\n"); goto out_return; } ecrt_master_callbacks(master, send_callback, receive_callback, master); printk(KERN_INFO PFX "Registering domain...\n"); if (!(domain1 = ecrt_master_create_domain(master))) { printk(KERN_ERR PFX "Domain creation failed!\n"); goto out_release_master; } printk(KERN_INFO PFX "Configuring PDOs...\n"); // create configuration for reference clock FIXME if (!(sc = ecrt_master_slave_config(master, 0, 0, Beckhoff_EK1100))) { printk(KERN_ERR PFX "Failed to get slave configuration.\n"); goto out_release_master; } for (i = 0; i < NUM_DIG_OUT; i++) { if (!(sc = ecrt_master_slave_config(master, DigOutSlavePos(i), Beckhoff_EL2008))) { printk(KERN_ERR PFX "Failed to get slave configuration.\n"); goto out_release_master; } if (ecrt_slave_config_pdos(sc, EC_END, el2008_syncs)) { printk(KERN_ERR PFX "Failed to configure PDOs.\n"); goto out_release_master; } off_dig_out[i] = ecrt_slave_config_reg_pdo_entry(sc, 0x7000, 1, domain1, NULL); if (off_dig_out[i] < 0) goto out_release_master; } if (!(sc = ecrt_master_slave_config(master, CounterSlavePos, IDS_Counter))) { printk(KERN_ERR PFX "Failed to get slave configuration.\n"); goto out_release_master; } off_counter_in = ecrt_slave_config_reg_pdo_entry(sc, 0x6020, 0x11, domain1, NULL); if (off_counter_in < 0) goto out_release_master; off_counter_out = ecrt_slave_config_reg_pdo_entry(sc, 0x7020, 1, domain1, NULL); if (off_counter_out < 0) goto out_release_master; // configure SYNC signals for this slave ecrt_slave_config_dc(sc, 0x0700, 1000000, 440000, 0, 0); printk(KERN_INFO PFX "Activating master...\n"); if (ecrt_master_activate(master)) { printk(KERN_ERR PFX "Failed to activate master!\n"); goto out_release_master; } // Get internal process data for domain domain1_pd = ecrt_domain_data(domain1); printk(KERN_INFO PFX "Starting cyclic sample thread...\n"); requested_ticks = nano2count(TIMERTICKS); tick_period = start_rt_timer(requested_ticks); printk(KERN_INFO PFX "RT timer started with %i/%i ticks.\n", (int) tick_period, (int) requested_ticks); if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) { printk(KERN_ERR PFX "Failed to init RTAI task!\n"); goto out_stop_timer; } now = rt_get_time(); if (rt_task_make_periodic(&task, now + tick_period, tick_period)) { printk(KERN_ERR PFX "Failed to run RTAI task!\n"); goto out_stop_task; } printk(KERN_INFO PFX "Initialized.\n"); return 0; out_stop_task: rt_task_delete(&task); out_stop_timer: stop_rt_timer(); out_release_master: printk(KERN_ERR PFX "Releasing master...\n"); ecrt_release_master(master); out_return: rt_sem_delete(&master_sem); printk(KERN_ERR PFX "Failed to load. Aborting.\n"); return ret; }
int main(int argc, char **argv) { // ec_slave_config_t *sc; struct sigaction sa; struct itimerval tv; master = ecrt_request_master(0); if (!master) return -1; domain_input = ecrt_master_create_domain(master); domain_output = ecrt_master_create_domain(master); if (!domain_input || !domain_output) { return -1; } if (!(sc_ana_in = ecrt_master_slave_config(master, AliasAndPositon, VendorID_ProductCode))) { fprintf(stderr, "Failed to get slave configuration.\n"); return -1; } #if SDO_ACCESS fprintf(stderr, "Creating SDO requests...\n"); if (!(sdo = ecrt_slave_config_create_sdo_request(sc_ana_in, 0x6061, 0, 1))) // data size 1 ? { fprintf(stderr, "Failed to create SDO modes_of_operation_display 0x6061 request.\n"); return -1; } //EC_WRITE_U16(ecrt_sdo_request_data(sdo), 0xFFFF); ecrt_sdo_request_timeout(sdo, 500); // ms #endif #if 1 printf("Configuring PDOs...\n"); if (ecrt_slave_config_pdos(sc_ana_in, EC_END, slave_0_syncs)) { fprintf(stderr, "Failed to configure PDOs.\n"); return -1; } #endif if (ecrt_domain_reg_pdo_entry_list(domain_output, domain_output_regs)) { fprintf(stderr, "Output PDO entry registration failed!\n"); return -1; } if (ecrt_domain_reg_pdo_entry_list(domain_input, domain_input_regs)) { fprintf(stderr, "Input PDO entry registration failed!\n"); return -1; } printf("Activating master...\n"); if (ecrt_master_activate(master)) return -1; if (!(domain_output_pd = ecrt_domain_data(domain_output))) { return -1; } if (!(domain_input_pd = ecrt_domain_data(domain_input))) { return -1; } #if PRIORITY pid_t pid = getpid(); if (setpriority(PRIO_PROCESS, pid, -19)) fprintf(stderr, "Warning: Failed to set priority: %s\n", strerror(errno)); #endif sa.sa_handler = signal_handler; sigemptyset(&sa.sa_mask); sa.sa_flags = 0; if (sigaction(SIGALRM, &sa, 0)) { fprintf(stderr, "Failed to install signal handler!\n"); return -1; } printf("Starting timer...\n"); tv.it_interval.tv_sec = 0; tv.it_interval.tv_usec = 1000000 / FREQUENCY; tv.it_value.tv_sec = 0; tv.it_value.tv_usec = 1000; if (setitimer(ITIMER_REAL, &tv, NULL)) { fprintf(stderr, "Failed to start timer: %s\n", strerror(errno)); return 1; } printf("Started.\n"); while (1) { pause(); #if 0 struct timeval t; gettimeofday(&t, NULL); printf("%u.%06u\n", t.tv_sec, t.tv_usec); #endif while (sig_alarms != user_alarms) { cyclic_task(); user_alarms++; } } return 0; }
ECDomain::ECDomain(ec_master_t *master) { domain_ = ecrt_master_create_domain(master); }
int uei_ethercat_initialize (void) { uint8_t *data; ec_pdo_entry_reg_t rw_pdos[] = { // {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CI, current_ci_off, NULL}, // {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CP, current_cp_off, NULL}, // {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_OFFSET, current_offset_off, NULL}, {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_VAL, current_val_off, NULL}, // {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_POSITION, motor_pos_off, NULL}, {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATE, state_off, NULL}, // {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATUS, status_off, NULL}, // {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_TEMP, drive_temp_off, NULL}, // {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_FAULT, latched_fault_off, NULL}, // {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_STATUS, latched_status_off, NULL}, // {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE, motor_temp_v_off, NULL}, // {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS, motor_enc_wrap_off, NULL}, {0, 0, 0x00, 0x00, 0x0, 0x0, NULL, NULL}}; // ec_pdo_entry_reg_t pv_pdos[] = { // {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_CI, current_ci_off+1, NULL}, // {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_CP, current_cp_off+1, NULL}, // {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_OFFSET, current_offset_off+1, NULL}, // {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_VAL, current_val_off+1, NULL}, // {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_STATUS, status_off+1, NULL}, // {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_TEMP, drive_temp_off+1, NULL}, // {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_LATCHED_DRIVE_FAULT, latched_fault_off+1, NULL}, // {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_LATCHED_DRIVE_STATUS, latched_status_off+1, NULL}, // {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_POSITION, motor_pos_off+1, NULL}, // {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE, motor_temp_v_off+1, NULL}, // {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS, motor_enc_wrap_off+1, NULL}, // {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_STATE, state_off+1, NULL}, // {0, 0, 0x00, 0x00, 0x0, 0x0, NULL, NULL}}; // ec_pdo_entry_reg_t el_pdos[] = { // {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CI, current_ci_off+2, NULL}, // {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CP, current_cp_off+2, NULL}, // {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_OFFSET, current_offset_off+2, NULL}, // {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_VAL, current_val_off+2, NULL}, // {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATUS, status_off+2, NULL}, // {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_TEMP, drive_temp_off+2, NULL}, // {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_FAULT, latched_fault_off+2, NULL}, // {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_STATUS, latched_status_off+2, NULL}, // {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_POSITION, motor_pos_off+2, NULL}, // {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE, motor_temp_v_off+2, NULL}, // {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS, motor_enc_wrap_off+2, NULL}, // {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATE, state_off+2, NULL}, // {0, 0, 0x00, 0x00, 0x0, 0x0, NULL, NULL}}; master = ecrt_request_master(0); if (!master){ printf("Could not request master!\n"); return -1; } domain = ecrt_master_create_domain(master); if (!domain) { printf("Could not create domain!\n"); return -1; } printf("Created Domain\n"); if (!(rx_controller = ecrt_master_slave_config(master,RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE))) { fprintf(stderr, "Failed to get slave configuration for Reaction Wheel controller!\n"); return -1; } // if (!(pv_controller = ecrt_master_slave_config(master,PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE))) { // fprintf(stderr, "Failed to get slave configuration for Pivot Motor controller!\n"); // return -1; // } // if (!(el_controller = ecrt_master_slave_config(master,EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE))) { // fprintf(stderr, "Failed to get slave configuration for Elevation Motor controller!\n"); // return -1; // } if (ecrt_slave_config_pdos(rx_controller, EC_END, copley_pdo_syncs)) { perror("ecrt_slave_config_pdos() failed for RX controller."); ecrt_release_master(master); return 3; } // if (ecrt_slave_config_pdos(pv_controller, 1, copley_pdo_syncs)) { // perror("ecrt_slave_config_pdos() failed for Pivot controller."); // ecrt_release_master(master); // return 3; // } // if (ecrt_slave_config_pdos(el_controller, 1, copley_pdo_syncs)) { // perror("ecrt_slave_config_pdos() failed for Elevation controller."); // ecrt_release_master(master); // return 3; // } /// Register the PDO list and variable mappings // if (ecrt_domain_reg_pdo_entry_list(domain, rw_pdos)) { // perror("ecrt_domain_reg_pdo_entry_list() failed for reaction wheel!"); // ecrt_release_master(master); // return -1; // } // if (ecrt_domain_reg_pdo_entry_list(domain, pv_pdos)) { // perror("ecrt_domain_reg_pdo_entry_list() failed for pivot motor!"); // ecrt_release_master(master); // return -1; // } // if (ecrt_domain_reg_pdo_entry_list(domain, el_pdos)) { // perror("ecrt_domain_reg_pdo_entry_list() failed for Elevation motor!"); // ecrt_release_master(master); // return -1; // } state_off[0] = ecrt_slave_config_reg_pdo_entry(rx_controller, ECAT_DRIVE_STATE, domain, NULL); current_val_off[0] = ecrt_slave_config_reg_pdo_entry(rx_controller, ECAT_CURRENT_LOOP_VAL, domain, NULL); // configure SYNC signals for this slave ecrt_slave_config_dc(rx_controller, 0, 1000000000ll / 100, 4400000, 0, 0); printf("Set Master/Slave Configuration\n"); if (ecrt_master_activate(master) < 0) { printf("Could not activate master!\n"); return -1; } if (!(data = ecrt_domain_data(domain))) { perror("ecrt_domain_data() failed!"); ecrt_release_master(master); return -1; } ethercat_set_offsets(&rx_controller_state, data, 0); // ethercat_set_offsets(&pv_controller_state, data, 1); // ethercat_set_offsets(&el_controller_state, data, 2); check_domain1_state(); check_master_state(); printf("Data: %p\t Current: %p\t State: %p\n", data, rx_controller_state.current_val, rx_controller_state.amp_state); ecrt_master_receive(master); ecrt_domain_process(domain); EC_WRITE_S16(data + current_val_off[0], 0); EC_WRITE_U16(data + state_off[0], ECAT_STATE_DISABLED); ecrt_domain_queue(domain); ecrt_master_send(master); check_domain1_state(); check_master_state(); return 0; }
int main(int argc, char **argv) { ec_slave_config_t *sc; struct sigaction sa; struct itimerval tv; uint16_t i; master = ecrt_request_master(0); if (!master) return -1; domain1 = ecrt_master_create_domain(master); if (!domain1) return -1; printf("Configuring PDOs...\n"); Init_EL100x(&off_dig_in[0], 0, Beckhoff_EL1004, el1004_syncs); Init_EL100x(&off_dig_in[1], 1, Beckhoff_EL1004, el1004_syncs); Init_EL100x(&off_dig_out[0], 2, Beckhoff_EL2004, el2004_syncs); Init_EL100x(&off_dig_out[1], 3, Beckhoff_EL2004, el2004_syncs); // Create configuration for bus coupler sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100); if (!sc) return -1; fprintf(stderr, "EK1100 configured.\n"); printf("Activating master...\n"); if (ecrt_master_activate(master)) return -1; if (!(domain1_pd = ecrt_domain_data(domain1))) { return -1; } #if PRIORITY pid_t pid = getpid(); if (setpriority(PRIO_PROCESS, pid, -19)) fprintf(stderr, "Warning: Failed to set priority: %s\n", strerror(errno)); #endif sa.sa_handler = signal_handler; sigemptyset(&sa.sa_mask); sa.sa_flags = 0; if (sigaction(SIGALRM, &sa, 0)) { fprintf(stderr, "Failed to install signal handler!\n"); return -1; } printf("Starting timer...\n"); tv.it_interval.tv_sec = 0; tv.it_interval.tv_usec = 1000000 / FREQUENCY; tv.it_value.tv_sec = 0; tv.it_value.tv_usec = 1000; if (setitimer(ITIMER_REAL, &tv, NULL)) { fprintf(stderr, "Failed to start timer: %s\n", strerror(errno)); return 1; } printf("Started.\n"); while (1) { pause(); #if 0 struct timeval t; gettimeofday(&t, NULL); printf("%u.%06u\n", t.tv_sec, t.tv_usec); #endif while (sig_alarms != user_alarms) { cyclic_task(); user_alarms++; } } return 0; }
int main(int argc, char **argv) { // Создаем мастер-объект gkMaster = ecrt_request_master(0); if (gkMaster) { fprintf(stdout, "1. Master created.\n"); } else { fprintf(stderr, "Unable to get requested master.\n"); return -1; } // Создаем объект для обмена PDO в циклическом режиме. gkDomain1 = ecrt_master_create_domain(gkMaster); if (gkDomain1) { fprintf(stdout, "2. Process data domain created.\n"); } else { fprintf(stderr, "Unable to create process data domain.\n"); return -1; } // Создаем объект конфигурации подчиненного. ec_slave_config_t* sc = ecrt_master_slave_config(gkMaster, 0, gkDriveNum, 0x00007595, 0x00000000); if (sc) { fprintf(stdout, "3. Slave configuration object created.\n"); } else { fprintf(stderr, "Failed to get slave configuration.\n"); return -1; } // Конфигурируем PDO подчиненного // TxPDO ec_pdo_entry_info_t l7na_tx_channel1[] = { {0x6041, 0, 16}, // Statusword {0x6061, 0, 8}, // The Modes of Operation Display {0x6062, 0, 32}, // The Position Demand Value {0x6064, 0, 32}, // The Position Actual Value {0x606B, 0, 32}, // The Velocity Demand Value {0x6081, 0, 32}, // The Profile Velocity {0x606C, 0, 32}, // The Actual Velocity Value {0x607A, 0, 32}, // The Target Position {0x6077, 0, 16}, // Actual torque value // {0x200F, 0, 16}, // Position Scale Denominator }; ec_pdo_info_t l7na_tx_pdos[] = { {0x1A00, 9, l7na_tx_channel1} }; // RxPDO ec_pdo_entry_info_t l7na_rx_channel1[] = { {0x6040, 0, 16}, // Controlword {0x6060, 0, 8}, // Modes of Operation {0x607A, 0, 32}, // The Target Position {0x606C, 0, 32}, // The Velocity Demand value {0x6081, 0, 32}, // The Profile Velocity {0x60FF, 0, 32}, // The Target Velocity (in Profile Velocity (Pv) mode and Cyclic Synchronous Velocity (Csv) modes) {0x6071, 0, 16}, // The Target Torque }; ec_pdo_info_t l7na_rx_pdos[] = { {0x1600, 7, l7na_rx_channel1} }; // Конфигурация SyncManagers 2 (FMMU0) и 3 (FMMU1) // { sync_mgr_idx, sync_mgr_direction, pdo_num, pdo_ptr, watch_dog_mode } // { 0xFF - end marker} ec_sync_info_t l7na_syncs[] = { {2, EC_DIR_OUTPUT, 1, l7na_rx_pdos, EC_WD_DISABLE}, {3, EC_DIR_INPUT, 1, l7na_tx_pdos, EC_WD_DISABLE}, {0xFF} }; if (ecrt_slave_config_pdos(sc, EC_END, l7na_syncs)) { fprintf(stderr, "Failed to configure slave pdo.\n"); return -1; } fprintf(stdout, "4. Configuring slave PDOs and sync managers done.\n"); // Регистируем PDO в домене if (ecrt_domain_reg_pdo_entry_list(gkDomain1, gkDomain1Regs)) { fprintf(stderr, "PDO entry registration failed!\n"); return -1; } fprintf(stdout, "5. PDO entries registered in domain.\n"); if (ecrt_master_activate(gkMaster)) { fprintf(stderr,"Master activation failed.\n"); return -1; } fprintf(stdout, "6. Master activated.\n"); if (!(gkDomain1PD = ecrt_domain_data(gkDomain1))) { fprintf(stderr,"Domain data initialization failed.\n"); return -1; } fprintf(stdout, "7. Domain data registered.\n"); //goto end; check_master_state(); check_domain1_state(); int32_t op_flag = 0, ipos = 0; uint16_t istatus = 0; //ждать режим OP for(uint32_t j = 0; ; j++) { ecrt_master_receive(gkMaster); //RECEIVE A FRAME ecrt_domain_process(gkDomain1); //DETERMINE THE DATAGRAM STATES // check_slave_config_states(); if (! op_flag) { check_domain1_state(); } if (gkDomain1State.wc_state == EC_WC_COMPLETE && !op_flag) { printf("Domain is up at %d cycles.\n", j); op_flag = 1; } ipos = EC_READ_U32(gkDomain1PD + gkOffIPos); //READ DATA 0x6064 position istatus = EC_READ_U16(gkDomain1PD + gkOffIStatus); //READ DATA 0x6041 status // send process data ecrt_domain_queue(gkDomain1); //MARK THE DOMAIN DATA AS READY FOR EXCHANGE ecrt_master_send(gkMaster); //SEND ALL QUEUED DATAGRAMS usleep(1000); //WAIT 1mS if (op_flag) { printf("1-Position: %d Status: 0x%x\n", ipos, istatus); break; } } fprintf(stdout, "8. Got OP state.\n"); if(argc > 1) { //перейти в позицию const int cmdpos = atoi(argv[1]); printf("cmd pos: %d\n", cmdpos); ecrt_master_receive(gkMaster); ecrt_domain_process(gkDomain1); EC_WRITE_U16(gkDomain1PD + gkOffOControl, 0xF); //0x6040 ControlWord EC_WRITE_U8(gkDomain1PD + gkOffOMode, 1); // 0x6060 Profile position mode // 3 - for velocity mode, 1- for position mode EC_WRITE_S32(gkDomain1PD + gkOffPVel, 1000000); // 0x60ff profile velocity // gkOffTVel - for velocity mode ecrt_domain_queue(gkDomain1); ecrt_master_send(gkMaster); usleep(1000); //wait for (uint32_t i = 0; i < 200; ++i) { ecrt_master_receive(gkMaster); ecrt_domain_process(gkDomain1); ecrt_domain_queue(gkDomain1); ecrt_master_send(gkMaster); usleep(1000); } ecrt_master_receive(gkMaster); ecrt_domain_process(gkDomain1); /* comment 2 lines for velocity mode */ EC_WRITE_S32(gkDomain1PD + gkOffOPos, cmdpos); EC_WRITE_U16(gkDomain1PD + gkOffOControl, 0x11F); ecrt_domain_queue(gkDomain1); ecrt_master_send(gkMaster); usleep(1000); //wait for (uint32_t i = 0; i < 200; ++i) { ecrt_master_receive(gkMaster); ecrt_domain_process(gkDomain1); ecrt_domain_queue(gkDomain1); ecrt_master_send(gkMaster); usleep(1000); } /* ecrt_master_receive(gkMaster); ecrt_domain_process(gkDomain1); EC_WRITE_S32(gkDomain1PD + gkOffOPos, cmdpos); ecrt_domain_queue(gkDomain1); ecrt_master_send(gkMaster); usleep(1000);*/ //wait /* for (uint32_t i = 0; i < 1000; ++i) { ecrt_master_receive(gkMaster); ecrt_domain_process(gkDomain1); ecrt_domain_queue(gkDomain1); ecrt_master_send(gkMaster); usleep(1000); } */ timespec tbegin, tend; ::clock_gettime(CLOCK_MONOTONIC, &tbegin); printf("Time begin: %lds/%ldns\n", tbegin.tv_sec, tbegin.tv_nsec); const uint32_t kIterationMax = 500000; uint32_t change_count = 0; bool target_reached = false; for (uint32_t j = 0; ; j++) { ecrt_master_receive(gkMaster); ecrt_domain_process(gkDomain1); int32_t ipos_new = EC_READ_S32(gkDomain1PD + gkOffIPos); //READ DATA 0x6064 position uint16_t istatus_new = EC_READ_U16(gkDomain1PD + gkOffIStatus); //READ DATA 0x6041 status int32_t imode = EC_READ_S8(gkDomain1PD + gkOffIMode); int32_t ipvel = EC_READ_S32(gkDomain1PD + gkOffPVel); int32_t idvel = EC_READ_S32(gkDomain1PD + gkOffDVel); int32_t iavel = EC_READ_S32(gkDomain1PD + gkOffIVel); int32_t idpos = EC_READ_S32(gkDomain1PD + gkOffDPos); int32_t itpos = EC_READ_S32(gkDomain1PD + gkOffOPos); int32_t icontrol = EC_READ_U16(gkDomain1PD + gkOffOControl); int16_t iatorq = EC_READ_S16(gkDomain1PD + gkOffITorq); // int32_t ipdenom = EC_READ_S16(gkDomain1PD + gkOffPDenom); if (ipos_new != ipos) { ipos = ipos_new; change_count++; printf("Position: %d Status: 0x%x Mode: %d ATorq: %d PVel: %d DVel: %d AVel: %d DPos: %d TPos: %d OControl: 0x%x\n", ipos, istatus, imode, iatorq, ipvel, idvel, iavel, idpos, itpos, icontrol); } // position mode if(! target_reached && ((istatus_new >> 10) & 0x1)) { ::clock_gettime(CLOCK_MONOTONIC, &tend); printf("Target reached. Pos: %d Status: 0x%x TEnd=%lds/%ldns\n", ipos, istatus, tend.tv_sec, tend.tv_nsec); target_reached = true; //break; } /* Velocity mode */ if (j == kIterationMax) { /* clock_gettime(CLOCK_MONOTONIC, &tend); printf("Iterations=%d, change_count=%d. time_end=%lds/%ldns Stopping...\n", j, change_count, tend.tv_sec, tend.tv_nsec); EC_WRITE_U16(gkDomain1PD + gkOffOControl, 0x6); break; */ } ecrt_domain_queue(gkDomain1); ecrt_master_send(gkMaster); usleep(100); //WAIT 1mS } } ecrt_master_receive(gkMaster); ecrt_domain_process(gkDomain1); printf("...Done. Releasing the master!\n"); // Освобождаем мастер-объект ecrt_release_master(gkMaster); return 0; }