Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00025 #include <sr_ronex_drivers/sr_board_0x.h>
00026
00027 #include <ros_ethercat_hardware/ethercat_hardware.h>
00028
00029 #include <sstream>
00030 #include <iomanip>
00031 #include <boost/foreach.hpp>
00032
00033 #include <math.h>
00034
00035
00036 #define ETHERCAT_COMMAND_DATA_ADDRESS 0x1000
00037
00038 PLUGINLIB_EXPORT_CLASS(SrBoard0X, EthercatDevice);
00039
00040 void SrBoard0X::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00041 {
00042 sh_ = sh;
00043
00044
00045
00046
00047
00048 n_digital_outputs = ((sh->get_product_code() & 0x00000F00) >> 8) * 2;
00049 n_digital_inputs = ((sh->get_product_code() & 0x00000F00) >> 8);
00050 n_analog_outputs = ((sh->get_product_code() & 0x000000F0) >> 4);
00051 n_analog_inputs = ((sh->get_product_code() & 0x0000000F));
00052 n_PWM_outputs = n_digital_outputs / 2;
00053
00054 command_base_ = start_address;
00055 command_size_ = ((n_digital_outputs/16 + 1) * 2) + (n_PWM_outputs * 4) + (n_analog_outputs * 2);
00056
00057 start_address += command_size_;
00058
00059 status_base_ = start_address;
00060 status_size_ = ((n_digital_inputs/16 + 1) * 2) + (n_analog_inputs * 2);
00061
00062 start_address += status_size_;
00063
00064
00065
00066
00067
00068
00069 ROS_INFO("First FMMU (command) : start_address : 0x%08X ; size : %3d bytes ; phy addr : 0x%08X", command_base_, command_size_,
00070 static_cast<int>(ETHERCAT_COMMAND_DATA_ADDRESS) );
00071 EC_FMMU *commandFMMU = new EC_FMMU( command_base_,
00072 command_size_,
00073 0x00,
00074 0x07,
00075 ETHERCAT_COMMAND_DATA_ADDRESS,
00076 0x00,
00077 false,
00078 true,
00079 true
00080 );
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092 ROS_INFO("Second FMMU (status) : start_address : 0x%08X ; size : %3d bytes ; phy addr : 0x%08X", status_base_, status_size_,
00093 static_cast<int>(ETHERCAT_COMMAND_DATA_ADDRESS + command_size_) );
00094 EC_FMMU *statusFMMU = new EC_FMMU( status_base_,
00095 status_size_,
00096 0x00,
00097 0x07,
00098 ETHERCAT_COMMAND_DATA_ADDRESS + (command_size_ * 4),
00099 0x00,
00100 true,
00101 false,
00102 true);
00103
00104
00105 EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(2);
00106
00107 (*fmmu)[0] = *commandFMMU;
00108 (*fmmu)[1] = *statusFMMU;
00109
00110 sh->set_fmmu_config(fmmu);
00111
00112 EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(2);
00113
00114 (*pd)[0] = EC_SyncMan(ETHERCAT_COMMAND_DATA_ADDRESS, command_size_, EC_BUFFERED, EC_WRITTEN_FROM_MASTER);
00115 (*pd)[1] = EC_SyncMan(ETHERCAT_COMMAND_DATA_ADDRESS + (command_size_ * 4), status_size_, EC_BUFFERED);
00116
00117
00118 (*pd)[0].ChannelEnable = true;
00119 (*pd)[0].ALEventEnable = true;
00120 (*pd)[0].WriteEvent = true;
00121
00122 (*pd)[1].ChannelEnable = true;
00123
00124 sh->set_pd_config(pd);
00125
00126 ROS_INFO("status_size_ : %d ; command_size_ : %d", status_size_, command_size_);
00127
00128 ROS_INFO("Finished constructing the SrBoard0X driver");
00129 }
00130
00131 void SrBoard0X::packCommand(unsigned char *buffer, bool halt, bool reset)
00132 {
00133 cod_decod_manager_->build_command(buffer);
00134 }
00135
00136 bool SrBoard0X::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00137 {
00138 unsigned char *status_data_ptr;
00139
00140
00141 status_data_ptr = this_buffer + command_size_;
00142
00143 cod_decod_manager_->update(status_data_ptr);
00144
00145 return true;
00146 }
00147
00148