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; }