sr0x.cpp
Go to the documentation of this file.
00001 
00027 #include <sr_edc_ethercat_drivers/sr0x.h>
00028 
00029 #include <dll/ethercat_dll.h>
00030 #include <al/ethercat_AL.h>
00031 #include <dll/ethercat_device_addressed_telegram.h>
00032 #include <dll/ethercat_frame.h>
00033 
00034 #include <sstream>
00035 #include <iomanip>
00036 #include <boost/foreach.hpp>
00037 
00038 #include <math.h>
00039 
00040 PLUGINLIB_REGISTER_CLASS(5, SR0X, EthercatDevice);
00041 
00042 SR0X::SR0X() : EthercatDevice()
00043 {
00044 }
00045 
00046 SR0X::~SR0X()
00047 {
00048   delete sh_->get_fmmu_config();
00049   delete sh_->get_pd_config();
00050 }
00051 
00052 void SR0X::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00053 {
00054   EthercatDevice::construct(sh,start_address);
00055 }
00056 
00057 int SR0X::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00058 {
00059   ROS_DEBUG("Device #%02d: SR0%d (%#08x) Firmware Revision %d.%02d, PCB Revision %c.%02d, Serial #: %d",
00060             sh_->get_ring_position(),
00061             sh_->get_product_code() % 100,
00062             sh_->get_product_code(), fw_major_, fw_minor_,
00063             'A' + board_major_, board_minor_,
00064             sh_->get_serial());
00065 
00066   device_offset_ = sh_->get_ring_position();// - hand_->getBridgeRingPosition();
00067 
00068   return 0;
00069 }
00070 
00071 int SR0X::readData(EthercatCom *com, EC_UINT address, void *data, EC_UINT length)
00072 {
00073   return EthercatDevice::readData(com, address, data, length, FIXED_ADDR);
00074 }
00075 
00076 
00077 int SR0X::writeData(EthercatCom *com, EC_UINT address, void const *data, EC_UINT length)
00078 {
00079   return EthercatDevice::writeData(com, sh_, address, data, length, FIXED_ADDR);
00080 }
00081 


sr_edc_ethercat_drivers
Author(s): Yann Sionneau, Ugo Cupcic / ugo@shadowrobot.com, software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:38:25