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 lcec_el5152_write(struct lcec_slave *slave, long period) { lcec_master_t *master = slave->master; lcec_el5152_data_t *hal_data = (lcec_el5152_data_t *) slave->hal_data; uint8_t *pd = master->process_data; int i; lcec_el5152_chan_t *chan; // set outputs for (i=0; i<LCEC_EL5152_CHANS; i++) { chan = &hal_data->chans[i]; // set output data EC_WRITE_BIT(&pd[chan->set_count_pdo_os], chan->set_count_pdo_bp, *(chan->set_raw_count)); EC_WRITE_S32(&pd[chan->set_count_val_pdo_os], *(chan->set_raw_count_val)); } }
void run(long data) { int i; struct timeval tv; unsigned int sync_ref_counter = 0; count2timeval(nano2count(rt_get_real_time_ns()), &tv); // while (deactive!=20) { while (1) { t_last_cycle = get_cycles(); // receive process data rt_sem_wait(&master_sem); ecrt_master_receive(master); ecrt_domain_process(domain1); rt_sem_signal(&master_sem); // check process data state (optional) //check_domain1_state(); inpu[0]=EC_READ_U16(domain1_pd + status_word); inpu[1]=EC_READ_U32(domain1_pd + pos_act); // if(servooff==1){//servo off // if(stop==1){ // if( ( inpu[0] == 0x1637 ) && ( inpu[2] == 0x1637 ) ){ // EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 ); // EC_WRITE_U16(domain1_pd+ctrl_word2, 0x0006 ); // } // else if( ( inpu[0] == 0x0650 ) && ( inpu[2] == 0x0650 ) ){ // printk(KERN_INFO PFX "want to PREOP"); // deactive++; // } // } if( (inpu[0]&0x004f) == 0x0040 ){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 ); } else if( (inpu[0]&0x006f) == 0x0021){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x0007 ); } else if( (inpu[0]&0x006f) == 0x0023){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x000f); EC_WRITE_S32(domain1_pd+tar_pos, 0); EC_WRITE_S32(domain1_pd+max_torq, 0xf00); } else if( (inpu[0]&0x006f) == 0x0027){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x001f); EC_WRITE_S32(domain1_pd+tar_pos , value ); //for mode 8 no sin if(value==180000){ speedup=0; speeddown=1; //printk(KERN_INFO PFX "top"); value=value-1; } else if(speeddown==1 && value!=0){ value=value-1; //printk(KERN_INFO PFX "slow down"); } else if(speeddown==1 && value==0){ speedup=0; speeddown=0; // stop=1; //printk(KERN_INFO PFX "stop"); } else if(!stop){ speedup=1; speeddown=0; value=value+1; //printk(KERN_INFO PFX "fast up "); } // change++; // } // else // change = 0; } rt_sem_wait(&master_sem); tv.tv_usec += 1000; if (tv.tv_usec >= 1000000) { tv.tv_usec -= 1000000; tv.tv_sec++; } ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv)); if (sync_ref_counter) { sync_ref_counter--; } else { sync_ref_counter = 1; //original = 9 ecrt_master_sync_reference_clock(master); } ecrt_master_sync_slave_clocks(master); ecrt_domain_queue(domain1); ecrt_master_send(master); rt_sem_signal(&master_sem); rt_task_wait_period(); } }
void cyclic_task() { struct timespec wakeupTime, time; // get current time clock_gettime(CLOCK_TO_USE, &wakeupTime); while(1) { if(deactive==1) { break; } wakeupTime = timespec_add(wakeupTime, cycletime); clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); // writter_receive(master); ecrt_master_receive(master); ecrt_domain_process(domain_r); ecrt_domain_process(domain_w); temp[0]=EC_READ_U16(domain_w_pd + status_word); temp[1]=EC_READ_S32(domain_w_pd + mode_display); if (counter) { counter--; } else { // do this at 1 Hz counter = FREQUENCY; check_master_state(); blink = !blink; } // write process data if(servo_flag==1) { //servo off EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0006 ); } else if( (temp[0]&0x004f) == 0x0040 ) { EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0006 ); } else if( (temp[0]&0x006f) == 0x0021) { EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0007 ); } else if( (temp[0]&0x006f) == 0x0023) { EC_WRITE_U16(domain_r_pd+ctrl_word, 0x000f ); EC_WRITE_S32(domain_r_pd+tar_pos,0); EC_WRITE_S32(domain_r_pd+tar_vel, 0xffff); EC_WRITE_S32(domain_r_pd+max_torq, 0xf00); } //operation enabled else if( (temp[0]&0x006f) == 0x0027) { EC_WRITE_S32(domain_r_pd+tar_pos, (move_value+=2000) ); EC_WRITE_U16(domain_r_pd+ctrl_word, 0x001f); } clock_gettime(CLOCK_TO_USE, &time); ecrt_master_application_time(master, TIMESPEC2NS(time)); if (sync_ref_counter) { sync_ref_counter--; } else { sync_ref_counter = 1; // sync every cycle ecrt_master_sync_reference_clock(master); } ecrt_master_sync_slave_clocks(master); // send process data ecrt_domain_queue(domain_r); ecrt_domain_queue(domain_w); ecrt_master_send(master); } }
void run(long data) { int i; struct timeval tv; unsigned int sync_ref_counter = 0; count2timeval(nano2count(rt_get_real_time_ns()), &tv); // while (deactive!=20) { while (1) { t_last_cycle = get_cycles(); /* if ( (inpu[0]==0x0650) &&(stop==1) ) break; */ // receive process data rt_sem_wait(&master_sem); ecrt_master_receive(master); ecrt_domain_process(domain1); ecrt_domain_process(domain2); rt_sem_signal(&master_sem); // check process data state (optional) //check_domain1_state(); inpu[0]=EC_READ_U16(domain2_pd + status_word); inpu[1]=EC_READ_U32(domain2_pd + actual_pos); /* if (counter) { counter--; } else { u32 c; counter = FREQUENCY; // check for master state (optional) check_master_state(); c = EC_READ_U32(domain1_pd + off_counter_in); if (counter_value != c) { counter_value = c; printk(KERN_INFO PFX "counter=%u\n", counter_value); } } */ /* if (blink_counter) { blink_counter--; } else { blink_counter = 9; // calculate new process data blink = !blink; } */ // write process data /* for (i = 0; i < NUM_DIG_OUT; i++) { EC_WRITE_U8(domain1_pd + off_dig_out[i], blink ? 0x66 : 0x99); } EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02); */ // if(servooff==1){//servo off if(stop==1){ if ( inpu[0] == 0x1637 ) { EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 ); } else if( inpu[0] == 0x0650 ){ //++deactive ; //EC_WRITE_U16(domain1_pd+alstat, 0x0002 ); printk(KERN_INFO PFX "want to PREOP"); deactive++; } /* else{ EC_WRITE_U16(domain1_pd+alstat, 0x0002 ); printk(KERN_INFO PFX "want to PREOP"); break; } */ } else if( (inpu[0]&0x0040) == 0x0040){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 ); } else if( (inpu[0]&0x006f) == 0x0021 ){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x0007 ); } else if( (inpu[0]&0x027f) == 0x0233){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x000f); EC_WRITE_S32(domain1_pd+interpolateddata, 0); //EC_WRITE_S32(domain1_pd+tar_velo, 0xffffff); EC_WRITE_S32(domain1_pd+max_torq, 0xf00); EC_WRITE_S32(domain1_pd+modeofoper, 8); } else if( (inpu[0]&0x027f) == 0x0237){ //if(change >= 0 && change<2 ){ if( change<1 ){ //start=1; //if(i==0){ //EC_WRITE_S32(domain1_pd+interpolateddata , 0 ); //EC_WRITE_S32(domain1_pd+interpolateddata2 , 0 ); //EC_WRITE_S32(domain1_pd+target_pos , 0 ); //EC_WRITE_S32(domain1_pd+target_pos2 , 0 ); //EC_WRITE_S32(domain1_pd+tar_velo , 0 ); //EC_WRITE_S32(domain1_pd+tar_velo2 , 0 ); //} //else { //EC_WRITE_S32(domain1_pd+interpolateddata , (sin(i)*180000) ); //for mode 7 //EC_WRITE_S32(domain1_pd+interpolateddata2 , (sin(i)*180000) ); //EC_WRITE_S32(domain1_pd+target_pos , (sin(i)*180000) ); //for mode 8 with sin //EC_WRITE_S32(domain1_pd+target_pos2 , (sin(i)*180000) ); EC_WRITE_S32(domain1_pd+target_pos , inpu[7] ); //for mode 8 no sin //EC_WRITE_S32(domain1_pd+tar_velo , 500000 ); //for mode 9 //EC_WRITE_S32(domain1_pd+tar_velo2 , 500000 ); //if(1){ if(inpu[7]==1800000){ speedup=0; speeddown=1; //printk(KERN_INFO PFX "top"); inpu[7]=inpu[7]-200; } else if(speeddown==1 && inpu[7]!=0){ inpu[7]=inpu[7]-200; //printk(KERN_INFO PFX "slow down"); } else if(speeddown==1 && inpu[7]==0){ speedup=0; speeddown=0; stop=1; //printk(KERN_INFO PFX "stop"); } else if(!stop){ speedup=1; speeddown=0; inpu[7]=inpu[7]+2000; //printk(KERN_INFO PFX "fast up "); } /* if(speedup==1) inpu[7]+=500; else if(speeddown==1) inpu[7]-=1000; else{ inpu[7]=0; servooff=1; } */ //EC_WRITE_S32(domain1_pd+tar_velo , inpu[7] ); //for mode 9 //EC_WRITE_S32(domain1_pd+tar_velo2 , inpu[7] ); //} //else{ //EC_WRITE_S32(domain1_pd+tar_velo , inpu[7] ); //for mode 9 //EC_WRITE_S32(domain1_pd+tar_velo2 , inpu[7] ); //} //} EC_WRITE_U16(domain1_pd+ctrl_word, 0x001f); change++; /* if(datacount<10001){ data[datacount][0]=(sin(i)*360000); data[datacount][1]=inpu[1]; data[datacount][2]=inpu[3]; data[datacount][3]=(inpu[1] - inpu[3]); datacount++; } */ } else change = 0; } //printk(KERN_INFO PFX "pos1=%d pos2=%d inpu7=%d\n",inpu[1],inpu[3],inpu[7]); rt_sem_wait(&master_sem); tv.tv_usec += 1000; if (tv.tv_usec >= 1000000) { tv.tv_usec -= 1000000; tv.tv_sec++; } ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv)); if (sync_ref_counter) { sync_ref_counter--; } else { sync_ref_counter = 1; //original = 9 ecrt_master_sync_reference_clock(master); } ecrt_master_sync_slave_clocks(master); ecrt_domain_queue(domain1); ecrt_domain_queue(domain2); ecrt_master_send(master); rt_sem_signal(&master_sem); rt_task_wait_period(); } }
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; }