void cyclic_task() { // receive process data ecrt_master_receive(master); ecrt_domain_process(domain1); // check process data state (optional) check_domain1_state(); if (counter) { counter--; } else { // do this at 1 Hz counter = FREQUENCY; // calculate new process data blink = !blink; // check for master state (optional) check_master_state(); // check for islave configuration state(s) (optional) check_slave_config_states(); #if SDO_ACCESS // read process data SDO read_sdo(); #endif } #if 0 // read process data printf("AnaIn: state %u value %u\n", EC_READ_U8(domain1_pd + off_ana_in_status), EC_READ_U16(domain1_pd + off_ana_in_value)); #endif #if 1 // write process data EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09); #endif // send process data ecrt_domain_queue(domain1); ecrt_master_send(master); }
void 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 read_sdo(); // send process data ecrt_domain_queue(domain1); ecrt_master_send(master); }
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; }
void cyclic_task() { /* sync the dc clock of the slaves */ ecrt_master_sync_slave_clocks(master); // receive process data ecrt_master_receive(master); ecrt_domain_process(domain1); // check process data state (optional) check_domain1_state(); if (counter) { counter--; } else { // do this at 1 Hz counter = FREQUENCY; // calculate new process data blink = !blink; // check for master state (optional) check_master_state(); // check for islave configuration state(s) (optional) check_slave_config_states(); #if SDO_ACCESS // read process data SDO read_sdo(sdo); read_sdo(request[0]); read_sdo(request[1]); read_sdo(request[2]); write_sdo(sdo_download_requests[0], sdoexample); /* SDO download value to the node */ #endif } /* Read process data */ unsigned int sn_status = EC_READ_U16(domain1_pd + off_pdo1_in); unsigned int sn_modes = EC_READ_U8(domain1_pd + off_pdo2_in); unsigned int sn_position = EC_READ_U32(domain1_pd + off_pdo3_in); unsigned int sn_velocity = EC_READ_U32(domain1_pd + off_pdo4_in); unsigned int sn_torque = EC_READ_U16(domain1_pd + off_pdo5_in); logmsg(2, "[REC] 0x%4x 0x%4x 0x%8x 0x%8x 0x%4x\n", sn_status, sn_modes, sn_position, sn_velocity, sn_torque); #if 0 // read process data printf("AnaIn: state %u value %u\n", EC_READ_U8(domain1_pd + off_ana_in_status), EC_READ_U16(domain1_pd + off_ana_in_value)); #endif // write process data //EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09); #ifdef CIA402 #define STATUSW1 0x88AA #define STATUSW2 0xAA88 #define OPMODES1 0xf1 #define OPMODES2 0x1f #define TORVAL1 0xabab #define TORVAL2 0xbaba #define VELVAL1 0x2d2d4d4d #define VELVAL2 0xd4d4d2d2 #define POSVAL1 0xe4e4e2e2 #define POSVAL2 0x2e2e4e4e EC_WRITE_U16(domain1_pd + off_pdo1_out, (blink ? STATUSW1 : STATUSW2)&0xffff); EC_WRITE_U8(domain1_pd + off_pdo2_out, (blink ? OPMODES1 : OPMODES2)&0xff); EC_WRITE_U16(domain1_pd + off_pdo3_out, (blink ? TORVAL1 : TORVAL2)&0xffff); EC_WRITE_U32(domain1_pd + off_pdo4_out, blink ? POSVAL1 : POSVAL2); EC_WRITE_U32(domain1_pd + off_pdo5_out, blink ? VELVAL1 : VELVAL2); #else #define TESTWORD1 0xdead #define TESTWORD2 0xbeef #define TESTWORD3 0xfefe #define TESTWORD4 0xa5a5 EC_WRITE_U16(domain1_pd + off_pdo1_out, blink ? TESTWORD1 : TESTWORD2); EC_WRITE_U16(domain1_pd + off_pdo2_out, blink ? TESTWORD3 : TESTWORD4); #endif // send process data ecrt_domain_queue(domain1); ecrt_master_send(master); //printf("Wrote %x to slave\n", blink ? TESTWORD1 : TESTWORD2); }
void cyclic_task() { // receive process data ecrt_master_receive(master); ecrt_domain_process(domain_output); ecrt_domain_process(domain_input); // check process data state (optional) check_domain1_state(); if (counter) { counter--; } else { // do this at 1 Hz counter = FREQUENCY; // check for master state (optional) check_master_state(); // check for islave configuration state(s) (optional) check_slave_config_states(); // read process data // {0x6040, 0x00, 16}, /* Controlword */ // {0x6060, 0x00, 8}, /* Mode_of_Operation */ // {0x6098, 0x00, 8}, /* Homing_Method */ // {0x607a, 0x00, 32}, /* Target_Position */ // {0x60ff, 0x00, 32}, /* Target_Velocity */ // {0x6071, 0x00, 16}, /* Target_Torque */ // {0x6041, 0x00, 16}, /* Statusword */ // {0x6064, 0x00, 32}, /* Position_Actual_Value */ // {0x6061, 0x00, 8}, /* Modes_Of_Operation_Display */ // {0x1001, 0x00, 8}, /* Error_Register */ // {0x606c, 0x00, 32}, /* Velocity_Actual_Value */ // {0x6077, 0x00, 16}, /* Torque_Actual_Value */ // printf("pdo value: %02x offset %u\n", // EC_READ_U16(domain1_pd + off_0x6040),off_0x6040); // printf("pdo value: %02x offset %u\n", // EC_READ_U16(domain1_pd + off_0x1001),off_0x1001); printf("pdo value: %04x offset %u\n", EC_READ_U16(domain_input_pd + off_0x6041),off_0x6041); printf("pdo value 6061asfsadf: %04x offset %u\n", EC_READ_U8(domain_input_pd + off_0x6061),off_0x6061); printf("pd: %u \n",*domain_input_pd); // EC_READ_U8(domain1_pd + off_ana_in_value)); #if SDO_ACCESS // read process data SDO read_sdo(); #endif } // send process data ecrt_domain_queue(domain_output); ecrt_domain_queue(domain_input); ecrt_master_send(master); #if 0 // write process data // EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09); #endif }