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; }
void __exit cleanup_mod(void) { printk(KERN_INFO PFX "Stopping...\n"); //ecrt_slave_config_dc(sc, 0x000, 2000000, 125000, 0, 0); //ecrt_slave_config_dc(sc2, 0x000, 2000000, 125000, 0, 0); //if( ( inpu[0] == 0x1637 ) && ( inpu[2] == 0x1637 ) ){ //EC_WRITE_U16(domain1_pd+ctrl_word, 0x0002 ); //EC_WRITE_U16(domain1_pd+ctrl_word2, 0x0002 ); //} //else if( ( inpu[0] == 0x0650 ) && ( inpu[2] == 0x0650 ) ){ //++deactive ; //} rt_task_delete(&task); stop_rt_timer(); ecrt_release_master(master); rt_sem_delete(&master_sem); printk(KERN_INFO PFX "Unloading.\n"); }
void __exit cleanup_mod(void) { printk(KERN_INFO PFX "Stopping...\n"); rt_task_delete(&task); stop_rt_timer(); ecrt_release_master(master); rt_sem_delete(&master_sem); printk(KERN_INFO PFX "Unloading.\n"); }
void __exit cleanup_mini_module(void) { printk(KERN_INFO PFX "Stopping...\n"); del_timer_sync(&timer); free_serial_devices(); printk(KERN_INFO PFX "Releasing master...\n"); ecrt_release_master(master); printk(KERN_INFO PFX "Unloading.\n"); }
/** Driver close. * * \return Always zero (success). */ int ec_rtdm_close( struct rtdm_dev_context *context, /**< Context. */ rtdm_user_info_t *user_info /**< User data. */ ) { ec_rtdm_context_t *ctx = (ec_rtdm_context_t *) context->dev_private; ec_rtdm_dev_t *rtdm_dev = (ec_rtdm_dev_t *) context->device->device_data; if (ctx->ioctl_ctx.requested) { ecrt_release_master(rtdm_dev->master); } #if DEBUG EC_MASTER_INFO(rtdm_dev->master, "RTDM device %s closed.\n", context->device->device_name); #endif return 0; }
void uei_ethercat_cleanup(void) { ecrt_master_receive(master); ecrt_domain_process(domain); EC_WRITE_S16(rx_controller_state.current_val, 0); EC_WRITE_U16(rx_controller_state.amp_state, ECAT_STATE_DISABLED); // EC_WRITE_S16(pv_controller_state.current_val, 0); // EC_WRITE_U16(pv_controller_state.amp_state, ECAT_STATE_DISABLED); // // EC_WRITE_S16(el_controller_state.current_val, 0); // EC_WRITE_U16(el_controller_state.amp_state, ECAT_STATE_DISABLED); ecrt_domain_queue(domain); ecrt_master_send(master); ecrt_release_master(master); }
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; }
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 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) { // Создаем мастер-объект 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; }