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; } }
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(); } }
/** Start reading data. */ void ec_fsm_foe_state_data_read( ec_fsm_foe_t *fsm, /**< FoE statemachine. */ ec_datagram_t *datagram /**< Datagram to use. */ ) { size_t rec_size; uint8_t *data, opCode, packet_no, mbox_prot; ec_slave_t *slave = fsm->slave; #ifdef DEBUG_FOE EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__); #endif if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR); EC_SLAVE_ERR(slave, "Failed to receive FoE DATA READ datagram: "); ec_datagram_print_state(fsm->datagram); return; } if (fsm->datagram->working_counter != 1) { ec_foe_set_rx_error(fsm, FOE_WC_ERROR); EC_SLAVE_ERR(slave, "Reception of FoE DATA READ failed: "); ec_datagram_print_wc_error(fsm->datagram); return; } data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size); if (IS_ERR(data)) { ec_foe_set_rx_error(fsm, FOE_MBOX_FETCH_ERROR); return; } if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", mbox_prot); ec_foe_set_rx_error(fsm, FOE_PROT_ERROR); return; } opCode = EC_READ_U8(data); if (opCode == EC_FOE_OPCODE_BUSY) { if (ec_foe_prepare_send_ack(fsm, datagram)) { ec_foe_set_rx_error(fsm, FOE_PROT_ERROR); } return; } if (opCode == EC_FOE_OPCODE_ERR) { fsm->request->error_code = EC_READ_U32(data + 2); EC_SLAVE_ERR(slave, "Received FoE Error Request (code 0x%08x).\n", fsm->request->error_code); if (rec_size > 6) { uint8_t text[256]; strncpy(text, data + 6, min(rec_size - 6, sizeof(text))); EC_SLAVE_ERR(slave, "FoE Error Text: %s\n", text); } ec_foe_set_rx_error(fsm, FOE_OPCODE_ERROR); return; } if (opCode != EC_FOE_OPCODE_DATA) { EC_SLAVE_ERR(slave, "Received OPCODE %x, expected %x.\n", opCode, EC_FOE_OPCODE_DATA); fsm->request->error_code = 0x00000000; ec_foe_set_rx_error(fsm, FOE_OPCODE_ERROR); return; } packet_no = EC_READ_U16(data + 2); if (packet_no != fsm->rx_expected_packet_no) { EC_SLAVE_ERR(slave, "Received unexpected packet number.\n"); ec_foe_set_rx_error(fsm, FOE_PACKETNO_ERROR); return; } rec_size -= EC_FOE_HEADER_SIZE; if (fsm->rx_buffer_size >= fsm->rx_buffer_offset + rec_size) { memcpy(fsm->rx_buffer + fsm->rx_buffer_offset, data + EC_FOE_HEADER_SIZE, rec_size); fsm->rx_buffer_offset += rec_size; } fsm->rx_last_packet = (rec_size + EC_MBOX_HEADER_SIZE + EC_FOE_HEADER_SIZE != slave->configured_rx_mailbox_size); if (fsm->rx_last_packet || (slave->configured_rx_mailbox_size - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE + fsm->rx_buffer_offset) <= fsm->rx_buffer_size) { // either it was the last packet or a new packet will fit into the // delivered buffer #ifdef DEBUG_FOE EC_SLAVE_DBG(fsm->slave, 0, "last_packet=true\n"); #endif if (ec_foe_prepare_send_ack(fsm, datagram)) { ec_foe_set_rx_error(fsm, FOE_RX_DATA_ACK_ERROR); return; } fsm->state = ec_fsm_foe_state_sent_ack; } else { // no more data fits into the delivered buffer // ... wait for new read request EC_SLAVE_ERR(slave, "Data do not fit in receive buffer!\n"); printk(" rx_buffer_size = %d\n", fsm->rx_buffer_size); printk("rx_buffer_offset = %d\n", fsm->rx_buffer_offset); printk(" rec_size = %zd\n", rec_size); printk(" rx_mailbox_size = %d\n", slave->configured_rx_mailbox_size); printk(" rx_last_packet = %d\n", fsm->rx_last_packet); fsm->request->result = FOE_READY; } }
void pdo_handle_ecat(master_setup_variables_t *master_setup, ctrlproto_slv_handle *slv_handles, unsigned int total_no_of_slaves) { int slv; if(sig_alarms == user_alarms) pause(); while (sig_alarms != user_alarms) { /* sync the dc clock of the slaves */ // ecrt_master_sync_slave_clocks(master); // receive process data ecrt_master_receive(master_setup->master); ecrt_domain_process(master_setup->domain); // check process data state (optional) //check_domain1_state(master_setup); // check for master state (optional) //check_master_state(master_setup); // check for islave configuration state(s) (optional) // check_slave_config_states(); for(slv=0;slv<total_no_of_slaves;++slv) { /* Read process data */ slv_handles[slv].motorctrl_status_in = EC_READ_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[0]); slv_handles[slv].operation_mode_disp = EC_READ_U8(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[1]); slv_handles[slv].position_in = EC_READ_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[2]); slv_handles[slv].speed_in = EC_READ_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[3]); slv_handles[slv].torque_in = EC_READ_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[4]); } /* printf("\n%x", slv_handles[slv].motorctrl_status_in); printf("\n%x", slv_handles[slv].operation_mode_disp); printf("\n%x", slv_handles[slv].position_in); printf("\n%x", slv_handles[slv].speed_in); printf("\n%x", slv_handles[slv].torque_in); */ for(slv=0;slv<total_no_of_slaves;++slv) { EC_WRITE_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[0], (slv_handles[slv].motorctrl_out)&0xffff); EC_WRITE_U8(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[1], (slv_handles[slv].operation_mode)&0xff); EC_WRITE_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[2], (slv_handles[slv].torque_setpoint)&0xffff); EC_WRITE_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[3], slv_handles[slv].position_setpoint); EC_WRITE_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[4], slv_handles[slv].speed_setpoint); } // send process data ecrt_domain_queue(master_setup->domain); ecrt_master_send(master_setup->master); //Check for master und domain state ecrt_master_state(master_setup->master, &master_setup->master_state); ecrt_domain_state(master_setup->domain, &master_setup->domain_state); if (master_setup->domain_state.wc_state == EC_WC_COMPLETE && !master_setup->op_flag) { //printf("System up!\n"); master_setup->op_flag = 1; } else { if(master_setup->domain_state.wc_state != EC_WC_COMPLETE && master_setup->op_flag) { //printf("System down!\n"); master_setup->op_flag = 0; } } user_alarms++; } }
void lcec_el5152_read(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, idx_flag; lcec_el5152_chan_t *chan; int32_t idx_count, raw_count, raw_delta; uint32_t raw_period; // wait for slave to be operational if (!slave->state.operational) { hal_data->last_operational = 0; return; } // check inputs for (i=0; i<LCEC_EL5152_CHANS; i++) { chan = &hal_data->chans[i]; // check for change in scale value if (*(chan->pos_scale) != chan->old_scale) { // scale value has changed, test and update it if ((*(chan->pos_scale) < 1e-20) && (*(chan->pos_scale) > -1e-20)) { // value too small, divide by zero is a bad thing *(chan->pos_scale) = 1.0; } // save new scale to detect future changes chan->old_scale = *(chan->pos_scale); // we actually want the reciprocal chan->scale = 1.0 / *(chan->pos_scale); } // get bit states *(chan->ina) = EC_READ_BIT(&pd[chan->ina_pdo_os], chan->ina_pdo_bp); *(chan->inb) = EC_READ_BIT(&pd[chan->inb_pdo_os], chan->inb_pdo_bp); *(chan->expol_stall) = EC_READ_BIT(&pd[chan->expol_stall_pdo_os], chan->expol_stall_pdo_bp); *(chan->tx_toggle) = EC_READ_BIT(&pd[chan->tx_toggle_pdo_os], chan->tx_toggle_pdo_bp); // read raw values raw_count = EC_READ_S32(&pd[chan->count_pdo_os]); raw_period = EC_READ_U32(&pd[chan->period_pdo_os]); // check for operational change of slave if (!hal_data->last_operational) { chan->last_count = raw_count; } // check for counter set done if (EC_READ_BIT(&pd[chan->set_count_done_pdo_os], chan->set_count_done_pdo_bp)) { chan->last_count = raw_count; *(chan->set_raw_count) = 0; } // update raw values if (! *(chan->set_raw_count)) { *(chan->raw_count) = raw_count; *(chan->raw_period) = raw_period; } // check for index edge idx_flag = 0; idx_count = 0; if (*(chan->index) && !chan->last_index) { idx_count = raw_count; idx_flag = 1; } chan->last_index = *(chan->index); // handle initialization if (chan->do_init || *(chan->reset)) { chan->do_init = 0; chan->last_count = raw_count; *(chan->count) = 0; idx_flag = 0; } // handle index if (idx_flag && *(chan->index_ena)) { chan->last_count = idx_count; *(chan->count) = 0; *(chan->index_ena) = 0; } // compute net counts raw_delta = raw_count - chan->last_count; chan->last_count = raw_count; *(chan->count) += raw_delta; // scale count to make floating point position *(chan->pos) = *(chan->count) * chan->scale; // scale period *(chan->period) = ((double) (*(chan->raw_period))) * LCEC_EL5152_PERIOD_SCALE; } hal_data->last_operational = 1; }
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 run(long data) { int i; struct timeval tv; unsigned int sync_ref_counter = 0; count2timeval(nano2count(rt_get_real_time_ns()), &tv); 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(); 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); 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 = 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 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(); } }
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); 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_U32(domain_w_pd + actual_pos); if (counter) { counter--; } else { // do this at 1 Hz counter = FREQUENCY; 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]&0x027f) == 0x0233) { EC_WRITE_U16(domain_r_pd+ctrl_word, 0x000f); EC_WRITE_S32(domain_r_pd+interpolateddata, 0); EC_WRITE_S32(domain_r_pd+max_torq, 0xf00); EC_WRITE_S32(domain_r_pd+modeofoper, 7); } //operation enabled else if( (temp[0]&0x027f) == 0x0237) { //600 800 EC_WRITE_S32(domain_r_pd+interpolateddata,( move_value+=1000 )); EC_WRITE_U16(domain_r_pd+ctrl_word, 0x001f); } // write application time to master 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); } }
/** Read PDO entry information. */ void ec_fsm_pdo_entry_read_state_entry( ec_fsm_pdo_entry_t *fsm, /**< finite state machine */ ec_datagram_t *datagram /**< Datagram to use. */ ) { if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) { return; } if (!ec_fsm_coe_success(fsm->fsm_coe)) { EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entry.\n"); fsm->state = ec_fsm_pdo_entry_state_error; return; } if (fsm->request.data_size != sizeof(uint32_t)) { EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu at" " uploading SDO 0x%04X:%02X.\n", fsm->request.data_size, fsm->request.index, fsm->request.subindex); fsm->state = ec_fsm_pdo_entry_state_error; } else { uint32_t pdo_entry_info; ec_pdo_entry_t *pdo_entry; pdo_entry_info = EC_READ_U32(fsm->request.data); if (!(pdo_entry = (ec_pdo_entry_t *) kmalloc(sizeof(ec_pdo_entry_t), GFP_KERNEL))) { EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO entry.\n"); fsm->state = ec_fsm_pdo_entry_state_error; return; } ec_pdo_entry_init(pdo_entry); pdo_entry->index = pdo_entry_info >> 16; pdo_entry->subindex = (pdo_entry_info >> 8) & 0xFF; pdo_entry->bit_length = pdo_entry_info & 0xFF; if (!pdo_entry->index && !pdo_entry->subindex) { if (ec_pdo_entry_set_name(pdo_entry, "Gap")) { ec_pdo_entry_clear(pdo_entry); kfree(pdo_entry); fsm->state = ec_fsm_pdo_entry_state_error; return; } } EC_SLAVE_DBG(fsm->slave, 1, "PDO entry 0x%04X:%02X, %u bit, \"%s\".\n", pdo_entry->index, pdo_entry->subindex, pdo_entry->bit_length, pdo_entry->name ? pdo_entry->name : "???"); list_add_tail(&pdo_entry->list, &fsm->target_pdo->entries); // next PDO entry fsm->entry_pos++; ec_fsm_pdo_entry_read_action_next(fsm, datagram); } }
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; }