/** Processes received mailbox data. * * \return Pointer to the received data, or ERR_PTR() code. */ uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */ const ec_datagram_t *datagram, /**< datagram */ uint8_t *type, /**< expected mailbox protocol */ size_t *size /**< size of the received data */ ) { size_t data_size; data_size = EC_READ_U16(datagram->data); if (data_size + EC_MBOX_HEADER_SIZE > slave->configured_tx_mailbox_size) { EC_SLAVE_ERR(slave, "Corrupt mailbox response received!\n"); ec_print_data(datagram->data, slave->configured_tx_mailbox_size); return ERR_PTR(-EPROTO); } *type = EC_READ_U8(datagram->data + 5) & 0x0F; *size = data_size; if (*type == 0x00) { const ec_code_msg_t *mbox_msg; uint16_t code = EC_READ_U16(datagram->data + 8); EC_SLAVE_ERR(slave, "Mailbox error response received - "); for (mbox_msg = mbox_error_messages; mbox_msg->code; mbox_msg++) { if (mbox_msg->code != code) continue; printk("Code 0x%04X: \"%s\".\n", mbox_msg->code, mbox_msg->message); break; } if (!mbox_msg->code) printk("Unknown error reply code 0x%04X.\n", code); if (slave->master->debug_level) ec_print_data(datagram->data + EC_MBOX_HEADER_SIZE, data_size); return ERR_PTR(-EPROTO); } return datagram->data + EC_MBOX_HEADER_SIZE; }
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 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; } }
/** EoE state: SET IP RESPONSE. */ void ec_fsm_eoe_set_ip_response( ec_fsm_eoe_t *fsm, /**< finite state machine */ ec_datagram_t *datagram /**< Datagram to use. */ ) { ec_slave_t *slave = fsm->slave; ec_master_t *master = slave->master; uint8_t *data, mbox_prot, frame_type; size_t rec_size; ec_eoe_request_t *req = fsm->request; if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. return; } if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { fsm->state = ec_fsm_eoe_error; EC_SLAVE_ERR(slave, "Failed to receive EoE read response datagram: "); ec_datagram_print_state(fsm->datagram); return; } if (fsm->datagram->working_counter != 1) { fsm->state = ec_fsm_eoe_error; EC_SLAVE_ERR(slave, "Reception of EoE read response 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)) { fsm->state = ec_fsm_eoe_error; return; } if (master->debug_level) { EC_SLAVE_DBG(slave, 0, "Set IP parameter response:\n"); ec_print_data(data, rec_size); } if (mbox_prot != EC_MBOX_TYPE_EOE) { fsm->state = ec_fsm_eoe_error; EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n", mbox_prot); return; } if (rec_size < 4) { fsm->state = ec_fsm_eoe_error; EC_SLAVE_ERR(slave, "Received currupted EoE set IP parameter response" " (%zu bytes)!\n", rec_size); ec_print_data(data, rec_size); return; } frame_type = EC_READ_U8(data) & 0x0f; if (frame_type != EC_EOE_FRAMETYPE_SET_IP_RES) { EC_SLAVE_ERR(slave, "Received no set IP parameter response" " (frame type %x).\n", frame_type); ec_print_data(data, rec_size); fsm->state = ec_fsm_eoe_error; return; } req->result = EC_READ_U16(data + 2); if (req->result) { fsm->state = ec_fsm_eoe_error; EC_SLAVE_DBG(slave, 1, "EoE set IP parameters failed with result code" " 0x%04X.\n", req->result); } else { fsm->state = ec_fsm_eoe_end; // success } }
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(); // 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 lcec_stmds5k_read(struct lcec_slave *slave, long period) { lcec_master_t *master = slave->master; lcec_stmds5k_data_t *hal_data = (lcec_stmds5k_data_t *) slave->hal_data; uint8_t *pd = master->process_data; int32_t index_tmp; int32_t pos_cnt, pos_cnt_diff; long long net_count; uint8_t dev_state; uint16_t speed_state; int16_t speed_raw, torque_raw; double torque; // wait for slave to be operational if (!slave->state.operational) { return; } // check for change in scale value lcec_stmds5k_check_scales(hal_data); // read device state dev_state = EC_READ_U8(&pd[hal_data->dev_state_pdo_os]); *(hal_data->ready) = (dev_state >> 0) & 0x01; *(hal_data->error) = (dev_state >> 1) & 0x01; *(hal_data->loc_ena) = (dev_state >> 6) & 0x01; *(hal_data->toggle) = (dev_state >> 7) & 0x01; // read speed state speed_state = EC_READ_U16(&pd[hal_data->speed_state_pdo_os]); *(hal_data->stopped) = (speed_state >> 0) & 0x01; *(hal_data->at_speed) = (speed_state >> 1) & 0x01; *(hal_data->overload) = (speed_state >> 2) & 0x01; // read current speed speed_raw = EC_READ_S16(&pd[hal_data->speed_mot_pdo_os]); *(hal_data->vel_fb_rpm) = hal_data->speed_max_rpm * (double)speed_raw * STMDS5K_PCT_REG_DIV; *(hal_data->vel_fb) = *(hal_data->vel_fb_rpm) * STMDS5K_RPM_DIV * hal_data->pos_scale_rcpt; // read torque // E02 : torque motor filterd (x 0,1 Nm) torque_raw = EC_READ_S16(&pd[hal_data->torque_mot_pdo_os]); torque = (double)torque_raw * STMDS5K_TORQUE_DIV; *(hal_data->torque_fb) = torque; torque = fabs(torque); *(hal_data->torque_fb_abs) = torque; *(hal_data->torque_fb_pct) = torque * hal_data->torque_reference_rcpt * 100.0; // update raw position counter pos_cnt = EC_READ_S32(&pd[hal_data->pos_mot_pdo_os]); *(hal_data->enc_raw) = pos_cnt; *(hal_data->on_home_neg) = (pos_cnt <= hal_data->home_raw); *(hal_data->on_home_pos) = (pos_cnt >= hal_data->home_raw); pos_cnt <<= 8; pos_cnt_diff = pos_cnt - hal_data->last_pos_cnt; hal_data->last_pos_cnt = pos_cnt; hal_data->pos_cnt += pos_cnt_diff; // check for index edge if (*(hal_data->index_ena)) { index_tmp = (hal_data->pos_cnt >> 32) & 0xffffffff; if (hal_data->do_init || !hal_data->last_index_ena) { hal_data->index_ref = index_tmp; } else if (index_tmp > hal_data->index_ref) { hal_data->index_cnt = (long long)index_tmp << 32; *(hal_data->index_ena) = 0; } else if (index_tmp < hal_data->index_ref) { hal_data->index_cnt = (long long)hal_data->index_ref << 32; *(hal_data->index_ena) = 0; } }
/** 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 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 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 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 }
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(); } }
int16_t ethercat_get_current(void) { return EC_READ_U16(rx_controller_state.current_val); }
void el60xx_port_run(el60xx_port_t *port, u8 *pd) { u16 status = EC_READ_U16(pd + port->off_status); u8 *rx_data = pd + port->off_rx; uint8_t tx_accepted_toggle, rx_request_toggle; switch (port->state) { case SER_READY: /* Check, if hardware handshaking has to be configured. */ if (!port->config_error && port->requested_rtscts != port->current_rtscts) { EC_WRITE_U8(ecrt_sdo_request_data(port->rtscts_sdo), port->requested_rtscts); ecrt_sdo_request_write(port->rtscts_sdo); port->state = SER_SET_RTSCTS; break; } /* Check, if the baud rate has to be configured. */ if (!port->config_error && port->requested_baud_rate != port->current_baud_rate) { EC_WRITE_U8(ecrt_sdo_request_data(port->baud_sdo), port->requested_baud_rate); ecrt_sdo_request_write(port->baud_sdo); port->state = SER_SET_BAUD_RATE; break; } /* Check, if the data frame has to be configured. */ if (!port->config_error && port->requested_data_frame != port->current_data_frame) { EC_WRITE_U8(ecrt_sdo_request_data(port->frame_sdo), port->requested_data_frame); ecrt_sdo_request_write(port->frame_sdo); port->state = SER_SET_DATA_FRAME; break; } /* Send data */ tx_accepted_toggle = status & 0x0001; if (tx_accepted_toggle != port->tx_accepted_toggle) { // ready port->tx_data_size = ectty_tx_data(port->tty, port->tx_data, port->max_tx_data_size); if (port->tx_data_size) { #if DEBUG printk(KERN_INFO PFX "%s: Sending %u bytes.\n", port->name, port->tx_data_size); #endif port->tx_request_toggle = !port->tx_request_toggle; port->tx_accepted_toggle = tx_accepted_toggle; } } /* Receive data */ rx_request_toggle = status & 0x0002; if (rx_request_toggle != port->rx_request_toggle) { uint8_t rx_data_size = status >> 8; port->rx_request_toggle = rx_request_toggle; #if DEBUG printk(KERN_INFO PFX "%s: Received %u bytes.\n", port->name, rx_data_size); #endif ectty_rx_data(port->tty, rx_data, rx_data_size); port->rx_accepted_toggle = !port->rx_accepted_toggle; } port->control = port->tx_request_toggle | port->rx_accepted_toggle << 1 | port->tx_data_size << 8; break; case SER_REQUEST_INIT: if (status & (1 << 2)) { port->control = 0x0000; port->state = SER_WAIT_FOR_INIT_RESPONSE; } else { port->control = 1 << 2; // CW.2, request initialization } break; case SER_WAIT_FOR_INIT_RESPONSE: if (!(status & (1 << 2))) { printk(KERN_INFO PFX "%s: Init successful.\n", port->name); port->tx_accepted_toggle = 1; port->control = 0x0000; port->state = SER_READY; } break; case SER_SET_RTSCTS: switch (ecrt_sdo_request_state(port->rtscts_sdo)) { case EC_REQUEST_SUCCESS: printk(KERN_INFO PFX "%s: Accepted RTS/CTS.\n", port->name); port->current_rtscts = port->requested_rtscts; port->state = SER_REQUEST_INIT; break; case EC_REQUEST_ERROR: printk(KERN_ERR PFX "Failed to set RTS/CTS on %s!\n", port->name); port->state = SER_REQUEST_INIT; port->config_error = 1; break; default: break; } break; case SER_SET_BAUD_RATE: switch (ecrt_sdo_request_state(port->baud_sdo)) { case EC_REQUEST_SUCCESS: printk(KERN_INFO PFX "%s: Accepted baud rate.\n", port->name); port->current_baud_rate = port->requested_baud_rate; port->state = SER_REQUEST_INIT; break; case EC_REQUEST_ERROR: printk(KERN_ERR PFX "Failed to set baud rate on %s!\n", port->name); port->state = SER_REQUEST_INIT; port->config_error = 1; break; default: break; } break; case SER_SET_DATA_FRAME: switch (ecrt_sdo_request_state(port->frame_sdo)) { case EC_REQUEST_SUCCESS: printk(KERN_INFO PFX "%s: Accepted data frame.\n", port->name); port->current_data_frame = port->requested_data_frame; port->state = SER_REQUEST_INIT; break; case EC_REQUEST_ERROR: printk(KERN_ERR PFX "Failed to set data frame on %s!\n", port->name); port->state = SER_REQUEST_INIT; port->config_error = 1; break; default: break; } break; }
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; }