void disable_operation() { bool isOperationDisabled=false; // while(!isOperationDisabled) for(int i=0;i<1;++i) { EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x0000);// p85, use state transition 9 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: fprintf(stderr, "Request to Write,But Still busy...\n"); break; case EC_REQUEST_SUCCESS: fprintf(stderr, "sdo_controlword_write write value 0x03"); ecrt_sdo_request_write(sdo_controlword_write); break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO state!\n"); break; } // 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: fprintf(stderr, "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,exit safely \n"); } break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); break; } if(isOperationDisabled) { break; } else { printf("bad exit!\n"); } }//while }
int write_sdo(ec_sdo_request_t *req, unsigned data) { EC_WRITE_S32(ecrt_sdo_request_data(req), data&0xffffffff); switch (ecrt_sdo_request_state(req)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_write(req); // trigger first read break; case EC_REQUEST_BUSY: //fprintf(stderr, "SDO write still busy...\n"); //logmsg(1, "SDO value written: \n",data ); pause(); break; case EC_REQUEST_SUCCESS: //logmsg(1, "SDO value written: 0x%X\n", data); pause(); ecrt_sdo_request_read(req); // trigger next read return 1; break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to write SDO!\n"); ecrt_sdo_request_write(req); // retry writing return 0; break; } }
void read_sdo(void) { switch (ecrt_sdo_request_state(sdo)) { case EC_REQUEST_UNUSED: // request was not used yet 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_U16(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; } }
void read_sdo(ec_sdo_request_t *req) { switch (ecrt_sdo_request_state(req)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(req); // trigger first read break; case EC_REQUEST_BUSY: fprintf(stderr, "SDO still busy...\n"); break; case EC_REQUEST_SUCCESS: logmsg(1, "SDO value read: 0x%X\n", EC_READ_U32(ecrt_sdo_request_data(req))); ecrt_sdo_request_read(req); // trigger next read break; case EC_REQUEST_ERROR: fprintf(stderr, "Failed to read SDO!\n"); ecrt_sdo_request_read(req); // retry reading break; } }
/** Request reading the number of mapped PDO entries. */ void ec_fsm_pdo_entry_read_state_start( ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */ ec_datagram_t *datagram /**< Datagram to use. */ ) { ecrt_sdo_request_index(&fsm->request, fsm->target_pdo->index, 0); ecrt_sdo_request_read(&fsm->request); fsm->state = ec_fsm_pdo_entry_read_state_count; ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately }
int read_sdo(ec_sdo_request_t *req) { int sdo_read_value; switch (ecrt_sdo_request_state(req)) { case EC_REQUEST_UNUSED: // request was not used yet ecrt_sdo_request_read(req); // trigger first read break; case EC_REQUEST_BUSY: //fprintf(stderr, "SDO still busy...\n"); break; case EC_REQUEST_SUCCESS: sdo_read_value = EC_READ_S32(ecrt_sdo_request_data(req)); //logmsg(1, "SDO value read: 0x%X\n", sdo_read_value); ecrt_sdo_request_write(req); // trigger next write break; case EC_REQUEST_ERROR: //fprintf(stderr, "Failed to read SDO!\n"); ecrt_sdo_request_read(req); // retry reading break; } return sdo_read_value; }
/** Read next PDO entry. */ void ec_fsm_pdo_entry_read_action_next( ec_fsm_pdo_entry_t *fsm, /**< finite state machine */ ec_datagram_t *datagram /**< Datagram to use. */ ) { if (fsm->entry_pos <= fsm->entry_count) { ecrt_sdo_request_index(&fsm->request, fsm->target_pdo->index, fsm->entry_pos); ecrt_sdo_request_read(&fsm->request); fsm->state = ec_fsm_pdo_entry_read_state_entry; ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately return; } // finished reading entries. fsm->state = ec_fsm_pdo_entry_state_end; }
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 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; }