├── CMakeLists.txt ├── Makefile ├── soem_beckhoff_drivers ├── CMakeLists.txt ├── Makefile ├── mainpage.dox ├── manifest.xml ├── msg │ ├── AnalogMsg.msg │ ├── CommMsg.msg │ ├── CommMsgBig.msg │ ├── DigitalMsg.msg │ ├── EncoderInMsg.msg │ ├── EncoderMsg.msg │ ├── EncoderOutMsg.msg │ └── PSUMsg.msg ├── package.xml └── src │ ├── COE_config.h │ ├── soem_beckhoff_drivers.cpp │ ├── soem_el1xxx.cpp │ ├── soem_el1xxx.h │ ├── soem_el2xxx.cpp │ ├── soem_el2xxx.h │ ├── soem_el3062.cpp │ ├── soem_el3062.h │ ├── soem_el30xx.cpp │ ├── soem_el30xx.h │ ├── soem_el3102.cpp │ ├── soem_el3102.h │ ├── soem_el3104.cpp │ ├── soem_el3104.h │ ├── soem_el4xxx.cpp │ ├── soem_el4xxx.h │ ├── soem_el5101.cpp │ ├── soem_el5101.h │ ├── soem_el6022.cpp │ └── soem_el6022.h ├── soem_core ├── CATKIN_IGNORE ├── CMakeLists.txt ├── Makefile ├── SOEM1.2.5.tar.bz2.md5sum ├── mainpage.dox ├── manifest.xml ├── rtnet.patch ├── soem.patch └── trinamic.patch ├── soem_ebox ├── CMakeLists.txt ├── Makefile ├── manifest.xml ├── msg │ ├── EBOXAnalog.msg │ ├── EBOXDigital.msg │ ├── EBOXOut.msg │ └── EBOXPWM.msg ├── package.xml └── src │ ├── soem_ebox.cpp │ └── soem_ebox.h ├── soem_master ├── CMakeLists.txt ├── Makefile ├── mainpage.dox ├── manifest.xml ├── package.xml ├── setcap.sh ├── soem_driver.h ├── soem_driver_factory.cpp ├── soem_driver_factory.h ├── soem_master_component.cpp ├── soem_master_component.h ├── soem_master_test.launch ├── soem_plugin.cpp └── test.ops └── stack.xml /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of 5 | # directories (or patterns, but directories should suffice) that should 6 | # be excluded from the distro. This is not the place to put things that 7 | # should be ignored everywhere, like "build" directories; that happens in 8 | # rosbuild/rosbuild.cmake. Here should be listed packages that aren't 9 | # ready for inclusion in a distro. 10 | # 11 | # This list is combined with the list in rosbuild/rosbuild.cmake. Note 12 | # that CMake 2.6 may be required to ensure that the two lists are combined 13 | # properly. CMake 2.4 seems to have unpredictable scoping rules for such 14 | # variables. 15 | #list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) 16 | 17 | rosbuild_make_distribution(0.1.0) 18 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake_stack.mk -------------------------------------------------------------------------------- /soem_beckhoff_drivers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6.3) 2 | 3 | project(soem_beckhoff_drivers) 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | 13 | # Do setup in case of ros package, If ROS_ROOT is set, it is # recommended to use RTT/OCL through the ros packages. 14 | #uncomment if you have defined messages 15 | if(NOT ORO_USE_ROSBUILD) 16 | find_package(catkin REQUIRED message_generation rtt_roscomm) 17 | add_message_files( FILES 18 | AnalogMsg.msg 19 | CommMsgBig.msg 20 | CommMsg.msg 21 | DigitalMsg.msg 22 | EncoderMsg.msg 23 | EncoderInMsg.msg 24 | EncoderOutMsg.msg 25 | PSUMsg.msg 26 | ) 27 | 28 | generate_messages() 29 | endif() 30 | # Set the CMAKE_PREFIX_PATH in case you're not using Orocos through 31 | # ROS for helping these find commands find RTT. 32 | find_package(OROCOS-RTT REQUIRED ${RTT_HINTS} ) 33 | # Defines the orocos_* cmake macros. See that file for additional 34 | # documentation. 35 | include(${OROCOS-RTT_USE_FILE_PATH}/UseOROCOS-RTT.cmake) 36 | 37 | if(ORO_USE_ROSBUILD) 38 | rosbuild_genmsg() 39 | rosbuild_include(rtt_rosnode GenerateRTTtypekit) 40 | endif() 41 | 42 | ros_generate_rtt_typekit(soem_beckhoff_drivers) 43 | 44 | find_package(soem REQUIRED) 45 | include_directories(${soem_INCLUDE_DIRS}) 46 | 47 | orocos_use_package(soem_master) 48 | 49 | orocos_plugin(soem_el1xxx src/soem_beckhoff_drivers.cpp src/soem_el1xxx.cpp) 50 | target_link_libraries(soem_el1xxx ${soem_LIBRARIES}) 51 | orocos_plugin(soem_el2xxx src/soem_beckhoff_drivers.cpp src/soem_el2xxx.cpp) 52 | target_link_libraries(soem_el2xxx ${soem_LIBRARIES}) 53 | orocos_plugin(soem_el4xxx src/soem_beckhoff_drivers.cpp src/soem_el4xxx.cpp) 54 | target_link_libraries(soem_el4xxx ${soem_LIBRARIES}) 55 | orocos_plugin(soem_el3102 src/soem_beckhoff_drivers.cpp src/soem_el3102.cpp) 56 | target_link_libraries(soem_el3102 ${soem_LIBRARIES}) 57 | orocos_plugin(soem_el5101 src/soem_beckhoff_drivers.cpp src/soem_el5101.cpp) 58 | target_link_libraries(soem_el5101 ${soem_LIBRARIES}) 59 | orocos_plugin(soem_el6022 src/soem_beckhoff_drivers.cpp src/soem_el6022.cpp) 60 | target_link_libraries(soem_el6022 ${soem_LIBRARIES}) 61 | orocos_plugin(soem_el30xx src/soem_beckhoff_drivers.cpp src/soem_el30xx.cpp) 62 | target_link_libraries(soem_el30xx ${soem_LIBRARIES}) 63 | orocos_plugin(soem_el3104 src/soem_beckhoff_drivers.cpp src/soem_el3104.cpp) 64 | target_link_libraries(soem_el3104 ${soem_LIBRARIES}) 65 | 66 | if(NOT ORO_USE_ROSBUILD) 67 | add_dependencies(soem_el1xxx ${PROJECT_NAME}_generate_messages_cpp) 68 | add_dependencies(soem_el2xxx ${PROJECT_NAME}_generate_messages_cpp) 69 | add_dependencies(soem_el4xxx ${PROJECT_NAME}_generate_messages_cpp) 70 | add_dependencies(soem_el3102 ${PROJECT_NAME}_generate_messages_cpp) 71 | add_dependencies(soem_el5101 ${PROJECT_NAME}_generate_messages_cpp) 72 | add_dependencies(soem_el6022 ${PROJECT_NAME}_generate_messages_cpp) 73 | add_dependencies(soem_el30xx ${PROJECT_NAME}_generate_messages_cpp) 74 | add_dependencies(soem_el3104 ${PROJECT_NAME}_generate_messages_cpp) 75 | endif() 76 | orocos_generate_package() 77 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/Makefile: -------------------------------------------------------------------------------- 1 | EXTRA_CMAKE_FLAGS += -DORO_USE_ROSBUILD=True 2 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /soem_beckhoff_drivers/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b soem_beckhoff_drivers is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | soem_beckhoff_drivers contains drivers for the ethercat beckhoff modules to work together with the soem_master package, every module creates the necessary services, dataports and properties for its own functionality. 5 | 6 | 7 | Ruben Smits 8 | BSD 9 | 10 | http://ros.org/wiki/soem_beckhoff_drivers 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/msg/AnalogMsg.msg: -------------------------------------------------------------------------------- 1 | float32[] values 2 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/msg/CommMsg.msg: -------------------------------------------------------------------------------- 1 | uint8[] datapacket 2 | uint8 datasize -------------------------------------------------------------------------------- /soem_beckhoff_drivers/msg/CommMsgBig.msg: -------------------------------------------------------------------------------- 1 | CommMsg[] channels 2 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/msg/DigitalMsg.msg: -------------------------------------------------------------------------------- 1 | bool[] values 2 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/msg/EncoderInMsg.msg: -------------------------------------------------------------------------------- 1 | uint8 status 2 | uint16 value 3 | uint16 latch 4 | uint32 frequency 5 | uint16 period 6 | uint16 window 7 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/msg/EncoderMsg.msg: -------------------------------------------------------------------------------- 1 | uint16 value 2 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/msg/EncoderOutMsg.msg: -------------------------------------------------------------------------------- 1 | uint8 control 2 | uint16 outvalue 3 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/msg/PSUMsg.msg: -------------------------------------------------------------------------------- 1 | bool power_ok 2 | bool overload -------------------------------------------------------------------------------- /soem_beckhoff_drivers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | soem_beckhoff_drivers 3 | 0.1.1 4 | 5 | soem_beckhoff_drivers contains drivers for the ethercat beckhoff modules to work together with the soem_master package, every module creates the necessary services, dataports and properties for its own functionality. 6 | 7 | 8 | Ruben Smits 9 | 10 | BSD 11 | 12 | http://wiki.ros.org/soem_master 13 | 14 | 15 | Ruben Smits 16 | 17 | catkin 18 | 19 | rtt 20 | soem 21 | soem_master 22 | message_generation 23 | rtt_roscomm 24 | 25 | rtt 26 | soem 27 | soem_master 28 | message_runtime 29 | rtt_roscomm 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/COE_config.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:30:46 CET 2010 COE_config.h 3 | 4 | COE_config.h - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | 29 | #ifndef COE_CONFIG_H 30 | #define COE_CONFIG_H 31 | 32 | extern "C"{ 33 | #include "ethercattype.h" 34 | #include "nicdrv.h" 35 | #include "ethercatbase.h" 36 | #include "ethercatmain.h" 37 | #include "ethercatconfig.h" 38 | #include "ethercatcoe.h" 39 | #include "ethercatdc.h" 40 | #include "ethercatprint.h" 41 | } 42 | 43 | 44 | typedef struct{ 45 | uint16 index; 46 | uint8 subindex; 47 | uint8 size; 48 | int param; 49 | std::string name; 50 | std::string description; 51 | }parameter; 52 | 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_beckhoff_drivers.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:31:20 CET 2010 soem_beckhoff_drivers.cpp 3 | 4 | soem_beckhoff_drivers.cpp - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | 29 | #include 30 | 31 | extern "C" { 32 | bool loadRTTPlugin(RTT::TaskContext* c){ 33 | return true; 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el1xxx.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:31:20 CET 2010 soem_el1xxx.cpp 3 | 4 | soem_el1xxx.cpp - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits, Koen Buys 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #include "soem_el1xxx.h" 29 | #include 30 | 31 | using namespace RTT; 32 | 33 | namespace soem_beckhoff_drivers 34 | { 35 | 36 | SoemEL1xxx::SoemEL1xxx(ec_slavet* mem_loc) : 37 | soem_master::SoemDriver(mem_loc), m_port("bits") 38 | { 39 | m_service->doc(std::string("Services for Beckhoff ") + std::string( 40 | m_datap->name) + std::string(" Dig. Input module")); 41 | m_service->addOperation("isOn", &SoemEL1xxx::isOn, this, RTT::OwnThread).doc( 42 | "Check if bit i is on").arg("i", "bit nr"); 43 | m_service->addOperation("isOff", &SoemEL1xxx::isOff, this, RTT::OwnThread).doc( 44 | "Check if bit i is off").arg("i", "bit nr"); 45 | m_service->addOperation("readBit", &SoemEL1xxx::readBit, this, 46 | RTT::OwnThread).doc("Read value of bit i").arg("i", "bit nr"); 47 | m_service->addConstant("size", m_size); 48 | 49 | m_service->addPort(m_port).doc("Data port to communicate full bitsets"); 50 | } 51 | 52 | bool SoemEL1xxx::start(){ 53 | m_size = m_datap->Ibits; 54 | m_msg.values.resize(m_size); 55 | m_port.setDataSample(m_msg); 56 | return m_size != 0; 57 | } 58 | 59 | bool SoemEL1xxx::isOn(unsigned int bit) const 60 | { 61 | return readBit(bit); 62 | } 63 | 64 | bool SoemEL1xxx::isOff(unsigned int bit) const 65 | { 66 | return !readBit(bit); 67 | } 68 | 69 | bool SoemEL1xxx::readBit(unsigned int bit) const 70 | { 71 | if (bit < m_size) 72 | return m_bits[m_datap->Istartbit + bit]; 73 | else 74 | { 75 | log(Error) << "Could not read bit " << bit 76 | << ", outside range of this module" << endlog(); 77 | return false; 78 | } 79 | } 80 | 81 | void SoemEL1xxx::update() 82 | { 83 | m_bits = ((out_el1xxxt*) (m_datap->inputs))->bits; 84 | for (unsigned int i = 0; i < m_size; i++) 85 | m_msg.values[i] = m_bits[m_datap->Istartbit + i]; 86 | m_port.write(m_msg); 87 | } 88 | 89 | #if 0 90 | unsigned int SoemEL1xxx::readSequence(unsigned int start_bit, unsigned int stop_bit) const 91 | { 92 | if(start_bitinputs))->outbits; 95 | std::bitset<8> out_bits; 96 | unsigned int j=0; 97 | for(unsigned int i=start_bit;i<=stop_bit;i++) 98 | out_bits.set(j,m_bits[datap_->Istartbit+i]); 99 | return out_bits.to_ulong(); 100 | } 101 | } 102 | #endif 103 | 104 | namespace 105 | { 106 | soem_master::SoemDriver* createSoemEL1xxx(ec_slavet* mem_loc) 107 | { 108 | return new SoemEL1xxx(mem_loc); 109 | } 110 | 111 | REGISTER_SOEM_DRIVER(EL1124, createSoemEL1xxx) 112 | REGISTER_SOEM_DRIVER(EL1144, createSoemEL1xxx) 113 | REGISTER_SOEM_DRIVER(EL1004, createSoemEL1xxx) 114 | REGISTER_SOEM_DRIVER(EL1008, createSoemEL1xxx) 115 | REGISTER_SOEM_DRIVER(EL1098, createSoemEL1xxx) 116 | } 117 | } 118 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el1xxx.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:30:46 CET 2010 soem_el1xxx.h 3 | 4 | soem_el1xxx.h - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | 29 | #ifndef SOEM_EL1xxx_H 30 | #define SOEM_EL1xxx_H 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace soem_beckhoff_drivers{ 38 | 39 | class SoemEL1xxx : public soem_master::SoemDriver 40 | { 41 | 42 | typedef struct PACKED 43 | { 44 | uint8 bits; 45 | } out_el1xxxt; 46 | 47 | public: 48 | SoemEL1xxx(ec_slavet* mem_loc); 49 | ~SoemEL1xxx(){}; 50 | 51 | bool isOn( unsigned int bit = 0) const; 52 | bool isOff( unsigned int bit = 0) const; 53 | bool readBit( unsigned int bit = 0) const; 54 | 55 | void update(); 56 | bool start(); 57 | 58 | 59 | private: 60 | unsigned int m_size; 61 | DigitalMsg m_msg; 62 | std::bitset<8> m_bits; 63 | RTT::OutputPort m_port; 64 | 65 | 66 | }; 67 | 68 | } 69 | #endif 70 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el2xxx.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:31:20 CET 2010 soem_el2xxx.cpp 3 | 4 | soem_el2xxx.cpp - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #include "soem_el2xxx.h" 29 | #include 30 | 31 | namespace soem_beckhoff_drivers 32 | { 33 | using namespace RTT; 34 | 35 | SoemEL2xxx::SoemEL2xxx(ec_slavet* mem_loc) : 36 | soem_master::SoemDriver(mem_loc), m_port("bits") 37 | { 38 | m_service->doc(std::string("Services for Beckhoff ") + std::string( 39 | m_datap->name) + std::string(" Dig. Output module")); 40 | m_service->addOperation("switchOn", &SoemEL2xxx::switchOn, this, 41 | RTT::OwnThread).doc("Switch bit i on").arg("i", "bit nr"); 42 | m_service->addOperation("switchOff", &SoemEL2xxx::switchOff, this, 43 | RTT::OwnThread).doc("Switch bit i off").arg("i", "bit nr"); 44 | m_service->addOperation("setBit", &SoemEL2xxx::setBit, this, RTT::OwnThread).doc( 45 | "Set value of bit i to val").arg("i", "bit nr").arg("val", 46 | "new value for bit"); 47 | m_service->addOperation("checkBit", &SoemEL2xxx::checkBit, this, 48 | RTT::OwnThread).doc("Check value of bit i").arg("i", "bit nr"); 49 | m_service->addConstant("size", m_size); 50 | m_service->addPort(m_port).doc( 51 | "DigitalMsg containing the desired values of _all_ bits"); 52 | } 53 | 54 | bool SoemEL2xxx::start(){ 55 | m_size = m_datap->Obits; 56 | m_mask.reset(); 57 | for (size_t i = m_datap->Ostartbit; i < m_datap->Ostartbit+m_size; i++) 58 | m_mask.set(i); 59 | m_bits = ~m_mask; 60 | 61 | m_msg.values.resize(m_size); 62 | 63 | return m_size != 0; 64 | } 65 | 66 | void SoemEL2xxx::update() 67 | { 68 | if (m_port.connected()) 69 | { 70 | if (m_port.read(m_msg) == RTT::NewData) 71 | { 72 | if (m_msg.values.size() == m_size) 73 | { 74 | for (unsigned int i = 0; i < m_size; i++) 75 | setBit(i, m_msg.values[i]); 76 | } 77 | } 78 | } 79 | std::bitset < 8 > tmp = m_mask | std::bitset<8> ( 80 | ((out_el2xxxt*) (m_datap->outputs))->bits);//xxxx1111 81 | ((out_el2xxxt*) (m_datap->outputs))->bits = (tmp & m_bits).to_ulong(); 82 | 83 | } 84 | 85 | bool SoemEL2xxx::setBit(unsigned int bit, bool value) 86 | { 87 | if (bit < m_size) 88 | { 89 | m_bits.set(bit + m_datap->Ostartbit, value); 90 | return true; 91 | } 92 | else 93 | log(Error) << "bit outside of slave range" << endlog(); 94 | return false; 95 | } 96 | 97 | bool SoemEL2xxx::switchOn(unsigned int n) 98 | { 99 | return this->setBit(n, true); 100 | } 101 | 102 | bool SoemEL2xxx::switchOff(unsigned int n) 103 | { 104 | return this->setBit(n, false); 105 | } 106 | 107 | #if 0 108 | void SoemEL2xxx::setSequence(unsigned int start_bit,unsigned int stop_bit,unsigned int value) 109 | { 110 | if(start_bitOstartbit,in_bits[i]); 114 | } 115 | } 116 | 117 | unsigned int SoemEL2xxx::checkSequence(unsigned int start_bit,unsigned int stop_bit)const 118 | { 119 | if(start_bit out_bits; 122 | out_bits.reset(); 123 | unsigned int j=0; 124 | for(unsigned int i=start_bit;i<=stop_bit;i++) 125 | out_bits.set(j,m_bits[m_datap->Ostartbit+i]); 126 | return out_bits.to_ulong(); 127 | } 128 | } 129 | #endif 130 | 131 | bool SoemEL2xxx::checkBit(unsigned int bit) const 132 | { 133 | if (bit < m_size) 134 | return m_bits.test(bit + m_datap->Ostartbit); 135 | else 136 | log(Error) << "bit outside of slave range" << endlog(); 137 | return false; 138 | } 139 | 140 | namespace 141 | { 142 | soem_master::SoemDriver* createSoemEL2xxx(ec_slavet* mem_loc) 143 | { 144 | return new SoemEL2xxx(mem_loc); 145 | } 146 | 147 | REGISTER_SOEM_DRIVER(EL2002, createSoemEL2xxx) 148 | REGISTER_SOEM_DRIVER(EL2004, createSoemEL2xxx) 149 | REGISTER_SOEM_DRIVER(EL2008, createSoemEL2xxx) 150 | REGISTER_SOEM_DRIVER(EL2034, createSoemEL2xxx) 151 | REGISTER_SOEM_DRIVER(EL2124, createSoemEL2xxx) 152 | REGISTER_SOEM_DRIVER(EL2612, createSoemEL2xxx) 153 | REGISTER_SOEM_DRIVER(EL2624, createSoemEL2xxx) 154 | 155 | } 156 | } 157 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el2xxx.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:30:46 CET 2010 soem_el2xxx.h 3 | 4 | soem_el2xxx.h - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | 29 | #ifndef SOEM_EL2XXX_H 30 | #define SOEM_EL2XXX_H 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace soem_beckhoff_drivers{ 38 | class SoemEL2xxx : public soem_master::SoemDriver 39 | { 40 | 41 | typedef struct PACKED 42 | { 43 | uint8 bits; 44 | } out_el2xxxt; 45 | 46 | public: 47 | SoemEL2xxx(ec_slavet* mem_loc); 48 | ~SoemEL2xxx(){}; 49 | 50 | bool switchOn( unsigned int n ); 51 | bool switchOff( unsigned int n ); 52 | bool setBit( unsigned int bit, bool value ); 53 | bool checkBit(unsigned int n) const; 54 | 55 | void update(); 56 | bool start(); 57 | 58 | private: 59 | unsigned int m_size; 60 | DigitalMsg m_msg; 61 | RTT::InputPort m_port; 62 | std::bitset<8> m_bits; 63 | std::bitset<8> m_mask; 64 | }; 65 | 66 | } 67 | #endif 68 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el3062.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:31:20 CET 2010 soem_el3062.cpp 3 | 4 | soem_el3062.cpp - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #include "soem_el3062.h" 29 | #include 30 | #include 31 | #include 32 | 33 | using namespace RTT; 34 | 35 | namespace soem_beckhoff_drivers 36 | { 37 | 38 | SoemEL30xx::SoemEL30xx(ec_slavet* mem_loc) : 39 | soem_master::SoemDriver(mem_loc), m_size(2), m_raw_range(32767), m_lowest( 40 | 0.0), m_highest(10.0), m_values(m_size), m_raw_values(m_size), 41 | m_values_port("values"), m_raw_values_port("raw_values") 42 | 43 | { 44 | m_service->doc(std::string("Services for Beckhoff ") + std::string( 45 | m_datap->name) + std::string(" module")); 46 | m_service->addOperation("rawRead", &SoemEL3062::rawRead, this, 47 | RTT::OwnThread).doc("Read raw value of channel i").arg("i", 48 | "channel nr"); 49 | m_service->addOperation("read", &SoemEL3062::read, this, RTT::OwnThread).doc( 50 | "Read value to channel i").arg("i", "channel nr"); 51 | m_service->addOperation("Over_Range", &SoemEL3062::isOverrange, this, 52 | RTT::OwnThread).doc( 53 | "For the channel i : 1 = overrange ; 0 = no overrange ").arg("i", 54 | "channel nr"); 55 | m_service->addOperation("Under_Range", &SoemEL3062::isUnderrange, this, 56 | RTT::OwnThread).doc( 57 | "For the channel i : 1 = Underrange ; 0 = no Underrange ").arg("i", 58 | "channel nr"); 59 | m_service->addOperation("Comp_val_to_lim", &SoemEL3062::checkLimit, 60 | this, RTT::OwnThread).doc( 61 | "Limit 1/2 value monitoring of channel i : 0= not active, 1= Value is higher than limit 1/2 value, 2= Value is lower than limit 1/2 value, 3: Value equals limit 1/2 value").arg( 62 | "i", "channel nr").arg("x", "Limit nr"); 63 | m_service->addOperation("Error", &SoemEL3062::is_error, this).doc( 64 | "For the channel i : 1 = error (Overrange or Underrange ; 0 = no error ").arg( 65 | "i", "channel nr"); 66 | 67 | m_resolution = ((m_highest - m_lowest) / (double) m_raw_range); 68 | 69 | m_service->addConstant("raw_range", m_raw_range); 70 | m_service->addConstant("resolution", m_resolution); 71 | m_service->addConstant("lowest", m_lowest); 72 | m_service->addConstant("highest", m_highest); 73 | 74 | m_service->addPort(m_values_port).doc( 75 | "AnalogMsg contain the read values of _all_ channels"); 76 | m_service->addPort(m_raw_values_port).doc( 77 | "AnalogMsg containing the read values of _all_ channels"); 78 | 79 | m_msg.values.resize(m_size); 80 | m_raw_msg.values.resize(m_size); 81 | #if 0 82 | // New properties : Component Configuration : Can be completed 83 | // Need to be complete for a specific usage : See Beckhoff : http://www.beckhoff.com/EL3062/ 84 | //see documentation (.xchm) / Commissionning / Object description and parameterization 85 | 86 | parameter temp; 87 | 88 | /// For parametrisation : uncomment the following code and complete it with using the document above 89 | temp.description = "Enable user scale"; 90 | temp.index = 0x8010; 91 | temp.subindex = 0x01; 92 | temp.name = "E_user_scl"; 93 | temp.size = 1; 94 | temp.param = 1; 95 | params.push_back(temp); 96 | 97 | temp.description = "Limit1 for channel 1"; 98 | temp.index = 0x8010; 99 | temp.subindex = 0x13; 100 | temp.name = "Limit1_chan1"; 101 | temp.size = 2; 102 | temp.param = 9174; 103 | params.push_back(temp); 104 | 105 | temp.description = "Limit2 for channel1"; 106 | temp.index = 0x8010; 107 | temp.subindex = 0x14; 108 | temp.name = "Limit2_chan1"; 109 | temp.size = 2; 110 | temp.param = 24247; 111 | params.push_back(temp); 112 | 113 | // Adding properties to the Soem_master_Component. 114 | for (unsigned int i = 0; i < params.size(); i++) 115 | { 116 | m_service->addProperty(params[i].name, params[i].param).doc( 117 | params[i].description); 118 | } 119 | #endif 120 | } 121 | 122 | bool SoemEL3062::configure() 123 | { 124 | #if 0 125 | for (unsigned int i = 0; i < params.size(); i++) 126 | { 127 | 128 | while (EcatError) 129 | log(RTT::Error) << ec_elist2string() << RTT::endlog(); 130 | 131 | //assigning parameters 132 | ec_SDOwrite(((m_datap->configadr) & 0x0F), params[i].index, 133 | params[i].subindex, FALSE, params[i].size, &val, EC_TIMEOUTRXM); 134 | } 135 | #endif 136 | return true; 137 | } 138 | 139 | void SoemEL3062::update() 140 | { 141 | m_raw_msg.values[0] = ((out_el3062t*) (m_datap->inputs))->val_ch1; 142 | m_raw_msg.values[1] = ((out_el3062t*) (m_datap->inputs))->val_ch2; 143 | 144 | m_raw_values_port.write(m_raw_msg); 145 | 146 | m_msg.values[0] = m_raw_msg.values[0] * m_resolution; 147 | m_msg.values[1] = m_raw_msg.values[0] * m_resolution; 148 | 149 | m_values_port.write(m_msg); 150 | 151 | m_params[0] = ((out_el3062t*) (m_datap->inputs))->param_ch1; 152 | m_params[1] = ((out_el3062t*) (m_datap->inputs))->param_ch2; 153 | 154 | } 155 | 156 | //rawRead : read the raw value of the input ///////////////////////////////////////////////////// 157 | int SoemEL3062::rawRead(unsigned int chan) 158 | { 159 | if (chan < m_size) 160 | { 161 | return m_raw_msg.values[chan]; 162 | } 163 | else 164 | log(Error) << "Channel " << chan << " outside of module's range" 165 | << endlog(); 166 | return false; 167 | } 168 | 169 | //read: read the value of one of the 2 channels in Volts /////////////////////////// 170 | double SoemEL3062::read(unsigned int chan) 171 | { 172 | 173 | if (chan < m_size) 174 | { 175 | return m_msg.values[chan]; 176 | } 177 | else 178 | log(Error) << "Channel " << chan << " is out of the module's range" 179 | << endlog(); 180 | return 0.0; 181 | 182 | } 183 | 184 | //Checking overrange//////////////////////////////////////////////////////////////// 185 | 186 | bool SoemEL3062::isOverrange(unsigned int chan) 187 | { 188 | if (chan < m_size) 189 | return m_params[chan].test(OVERRANGE); 190 | else 191 | log(Error) << "Channel " << chan << " is out of the module's range" 192 | << endlog(); 193 | return false; 194 | } 195 | 196 | //Checking Underrange//////////////////////////////////////////////////////////////// 197 | 198 | bool SoemEL3062::isUnderrange(unsigned int chan) 199 | { 200 | if (chan < m_size) 201 | return m_params[chan].test(UNDERRANGE); 202 | else 203 | log(Error) << "Channel " << chan << " is out of the module's range" 204 | << endlog(); 205 | return false; 206 | } 207 | 208 | // Comparing the Value of Channel chan with its own limits (1/2) 209 | bool SoemEL3062::checkLimit(unsigned int chan, unsigned int lim_num) 210 | { 211 | if (!chan < m_size) 212 | { 213 | log(Error) << "Channel " << chan << " is out of the module's range" 214 | << endlog(); 215 | return false; 216 | } 217 | if (!(lim_num > 0 && lim_num < 3)) 218 | { 219 | log(Error) << "Limit nr " << lim_num << " should be 1 or 2" << endlog(); 220 | return false; 221 | } 222 | if (lim_num == 1) 223 | return m_params[chan].test(LIMIT1SMALLER); 224 | else 225 | return m_params[chan].test(LIMIT2SMALLER); 226 | } 227 | 228 | // Checking for errors 229 | bool SoemEL3062::is_error(unsigned int chan) 230 | { 231 | if (chan < m_size) 232 | return m_params[chan].test(ERROR); 233 | else 234 | log(Error) << "Channel " << chan << " is out of the module's range" 235 | << endlog(); 236 | return false; 237 | } 238 | 239 | namespace 240 | { 241 | soem_master::SoemDriver* createSoemEL3062(ec_slavet* mem_loc) 242 | { 243 | return new SoemEL3062(mem_loc); 244 | } 245 | 246 | const bool registered0 = 247 | soem_master::SoemDriverFactory::Instance().registerDriver("EL3062", 248 | createSoemEL3062); 249 | 250 | } 251 | 252 | }//namespace 253 | 254 | 255 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el3062.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:30:46 CET 2010 soem_el3062.h 3 | 4 | soem_el3062.h - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #ifndef SOEM_EL3062_H 29 | #define SOEM_EL3062_H 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include "COE_config.h" 37 | 38 | namespace soem_beckhoff_drivers 39 | { 40 | 41 | template 42 | class SoemEL3062: public soem_master::SoemDriver 43 | { 44 | enum 45 | { 46 | UNDERRANGE = 0, 47 | OVERRANGE, 48 | LIMIT1SMALLER, 49 | LIMIT1HIGHER, 50 | LIMIT2SMALLER, 51 | LIMIT2HIGHER, 52 | ERROR 53 | }; 54 | 55 | typedef struct PACKED{ 56 | int16 status; 57 | int16 value; 58 | } el30xx_single_channel; 59 | 60 | typedef struct 61 | PACKED 62 | { 63 | int16 param_ch1; 64 | int16 val_ch1; 65 | 66 | int16 param_ch2; 67 | int16 val_ch2; 68 | } out_el3062t; 69 | 70 | public: 71 | SoemEL3062(ec_slavet* mem_loc); 72 | ~SoemEL3062() 73 | { 74 | } 75 | ; 76 | 77 | int rawRead(unsigned int chan); 78 | double read(unsigned int chan); 79 | bool isOverrange(unsigned int chan = 0); 80 | bool isUnderrange(unsigned int chan = 0); 81 | bool checkLimit(unsigned int chan = 0, unsigned int Lim_num = 0); 82 | bool is_error(unsigned int chan); 83 | 84 | void update(); 85 | bool configure(); 86 | 87 | private: 88 | 89 | const unsigned int m_size; 90 | const unsigned int m_raw_range; 91 | const double m_lowest; 92 | const double m_highest; 93 | double m_resolution; 94 | std::vector > m_params; 95 | AnalogMsg m_msg; 96 | AnalogMsg m_raw_msg; 97 | std::vector m_values; 98 | std::vector m_raw_values; 99 | 100 | //Ports/////////// 101 | RTT::OutputPort m_values_port; 102 | RTT::OutputPort m_raw_values_port; 103 | 104 | }; 105 | 106 | } 107 | #endif 108 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el30xx.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:31:20 CET 2010 soem_el3062.cpp 3 | 4 | soem_el3062.cpp - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #include "soem_el30xx.h" 29 | #include 30 | #include 31 | #include 32 | 33 | using namespace RTT; 34 | 35 | namespace soem_beckhoff_drivers 36 | { 37 | 38 | namespace 39 | { 40 | soem_master::SoemDriver* createSoemEL3052(ec_slavet* mem_loc) 41 | { 42 | return new SoemEL30xx<2>(mem_loc,32767,4,20); 43 | } 44 | soem_master::SoemDriver* createSoemEL3062(ec_slavet* mem_loc) 45 | { 46 | return new SoemEL30xx<2>(mem_loc,32767,0,10); 47 | } 48 | soem_master::SoemDriver* createSoemEL3004(ec_slavet* mem_loc) 49 | { 50 | return new SoemEL30xx<4>(mem_loc,65535,-10,10); 51 | } 52 | soem_master::SoemDriver* createSoemEL3008(ec_slavet* mem_loc) 53 | { 54 | return new SoemEL30xx<8>(mem_loc,65535,-10,10); 55 | } 56 | 57 | REGISTER_SOEM_DRIVER(EL3052, createSoemEL3052) 58 | REGISTER_SOEM_DRIVER(EL3062, createSoemEL3062) 59 | REGISTER_SOEM_DRIVER(EL3004, createSoemEL3004) 60 | REGISTER_SOEM_DRIVER(EL3008, createSoemEL3008) 61 | 62 | } 63 | 64 | }//namespace 65 | 66 | 67 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el30xx.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:30:46 CET 2010 soem_el3062.h 3 | 4 | soem_el3062.h - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #ifndef SOEM_EL30XX_H 29 | #define SOEM_EL30XX_H 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include "COE_config.h" 37 | 38 | using namespace RTT; 39 | 40 | namespace soem_beckhoff_drivers 41 | { 42 | 43 | template 44 | class SoemEL30xx: public soem_master::SoemDriver 45 | { 46 | enum 47 | { 48 | UNDERRANGE = 0, 49 | OVERRANGE, 50 | LIMIT1SMALLER, 51 | LIMIT1HIGHER, 52 | LIMIT2SMALLER, 53 | LIMIT2HIGHER, 54 | ERROR 55 | }; 56 | 57 | typedef struct PACKED{ 58 | int16 status; 59 | int16 value; 60 | } el30xx_channel; 61 | 62 | typedef struct 63 | PACKED 64 | { 65 | el30xx_channel channel[N]; 66 | } out_el30xxt; 67 | 68 | public: 69 | SoemEL30xx(ec_slavet* mem_loc, int range, double lowest, double highest) : 70 | soem_master::SoemDriver(mem_loc), m_size(N), m_raw_range(range), m_lowest( 71 | lowest), m_highest(highest), m_offset(0), m_status(m_size), m_values(m_size), m_raw_values(m_size), 72 | m_values_port("values"), m_raw_values_port("raw_values") 73 | { 74 | m_service->doc(std::string("Services for Beckhoff ") + std::string( 75 | m_datap->name) + std::string(" module")); 76 | m_service->addOperation("rawRead", &SoemEL30xx::rawRead, this, 77 | RTT::OwnThread).doc("Read raw value of channel i").arg("i", 78 | "channel nr"); 79 | m_service->addOperation("read", &SoemEL30xx::read, this, RTT::OwnThread).doc( 80 | "Read value to channel i").arg("i", "channel nr"); 81 | m_service->addOperation("Over_Range", &SoemEL30xx::isOverrange, this, 82 | RTT::OwnThread).doc( 83 | "For the channel i : 1 = overrange ; 0 = no overrange ").arg("i", 84 | "channel nr"); 85 | m_service->addOperation("Under_Range", &SoemEL30xx::isUnderrange, this, 86 | RTT::OwnThread).doc( 87 | "For the channel i : 1 = Underrange ; 0 = no Underrange ").arg("i", 88 | "channel nr"); 89 | m_service->addOperation("Comp_val_to_lim", &SoemEL30xx::checkLimit, 90 | this, RTT::OwnThread).doc( 91 | "Limit 1/2 value monitoring of channel i : 0= not active, 1= Value is higher than limit 1/2 value, 2= Value is lower than limit 1/2 value, 3: Value equals limit 1/2 value").arg( 92 | "i", "channel nr").arg("x", "Limit nr"); 93 | m_service->addOperation("Error", &SoemEL30xx::is_error, this).doc( 94 | "For the channel i : 1 = error (Overrange or Underrange ; 0 = no error ").arg( 95 | "i", "channel nr"); 96 | 97 | m_offset = (m_lowest > 0) ? m_lowest : 0.; 98 | m_resolution = ((m_highest - m_lowest) / (double) m_raw_range); 99 | 100 | m_service->addConstant("size",m_size); 101 | m_service->addConstant("raw_range", m_raw_range); 102 | m_service->addConstant("resolution", m_resolution); 103 | m_service->addConstant("lowest", m_lowest); 104 | m_service->addConstant("highest", m_highest); 105 | 106 | m_service->addPort(m_values_port).doc( 107 | "AnalogMsg contain the read values of _all_ channels"); 108 | m_service->addPort(m_raw_values_port).doc( 109 | "AnalogMsg containing the read values of _all_ channels"); 110 | 111 | m_msg.values.resize(m_size); 112 | m_raw_msg.values.resize(m_size); 113 | } 114 | 115 | ~SoemEL30xx() 116 | { 117 | } 118 | ; 119 | 120 | int rawRead(unsigned int chan){ 121 | if (chan < m_size) 122 | { 123 | return m_raw_msg.values[chan]; 124 | } 125 | else 126 | log(Error) << "Channel " << chan << " outside of module's range" 127 | << endlog(); 128 | return false; 129 | } 130 | double read(unsigned int chan){ 131 | 132 | if (chan < m_size) 133 | { 134 | return m_msg.values[chan]; 135 | } 136 | else 137 | log(Error) << "Channel " << chan << " is out of the module's range" 138 | << endlog(); 139 | return 0.0; 140 | } 141 | bool isOverrange(unsigned int chan = 0){ 142 | if (chan < m_size) 143 | return m_status[chan].test(OVERRANGE); 144 | else 145 | log(Error) << "Channel " << chan << " is out of the module's range" 146 | << endlog(); 147 | return false; 148 | } 149 | bool isUnderrange(unsigned int chan = 0){ 150 | if (chan < m_size) 151 | return m_status[chan].test(UNDERRANGE); 152 | else 153 | log(Error) << "Channel " << chan << " is out of the module's range" 154 | << endlog(); 155 | return false; 156 | } 157 | bool checkLimit(unsigned int chan = 0, unsigned int lim_num = 0){ 158 | if (!chan < m_size) 159 | { 160 | log(Error) << "Channel " << chan << " is out of the module's range" 161 | << endlog(); 162 | return false; 163 | } 164 | if (!(lim_num > 0 && lim_num < 3)) 165 | { 166 | log(Error) << "Limit nr " << lim_num << " should be 1 or 2" << endlog(); 167 | return false; 168 | } 169 | if (lim_num == 1) 170 | return m_status[chan].test(LIMIT1SMALLER); 171 | else 172 | return m_status[chan].test(LIMIT2SMALLER); 173 | }; 174 | 175 | bool is_error(unsigned int chan){ 176 | if (chan < m_size) 177 | return m_status[chan].test(ERROR); 178 | else 179 | log(Error) << "Channel " << chan << " is out of the module's range" 180 | << endlog(); 181 | return false; 182 | } 183 | 184 | void update(){ 185 | for(unsigned int i=0;iinputs))->channel[i].value; 187 | m_status[i] = ((out_el30xxt*) (m_datap->inputs))->channel[i].status; 188 | m_msg.values[i] = convert_from_raw(m_raw_msg.values[i]); 189 | } 190 | m_raw_values_port.write(m_raw_msg); 191 | m_values_port.write(m_msg); 192 | } 193 | 194 | bool configure(){return true;}; 195 | 196 | double convert_from_raw(double value) 197 | { 198 | return m_offset + value*m_resolution; 199 | } 200 | 201 | private: 202 | 203 | const unsigned int m_size; 204 | const unsigned int m_raw_range; 205 | const double m_lowest; 206 | const double m_highest; 207 | double m_offset; 208 | double m_resolution; 209 | std::vector > m_status; 210 | AnalogMsg m_msg; 211 | AnalogMsg m_raw_msg; 212 | std::vector m_values; 213 | std::vector m_raw_values; 214 | 215 | //Ports/////////// 216 | RTT::OutputPort m_values_port; 217 | RTT::OutputPort m_raw_values_port; 218 | 219 | }; 220 | 221 | } 222 | #endif 223 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el3102.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Sava Marinkov Thu Mar 3 12:44:00 CET 2011 soem_el3102.cpp 3 | 4 | soem_el3102.cpp - 2-channel analog input terminals -10…+10 V, differential input, 16 bits 5 | ------------------- 6 | begin : Thu March 3 2011 7 | copyright : (C) 2011 Sava Marinkov 8 | email : s.marinkov@student.tue.nl 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #include "soem_el3102.h" 29 | #include 30 | 31 | using namespace RTT; 32 | 33 | namespace soem_beckhoff_drivers { 34 | 35 | SoemEL3102::SoemEL3102(ec_slavet* mem_loc) : 36 | soem_master::SoemDriver(mem_loc), m_status(CHANNEL_NUM), m_values_port("values") 37 | 38 | { 39 | m_service->doc(std::string("Services for Beckhoff ") + std::string(m_datap->name) + std::string(" module")); 40 | 41 | m_service->addOperation("read", &SoemEL3102::read, this, RTT::OwnThread).doc("Read value to channel i").arg("i", "channel nr"); 42 | m_service->addOperation("Over_Range", &SoemEL3102::isOverrange, this,RTT::OwnThread).doc("For the channel i : 1 = overrange ; 0 = no overrange ").arg("i", "channel nr"); 43 | m_service->addOperation("Under_Range", &SoemEL3102::isUnderrange, this,RTT::OwnThread).doc("For the channel i : 1 = Underrange ; 0 = no Underrange ").arg("i", "channel nr"); 44 | 45 | m_resolution = ((HIGHEST - LOWEST) / (double) RAW_RANGE); 46 | 47 | m_service->addPort(m_values_port).doc("AnalogMsg contain the read values of _all_ channels"); 48 | 49 | m_msg.values.resize(CHANNEL_NUM); 50 | 51 | // parameter temp; 52 | // 53 | // temp.description = "Enable user scale"; 54 | // temp.index = 0x8010; 55 | // temp.subindex = 0x01; 56 | // temp.name = "E_user_scl"; 57 | // temp.size = 1; 58 | // temp.param = 1; 59 | // m_params.push_back(temp); 60 | // 61 | // temp.description = "Limit1 for channel 1"; 62 | // temp.index = 0x8010; 63 | // temp.subindex = 0x13; 64 | // temp.name = "Limit1_chan1"; 65 | // temp.size = 2; 66 | // temp.param = 9174; 67 | // m_params.push_back(temp); 68 | // 69 | // temp.description = "Limit2 for channel1"; 70 | // temp.index = 0x8010; 71 | // temp.subindex = 0x14; 72 | // temp.name = "Limit2_chan1"; 73 | // temp.size = 2; 74 | // temp.param = 24247; 75 | // m_params.push_back(temp); 76 | // 77 | // for (unsigned int i = 0; i < m_params.size(); i++) { 78 | // m_service->addProperty(m_params[i].name, m_params[i].param).doc(m_params[i].description); 79 | // } 80 | } 81 | 82 | bool SoemEL3102::configure() { 83 | // for (unsigned int i = 0; i < m_params.size(); i++) { 84 | // 85 | // while (EcatError) 86 | // log(RTT::Error) << ec_elist2string() << RTT::endlog(); 87 | // 88 | // ec_SDOwrite(((m_datap->configadr) & 0x0F), m_params[i].index, m_params[i].subindex, FALSE, m_params[i].size, 89 | // &(m_params[i].param), 90 | // EC_TIMEOUTRXM); 91 | // } 92 | return true; 93 | } 94 | 95 | void SoemEL3102::update() { 96 | //log(Debug) << "SoemEL3102::update()" << endlog(); 97 | m_inputs[CHANNEL_1]=((in_el3102t*)(m_datap->inputs)); 98 | m_inputs[CHANNEL_2]=((in_el3102t*)(m_datap->inputs + m_datap->Ibytes / 2)); 99 | 100 | /* 101 | int part1,part2,part3; 102 | part1= *((int*)(m_datap->inputs)); 103 | part2= *((int*)(m_datap->inputs + 16)); 104 | part3= *((int*)(m_datap->inputs + 32)); 105 | for (int i=15; i>=0; i--) { 106 | int bit = ((part1 >> i) & 1); 107 | log(Info) << bit; 108 | if (i==8) log(Info) << " "; 109 | } 110 | log(Info) << " "; 111 | for (int i=15; i>=0; i--) { 112 | int bit = ((part2 >> i) & 1); 113 | log(Info) << bit; 114 | if (i==8) log(Info) << " "; 115 | } 116 | log(Info) << " "; 117 | for (int i=15; i>=0; i--) { 118 | int bit = ((part3 >> i) & 1); 119 | log(Info) << bit; 120 | if (i==8) log(Info) << " "; 121 | } 122 | log(Info) << endlog(); 123 | */ 124 | 125 | for (unsigned int chan = 0; chan < CHANNEL_NUM; chan++) { 126 | m_status[chan] = m_inputs[chan]->status; 127 | if (!(m_inputs[chan]->value & SIGN)) { 128 | m_msg.values[chan] = m_inputs[chan]->value * m_resolution; 129 | //if (chan == CHANNEL_1) 130 | //log(Info) << "Chan " << chan << ". POS. (status, value) = " << "(" << (m_status[chan]) << "," << (m_msg.values[chan]) << ")" << endlog(); 131 | } else { 132 | m_msg.values[chan] = -((~(m_inputs[chan]->value) + 0x0001) * m_resolution); 133 | //if (chan == CHANNEL_1) 134 | //log(Info) << "Chan " << chan << ". NEG. (status, value) = " << "(" << (m_status[chan]) << "," << (m_msg.values[chan]) << ")" << endlog(); 135 | } 136 | if (m_values_port.connected()) { 137 | m_values_port.write(m_msg); 138 | } 139 | } 140 | } 141 | 142 | double SoemEL3102::read(unsigned int chan) { 143 | 144 | if (chan < CHANNEL_NUM) { 145 | return m_msg.values[chan]; 146 | } else 147 | log(Error) << "Channel " << chan << " is out of the module's range" << endlog(); 148 | return 0.0; 149 | } 150 | 151 | bool SoemEL3102::isOverrange(unsigned int chan) { 152 | if (chan < CHANNEL_NUM) 153 | return (m_status[chan] & OVERRANGE) == OVERRANGE; 154 | else 155 | log(Error) << "Channel " << chan << " is out of the module's range" << endlog(); 156 | return false; 157 | } 158 | 159 | bool SoemEL3102::isUnderrange(unsigned int chan) { 160 | if (chan < CHANNEL_NUM) 161 | return (m_status[chan] & UNDERRANGE) == UNDERRANGE; 162 | else 163 | log(Error) << "Channel " << chan << " is out of the module's range" << endlog(); 164 | return false; 165 | } 166 | 167 | namespace { 168 | soem_master::SoemDriver* createSoemEL3102(ec_slavet* mem_loc) { 169 | return new SoemEL3102(mem_loc); 170 | } 171 | 172 | REGISTER_SOEM_DRIVER(EL3102, createSoemEL3102) 173 | 174 | } 175 | 176 | } 177 | 178 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el3102.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Sava Marinkov Thu Mar 3 12:44:00 CET 2011 soem_el3102.h 3 | 4 | soem_el3102.h - 2-channel analog input terminals -10…+10 V, differential input, 16 bits 5 | ------------------- 6 | begin : Thu March 3 2011 7 | copyright : (C) 2011 Sava Marinkov 8 | email : s.marinkov@student.tue.nl 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #ifndef SOEM_EL3102_H 29 | #define SOEM_EL3102_H 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include "COE_config.h" 37 | 38 | #define CHANNEL_1 0 39 | #define CHANNEL_2 1 40 | #define CHANNEL_NUM 2 41 | #define UNDERRANGE 0x00 42 | #define OVERRANGE 0x01 43 | #define RAW_RANGE 0x7FFF 44 | #define LOWEST 0 45 | #define HIGHEST 10 46 | #define SIGN 0x8000 47 | 48 | using namespace RTT; 49 | 50 | namespace soem_beckhoff_drivers { 51 | 52 | typedef struct PACKED { 53 | uint8 status; 54 | uint16 value; 55 | }in_el3102t; 56 | 57 | class SoemEL3102 : public soem_master::SoemDriver { 58 | 59 | public: 60 | SoemEL3102(ec_slavet* mem_loc); 61 | ~SoemEL3102() 62 | { 63 | } 64 | ; 65 | 66 | double read(unsigned int chan); 67 | bool isOverrange(unsigned int chan = 0); 68 | bool isUnderrange(unsigned int chan = 0); 69 | 70 | void update(); 71 | bool configure(); 72 | 73 | private: 74 | 75 | in_el3102t* m_inputs[CHANNEL_NUM]; 76 | 77 | double m_resolution; 78 | 79 | AnalogMsg m_msg; 80 | 81 | std::vector m_status; 82 | 83 | // std::vector m_params; 84 | 85 | RTT::OutputPort m_values_port; 86 | }; 87 | 88 | } 89 | #endif 90 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el3104.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:31:20 CET 2010 soem_el3062.cpp 3 | 4 | soem_el3062.cpp - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #include "soem_el3104.h" 29 | #include 30 | #include 31 | #include 32 | 33 | using namespace RTT; 34 | 35 | namespace soem_beckhoff_drivers 36 | { 37 | 38 | SoemEL3104::SoemEL3104(ec_slavet* mem_loc) : 39 | soem_master::SoemDriver(mem_loc), m_size(4), m_raw_range(65535), m_lowest( 40 | -10.0), m_highest(10.0), m_values(m_size), m_raw_values(m_size),m_params(m_size), 41 | m_values_port("values"), m_raw_values_port("raw_values") 42 | 43 | { 44 | m_service->doc(std::string("Services for Beckhoff ") + std::string( 45 | m_datap->name) + std::string(" module")); 46 | m_service->addOperation("rawRead", &SoemEL3104::rawRead, this, 47 | RTT::OwnThread).doc("Read raw value of channel i").arg("i", 48 | "channel nr"); 49 | m_service->addOperation("read", &SoemEL3104::read, this, RTT::OwnThread).doc( 50 | "Read value to channel i").arg("i", "channel nr"); 51 | m_service->addOperation("Over_Range", &SoemEL3104::isOverrange, this, 52 | RTT::OwnThread).doc( 53 | "For the channel i : 1 = overrange ; 0 = no overrange ").arg("i", 54 | "channel nr"); 55 | m_service->addOperation("Under_Range", &SoemEL3104::isUnderrange, this, 56 | RTT::OwnThread).doc( 57 | "For the channel i : 1 = Underrange ; 0 = no Underrange ").arg("i", 58 | "channel nr"); 59 | m_service->addOperation("Comp_val_to_lim", &SoemEL3104::checkLimit, 60 | this, RTT::OwnThread).doc( 61 | "Limit 1/2 value monitoring of channel i : 0= not active, 1= Value is higher than limit 1/2 value, 2= Value is lower than limit 1/2 value, 3: Value equals limit 1/2 value").arg( 62 | "i", "channel nr").arg("x", "Limit nr"); 63 | m_service->addOperation("Error", &SoemEL3104::is_error, this).doc( 64 | "For the channel i : 1 = error (Overrange or Underrange ; 0 = no error ").arg( 65 | "i", "channel nr"); 66 | 67 | m_resolution = ((m_highest - m_lowest) / (double) m_raw_range); 68 | 69 | m_service->addConstant("raw_range", m_raw_range); 70 | m_service->addConstant("resolution", m_resolution); 71 | m_service->addConstant("lowest", m_lowest); 72 | m_service->addConstant("highest", m_highest); 73 | 74 | m_service->addPort(m_values_port).doc( 75 | "AnalogMsg contain the read values of _all_ channels"); 76 | m_service->addPort(m_raw_values_port).doc( 77 | "AnalogMsg containing the read values of _all_ channels"); 78 | 79 | m_msg.values.resize(m_size); 80 | m_raw_msg.values.resize(m_size); 81 | #if 0 82 | // New properties : Component Configuration : Can be completed 83 | // Need to be complete for a specific usage : See Beckhoff : http://www.beckhoff.com/EL3104/ 84 | //see documentation (.xchm) / Commissionning / Object description and parameterization 85 | 86 | parameter temp; 87 | 88 | /// For parametrisation : uncomment the following code and complete it with using the document above 89 | temp.description = "Enable user scale"; 90 | temp.index = 0x8010; 91 | temp.subindex = 0x01; 92 | temp.name = "E_user_scl"; 93 | temp.size = 1; 94 | temp.param = 1; 95 | params.push_back(temp); 96 | 97 | temp.description = "Limit1 for channel 1"; 98 | temp.index = 0x8010; 99 | temp.subindex = 0x13; 100 | temp.name = "Limit1_chan1"; 101 | temp.size = 2; 102 | temp.param = 9174; 103 | params.push_back(temp); 104 | 105 | temp.description = "Limit2 for channel1"; 106 | temp.index = 0x8010; 107 | temp.subindex = 0x14; 108 | temp.name = "Limit2_chan1"; 109 | temp.size = 2; 110 | temp.param = 24247; 111 | params.push_back(temp); 112 | 113 | // Adding properties to the Soem_master_Component. 114 | for (unsigned int i = 0; i < params.size(); i++) 115 | { 116 | m_service->addProperty(params[i].name, params[i].param).doc( 117 | params[i].description); 118 | } 119 | #endif 120 | } 121 | 122 | bool SoemEL3104::configure() 123 | { 124 | #if 0 125 | for (unsigned int i = 0; i < params.size(); i++) 126 | { 127 | 128 | while (EcatError) 129 | log(RTT::Error) << ec_elist2string() << RTT::endlog(); 130 | 131 | //assigning parameters 132 | ec_SDOwrite(((m_datap->configadr) & 0x0F), params[i].index, 133 | params[i].subindex, FALSE, params[i].size, &val, EC_TIMEOUTRXM); 134 | } 135 | #endif 136 | return true; 137 | } 138 | 139 | void SoemEL3104::update() 140 | { 141 | 142 | 143 | m_raw_msg.values[0] = ((out_el3104t*) (m_datap->inputs))->val_ch1; 144 | m_raw_msg.values[1] = ((out_el3104t*) (m_datap->inputs))->val_ch2; 145 | m_raw_msg.values[2] = ((out_el3104t*) (m_datap->inputs))->val_ch3; 146 | m_raw_msg.values[3] = ((out_el3104t*) (m_datap->inputs))->val_ch4; 147 | m_raw_values_port.write(m_raw_msg); 148 | 149 | for(int i=0;iinputs))->param_ch1; 157 | m_params[1] = ((out_el3104t*) (m_datap->inputs))->param_ch2; 158 | m_params[2] = ((out_el3104t*) (m_datap->inputs))->param_ch3; 159 | m_params[3] = ((out_el3104t*) (m_datap->inputs))->param_ch4; 160 | 161 | } 162 | 163 | //rawRead : read the raw value of the input ///////////////////////////////////////////////////// 164 | int SoemEL3104::rawRead(unsigned int chan) 165 | { 166 | if (chan < m_size) 167 | { 168 | return m_raw_msg.values[chan]; 169 | } 170 | else 171 | log(Error) << "Channel " << chan << " outside of module's range" 172 | << endlog(); 173 | return false; 174 | } 175 | 176 | //read: read the value of one of the 2 channels in Volts /////////////////////////// 177 | double SoemEL3104::read(unsigned int chan) 178 | { 179 | 180 | if (chan < m_size) 181 | { 182 | return m_msg.values[chan]; 183 | } 184 | else 185 | log(Error) << "Channel " << chan << " is out of the module's range" 186 | << endlog(); 187 | return 0.0; 188 | 189 | } 190 | 191 | //Checking overrange//////////////////////////////////////////////////////////////// 192 | 193 | bool SoemEL3104::isOverrange(unsigned int chan) 194 | { 195 | if (chan < m_size) 196 | return m_params[chan].test(OVERRANGE); 197 | else 198 | log(Error) << "Channel " << chan << " is out of the module's range" 199 | << endlog(); 200 | return false; 201 | } 202 | 203 | //Checking Underrange//////////////////////////////////////////////////////////////// 204 | 205 | bool SoemEL3104::isUnderrange(unsigned int chan) 206 | { 207 | if (chan < m_size) 208 | return m_params[chan].test(UNDERRANGE); 209 | else 210 | log(Error) << "Channel " << chan << " is out of the module's range" 211 | << endlog(); 212 | return false; 213 | } 214 | 215 | // Comparing the Value of Channel chan with its own limits (1/2) 216 | bool SoemEL3104::checkLimit(unsigned int chan, unsigned int lim_num) 217 | { 218 | if (!chan < m_size) 219 | { 220 | log(Error) << "Channel " << chan << " is out of the module's range" 221 | << endlog(); 222 | return false; 223 | } 224 | if (!(lim_num > 0 && lim_num < 3)) 225 | { 226 | log(Error) << "Limit nr " << lim_num << " should be 1 or 2" << endlog(); 227 | return false; 228 | } 229 | if (lim_num == 1) 230 | return m_params[chan].test(LIMIT1SMALLER); 231 | else 232 | return m_params[chan].test(LIMIT2SMALLER); 233 | } 234 | 235 | // Checking for errors 236 | bool SoemEL3104::is_error(unsigned int chan) 237 | { 238 | if (chan < m_size) 239 | return m_params[chan].test(ERROR); 240 | else 241 | log(Error) << "Channel " << chan << " is out of the module's range" 242 | << endlog(); 243 | return false; 244 | } 245 | 246 | namespace 247 | { 248 | soem_master::SoemDriver* createSoemEL3104(ec_slavet* mem_loc) 249 | { 250 | return new SoemEL3104(mem_loc); 251 | } 252 | 253 | 254 | REGISTER_SOEM_DRIVER(EL3104, createSoemEL3104) 255 | 256 | } 257 | 258 | }//namespace 259 | 260 | 261 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el3104.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:30:46 CET 2010 soem_el3062.h 3 | 4 | soem_el3062.h - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #ifndef SOEM_EL3104_H 29 | #define SOEM_EL3104_H 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include "COE_config.h" 37 | 38 | namespace soem_beckhoff_drivers 39 | { 40 | 41 | class SoemEL3104: public soem_master::SoemDriver 42 | { 43 | enum 44 | { 45 | UNDERRANGE = 0, 46 | OVERRANGE, 47 | LIMIT1SMALLER, 48 | LIMIT1HIGHER, 49 | LIMIT2SMALLER, 50 | LIMIT2HIGHER, 51 | ERROR 52 | }; 53 | 54 | typedef struct 55 | PACKED 56 | { 57 | int16 param_ch1; 58 | int16 val_ch1; 59 | int16 param_ch2; 60 | int16 val_ch2; 61 | int16 param_ch3; 62 | int16 val_ch3; 63 | int16 param_ch4; 64 | int16 val_ch4; 65 | 66 | } out_el3104t; 67 | 68 | public: 69 | SoemEL3104(ec_slavet* mem_loc); 70 | ~SoemEL3104() 71 | { 72 | } 73 | ; 74 | 75 | int rawRead(unsigned int chan); 76 | double read(unsigned int chan); 77 | bool isOverrange(unsigned int chan = 0); 78 | bool isUnderrange(unsigned int chan = 0); 79 | bool checkLimit(unsigned int chan = 0, unsigned int Lim_num = 0); 80 | bool is_error(unsigned int chan); 81 | 82 | void update(); 83 | bool configure(); 84 | 85 | private: 86 | 87 | const unsigned int m_size; 88 | const unsigned int m_raw_range; 89 | const double m_lowest; 90 | const double m_highest; 91 | double m_resolution; 92 | std::vector > m_params; 93 | AnalogMsg m_msg; 94 | AnalogMsg m_raw_msg; 95 | std::vector m_values; 96 | std::vector m_raw_values; 97 | 98 | //Ports/////////// 99 | RTT::OutputPort m_values_port; 100 | RTT::OutputPort m_raw_values_port; 101 | 102 | }; 103 | 104 | } 105 | #endif 106 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el4xxx.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:31:20 CET 2010 soem_EL4xxx.cpp 3 | 4 | soem_EL4xxx.cpp - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #include "soem_el4xxx.h" 29 | #include 30 | 31 | namespace soem_beckhoff_drivers 32 | { 33 | 34 | namespace 35 | { 36 | soem_master::SoemDriver* createSoemEL4002(ec_slavet* mem_loc) 37 | { 38 | return new SoemEL4xxx<2> (mem_loc, 32767, 0.0, 10.0); 39 | } 40 | soem_master::SoemDriver* createSoemEL4004(ec_slavet* mem_loc) 41 | { 42 | return new SoemEL4xxx<4> (mem_loc, 32767, 0.0, 10.0); 43 | } 44 | soem_master::SoemDriver* createSoemEL4008(ec_slavet* mem_loc) 45 | { 46 | return new SoemEL4xxx<8> (mem_loc, 32767, 0.0, 10.0); 47 | } 48 | soem_master::SoemDriver* createSoemEL4022(ec_slavet* mem_loc) 49 | { 50 | return new SoemEL4xxx<2> (mem_loc, 32767, 4., 20.); 51 | } 52 | soem_master::SoemDriver* createSoemEL4032(ec_slavet* mem_loc) 53 | { 54 | return new SoemEL4xxx<2> (mem_loc, 65535, -10.0, 10.0); 55 | } 56 | soem_master::SoemDriver* createSoemEL4034(ec_slavet* mem_loc) //Resolution wrong???? 57 | { 58 | return new SoemEL4xxx<4> (mem_loc, 65535, -10.0, 10.0); 59 | } 60 | soem_master::SoemDriver* createSoemEL4038(ec_slavet* mem_loc) 61 | { 62 | return new SoemEL4xxx<8> (mem_loc, 65535, -10.0, 10.0); 63 | } 64 | soem_master::SoemDriver* createSoemEL4134(ec_slavet* mem_loc) // Added by Bert 65 | { 66 | return new SoemEL4xxx<4> (mem_loc, 65535, -10.0, 10.0); 67 | } 68 | 69 | 70 | REGISTER_SOEM_DRIVER(EL4002, createSoemEL4002) 71 | REGISTER_SOEM_DRIVER(EL4004, createSoemEL4004) 72 | REGISTER_SOEM_DRIVER(EL4008, createSoemEL4008) 73 | REGISTER_SOEM_DRIVER(EL4022, createSoemEL4022) 74 | REGISTER_SOEM_DRIVER(EL4032, createSoemEL4032) 75 | REGISTER_SOEM_DRIVER(EL4034, createSoemEL4034) 76 | REGISTER_SOEM_DRIVER(EL4038, createSoemEL4038) 77 | REGISTER_SOEM_DRIVER(EL4134, createSoemEL4134) 78 | 79 | } 80 | 81 | }//namespace 82 | 83 | 84 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el4xxx.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:30:46 CET 2010 soem_el4004.h 3 | 4 | soem_el4004.h - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #ifndef SOEM_EL4004_H 29 | #define SOEM_EL4004_H 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | using namespace RTT; 36 | namespace soem_beckhoff_drivers 37 | { 38 | 39 | template 40 | class SoemEL4xxx: public soem_master::SoemDriver 41 | { 42 | private: 43 | 44 | typedef struct PACKED 45 | { 46 | uint16 values[N]; 47 | } out_el4xxxt; 48 | 49 | const unsigned int m_size; 50 | const unsigned int m_raw_range; 51 | const double m_lowest; 52 | const double m_highest; 53 | double m_offset; 54 | double m_resolution; 55 | 56 | 57 | AnalogMsg m_msg; 58 | AnalogMsg m_raw_msg; 59 | std::vector m_values; 60 | std::vector m_raw_values; 61 | RTT::InputPort port_values; 62 | RTT::InputPort port_raw_values; 63 | 64 | bool prop_enable_user_scale; 65 | double prop_offset; 66 | double prop_gain; 67 | 68 | public: 69 | SoemEL4xxx(ec_slavet* mem_loc, int range, double lowest, double highest) : 70 | soem_master::SoemDriver(mem_loc), m_size(N), m_raw_range(range), 71 | m_lowest(lowest), m_highest(highest), m_offset(0.), m_values(m_size), 72 | m_raw_values(m_size), prop_enable_user_scale(false), 73 | prop_offset(0), prop_gain(1.0) 74 | { 75 | 76 | m_service->doc(std::string("Services for Beckhoff ") + std::string( 77 | m_datap->name) + std::string(" module")); 78 | m_service->addOperation("rawWrite", &SoemEL4xxx::rawWrite, this, 79 | RTT::OwnThread).doc("Write raw value to channel i").arg("i", 80 | "channel nr").arg("value", "raw value"); 81 | m_service->addOperation("rawRead", &SoemEL4xxx::rawRead, this, 82 | RTT::OwnThread).doc("Read raw value of channel i").arg("i", 83 | "channel nr"); 84 | m_service->addOperation("write", &SoemEL4xxx::write, this, 85 | RTT::OwnThread).doc("Write value to channel i").arg("i", 86 | "channel nr").arg("value", "value"); 87 | m_service->addOperation("read", &SoemEL4xxx::read, this, RTT::OwnThread).doc( 88 | "Read value to channel i").arg("i", "channel nr"); 89 | 90 | m_service->addOperation("configure_channel",&SoemEL4xxx::configure_channel,this,RTT::OwnThread).doc("Configure offset and gain of channel i").arg("i","channel").arg("offset","offset").arg("gain","gain"); 91 | 92 | m_offset = (m_lowest > 0) ? m_lowest : 0.; 93 | m_resolution = ((m_highest - m_lowest) / (double) m_raw_range); 94 | 95 | 96 | m_service->addConstant("raw_range", m_raw_range); 97 | m_service->addConstant("resolution", m_resolution); 98 | m_service->addConstant("lowest", m_lowest); 99 | m_service->addConstant("highest", m_highest); 100 | 101 | m_service->addProperty("enable_user_scale", prop_enable_user_scale); 102 | m_service->addProperty("gain", prop_gain); 103 | m_service->addProperty("offset", prop_offset); 104 | 105 | m_msg.values.resize(m_size); 106 | m_raw_msg.values.resize(m_size); 107 | 108 | m_service->addPort("values", port_values).doc( 109 | "AnalogMsg containing the desired values of _all_ channels"); 110 | m_service->addPort("raw_values", port_raw_values).doc( 111 | "AnalogMsg containing the desired values of _all_ channels"); 112 | 113 | } 114 | 115 | ~SoemEL4xxx() 116 | { 117 | } 118 | ; 119 | 120 | bool configure() 121 | { 122 | return true; 123 | } 124 | 125 | bool configure_channel(unsigned int channel, double offset, double gain){ 126 | if(channeloutputs))->values[i] 175 | = ((int) (m_raw_values[i])); 176 | } 177 | } 178 | 179 | bool rawWrite(unsigned int chan, int value) 180 | { 181 | if (chan < m_size) 182 | { 183 | m_raw_values[chan] = value; 184 | return true; 185 | } 186 | else 187 | log(Error) << "Channel " << chan << " is out of the module's range" 188 | << endlog(); 189 | return false; 190 | } 191 | int rawRead(unsigned int chan) 192 | { 193 | if (chan < m_size) 194 | return m_raw_values[chan]; 195 | else 196 | log(Error) << "Channel " << chan << " is out of the module's range" 197 | << endlog(); 198 | return -1; 199 | } 200 | 201 | bool write(unsigned int chan, double value) 202 | { 203 | if (chan < m_size) 204 | { 205 | m_raw_values[chan] = convert_to_raw(value); 206 | return true; 207 | } 208 | else 209 | log(Error) << "Channel " << chan << " is out of the module's range" 210 | << endlog(); 211 | return false; 212 | } 213 | 214 | double read(unsigned int chan) 215 | { 216 | if (chan < m_size) 217 | return convert_from_raw(m_raw_values[chan]); 218 | else 219 | log(Error) << "Channel " << chan << " is out of the module's range" 220 | << endlog(); 221 | return -1; 222 | } 223 | 224 | }; 225 | 226 | } 227 | #endif 228 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el5101.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:31:20 CET 2010 soem_el5101.cpp 3 | 4 | soem_el5101.cpp - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #include "soem_el5101.h" 29 | #include 30 | 31 | namespace soem_beckhoff_drivers 32 | { 33 | 34 | SoemEL5101::SoemEL5101(ec_slavet* mem_loc) : 35 | soem_master::SoemDriver(mem_loc), values_port_(this->getName() + "_value") 36 | { 37 | m_service->doc(std::string("Services for Beckhoff ") + std::string( 38 | m_datap->name) + std::string(" Encoder module")); 39 | m_service->addOperation("read", &SoemEL5101::read, this).doc( 40 | "Read in value of the encoder"); 41 | //m_service->addOperation("read_out",&SoemEL5101::read_out,this).doc("Read out value of the encoder"); 42 | //m_service->addOperation("write_out",&SoemEL5101::write_out,this).doc("Write the initial value").arg("value","Value"); 43 | //m_service->addOperation("control",&SoemEL5101::control,this).doc("Read control of the encoder"); 44 | //m_service->addOperation("status",&SoemEL5101::status,this).doc("Read status of the encoder"); 45 | 46 | m_service->addPort(values_port_).doc( 47 | "Uint msg containing the value of the encoder"); 48 | 49 | parameter temp; 50 | temp.description = "Essai description"; 51 | temp.index = 1000; 52 | temp.subindex = 0; 53 | temp.name = this->getName() + "essai"; 54 | temp.size = 1; 55 | temp.param = 10; 56 | 57 | params.push_back(temp); 58 | 59 | // propriete. 60 | if (params.empty()) // Si le tableau est vide. 61 | { 62 | std::cout << "Le tableau est vide" << std::endl; 63 | } 64 | else 65 | { 66 | for (unsigned int i(0); i < params.size(); ++i) // On parcourt le tableau. 67 | { 68 | m_service->addProperty(params[i].name, params[i].param).doc( 69 | params[i].description); 70 | } 71 | } 72 | 73 | // std::cout << this.properties()->getProperty(this->getName()+"_testparam") < 75 | //tc->addP 76 | //RTT::Property param(this->getName()+"_TEST2","description de ma proprietee"); 77 | //tc->addProperty(param); 78 | //tc->addProperty(this->getName()+"_param",test.param).doc("TEST description de ma proprietee"); 79 | 80 | // tc->getProvider("marshalling")->writeProperties("essai.xml"); 81 | // tc->addProperty(); 82 | //std::cout << tc->properties()->getProperty(this->getName()+"_testparam")<properties()->getProperty(this->getName()+"_testparam2")<properties()->getProperty(this->getName()+"_testparam3")<inputs))->invalue; 98 | } 99 | 100 | /*double SoemEL5101::read_out( void){ 101 | return ((out_el5101t*)(m_datap->outputs))->outvalue; 102 | } 103 | 104 | int SoemEL5101::write_out( uint value){ 105 | ((out_el5101t*)(m_datap->outputs))->outvalue=value; 106 | return 1; 107 | //return ((out_el5101t*)(m_datap->outputs))->outvalue; 108 | } 109 | 110 | unsigned int SoemEL5101::status( void){ 111 | return ((in_el5101t*)(m_datap->inputs))->status; 112 | } 113 | 114 | unsigned int SoemEL5101::control( void){ 115 | return ((out_el5101t*)(m_datap->outputs))->control; 116 | } 117 | */ 118 | namespace 119 | { 120 | soem_master::SoemDriver* createSoemEL5101(ec_slavet* mem_loc) 121 | { 122 | return new SoemEL5101(mem_loc); 123 | } 124 | 125 | REGISTER_SOEM_DRIVER(EL5101, createSoemEL5101) 126 | 127 | } 128 | 129 | }//namespace 130 | 131 | 132 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el5101.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:30:46 CET 2010 soem_el5101.h 3 | 4 | soem_el5101.h - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #ifndef SOEM_EL5101_H 29 | #define SOEM_EL5101_H 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace soem_beckhoff_drivers 39 | { 40 | 41 | class SoemEL5101: public soem_master::SoemDriver 42 | { 43 | 44 | typedef struct PACKED 45 | { 46 | uint8 control; 47 | uint16 outvalue; 48 | } out_el5101t; 49 | 50 | typedef struct PACKED 51 | { 52 | uint8 status; 53 | uint16 invalue; 54 | uint16 latch; 55 | uint32 frequency; 56 | uint16 period; 57 | uint16 window; 58 | } in_el5101t; 59 | 60 | public: 61 | SoemEL5101(ec_slavet* mem_loc); 62 | ~SoemEL5101() 63 | { 64 | } 65 | ; 66 | // Returns the encoder value as a double. 67 | uint32_t read(void); 68 | 69 | /*double read_out(void); 70 | int write_out(uint); 71 | unsigned int control(void); 72 | unsigned int status(void);*/ 73 | 74 | virtual void update(); 75 | 76 | private: 77 | 78 | typedef struct { 79 | uint16 index; 80 | uint8 subindex; 81 | uint8 size; 82 | int param; 83 | std::string name; 84 | std::string description; 85 | } parameter; 86 | 87 | EncoderMsg msg_; 88 | std::vector values_in_; 89 | RTT::OutputPort values_port_; 90 | RTT::Property propriete; 91 | std::vector params; 92 | }; 93 | 94 | } 95 | #endif 96 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el6022.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Sava Marinkov Sat Feb 19 12:50:00 CET 2011 soem_el6021.cpp 3 | 4 | soem_el6022.cpp - Beckhoff 2-channel serial interfaces RS232/RS422/RS485, 5 | D-sub connection 6 | ------------------- 7 | begin : Sat February 19 2011 8 | copyright : (C) 2011 Sava Marinkov 9 | email : s.marinkov@student.tue.nl 10 | 11 | *************************************************************************** 12 | * This library is free software; you can redistribute it and/or * 13 | * modify it under the terms of the GNU Lesser General Public * 14 | * License as published by the Free Software Foundation; either * 15 | * version 2.1 of the License, or (at your option) any later version. * 16 | * * 17 | * This library is distributed in the hope that it will be useful, * 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 20 | * Lesser General Public License for more details. * 21 | * * 22 | * You should have received a copy of the GNU Lesser General Public * 23 | * License along with this library; if not, write to the Free Software * 24 | * Foundation, Inc., 59 Temple Place, * 25 | * Suite 330, Boston, MA 02111-1307 USA * 26 | * * 27 | ***************************************************************************/ 28 | 29 | #include "soem_el6022.h" 30 | #include 31 | 32 | using namespace RTT; 33 | 34 | namespace soem_beckhoff_drivers { 35 | 36 | SoemEL6022::SoemEL6022(ec_slavet* mem_loc) : 37 | soem_master::SoemDriver(mem_loc) { 38 | m_service->doc(std::string("Services for Beckhoff ") + std::string(m_datap->name) 39 | + std::string(" RS422/RS485 module")); 40 | m_service->addPort("data_rx", port_out).doc("Msg containing the received data from serial device"); 41 | m_service->addPort("data_tx", port_in).doc("Msg containing the data to send to the serial device"); 42 | m_service->addPort("ready_rx", port_rx_ready).doc("Signal specifying that the serial device is ready to receive the data"); 43 | m_service->addPort("running", port_running).doc("Signal specifying that the serial device is ready to transmit the data"); 44 | 45 | #if 0 46 | parameter temp; 47 | 48 | temp.description = "Send handshake Ch1"; 49 | temp.index = 0x8000; 50 | temp.subindex = 0x02; 51 | temp.name = "S_Handshake1"; 52 | temp.size = 1; 53 | temp.param = XON_XOFF_DISABLE; 54 | m_params.push_back(temp); 55 | 56 | temp.description = "Receive handshake Ch1"; 57 | temp.index = 0x8000; 58 | temp.subindex = 0x03; 59 | temp.name = "R_Handshake1"; 60 | temp.size = 1; 61 | temp.param = XON_XOFF_DISABLE; 62 | m_params.push_back(temp); 63 | 64 | temp.description = "Send handshake Ch2"; 65 | temp.index = 0x8010; 66 | temp.subindex = 0x02; 67 | temp.name = "S_Handshake2"; 68 | temp.size = 1; 69 | temp.param = XON_XOFF_DISABLE; 70 | m_params.push_back(temp); 71 | 72 | temp.description = "Receive handshake Ch2"; 73 | temp.index = 0x8010; 74 | temp.subindex = 0x03; 75 | temp.name = "R_Handshake2"; 76 | temp.size = 1; 77 | temp.param = XON_XOFF_DISABLE; 78 | m_params.push_back(temp); 79 | 80 | temp.description = "Baudrate Ch1"; 81 | temp.index = 0x8000; 82 | temp.subindex = 0x11; 83 | temp.name = "Baud1"; 84 | temp.size = 1; 85 | temp.param = RS485_57600_BAUD; 86 | m_params.push_back(temp); 87 | 88 | temp.description = "Baudrate Ch2"; 89 | temp.index = 0x8010; 90 | temp.subindex = 0x11; 91 | temp.name = "Baud2"; 92 | temp.size = 1; 93 | temp.param = RS485_57600_BAUD; 94 | m_params.push_back(temp); 95 | 96 | temp.description = "Data frame Ch1"; 97 | temp.index = 0x8000; 98 | temp.subindex = 0x15; 99 | temp.name = "Data_frame1"; 100 | temp.size = 1; 101 | temp.param = RS485_8B_NP_1S; 102 | m_params.push_back(temp); 103 | 104 | temp.description = "Data frame Ch2"; 105 | temp.index = 0x8010; 106 | temp.subindex = 0x15; 107 | temp.name = "Data_frame2"; 108 | temp.size = 1; 109 | temp.param = RS485_8B_NP_1S; 110 | m_params.push_back(temp); 111 | 112 | temp.description = "Enable half duplex Ch1"; 113 | temp.index = 0x8000; 114 | temp.subindex = 0x06; 115 | temp.name = "E_half_duplex1"; 116 | temp.size = 1; 117 | temp.param = RS485_HALF_DUPLEX; 118 | m_params.push_back(temp); 119 | 120 | temp.description = "Enable half duplex Ch2"; 121 | temp.index = 0x8010; 122 | temp.subindex = 0x06; 123 | temp.name = "E_half_duplex2"; 124 | temp.size = 1; 125 | temp.param = RS485_HALF_DUPLEX; 126 | m_params.push_back(temp); 127 | 128 | for (unsigned int i = 0; i < m_params.size(); i++) { 129 | m_service->addProperty(m_params[i].name, m_params[i].param).doc(m_params[i].description); 130 | } 131 | #endif 132 | } 133 | 134 | bool SoemEL6022::configure() { 135 | #if 0 136 | for (unsigned int i = 0; i < m_params.size(); i++) { 137 | 138 | while (EcatError) 139 | log(RTT::Error) << ec_elist2string() << RTT::endlog(); 140 | 141 | ec_SDOwrite(((m_datap->configadr) & 0x0F), m_params[i].index, m_params[i].subindex, FALSE, m_params[i].size, 142 | &(m_params[i].param), 143 | EC_TIMEOUTRXM); 144 | } 145 | #endif 146 | 147 | msg_in.channels.resize(CHANNEL_NUM); 148 | for (unsigned int chan = 0; chan < CHANNEL_NUM; chan++) state[chan] = START; 149 | 150 | log(Debug) << "SoemEL6022::configure()"<< endlog(); 151 | return true; 152 | } 153 | 154 | void SoemEL6022::update() { 155 | for (unsigned int chan = 0; chan < CHANNEL_NUM; chan++) { 156 | executeStateActions(chan); 157 | updateState(chan); 158 | } 159 | } 160 | 161 | void SoemEL6022::updateState(unsigned int chan) { 162 | switch (state[chan]) { 163 | case START: 164 | state[chan]=INIT_REQ; 165 | log(Debug) << "The state machine (re)started on chan "<< chan << endlog(); 166 | break; 167 | case INIT_REQ: 168 | state[chan]=INIT_WAIT; 169 | log(Debug) << "The controller requests terminal for initialisation on chan "<< chan << endlog(); 170 | break; 171 | case INIT_WAIT: 172 | if (readSB(chan, INIT_ACCEPTED)) { 173 | state[chan]=PREP_REQ; 174 | log(Debug) << "Initialisation was completed by the terminal on chan "<< chan << endlog(); 175 | break; 176 | } 177 | if (trial[chan]>MAX_TRIALS) { 178 | state[chan]=START; 179 | log(Debug) << "Max num of terminal initialization trials reached on chan "<< chan << endlog(); 180 | break; 181 | } else { 182 | state[chan]=INIT_WAIT; 183 | break; 184 | } 185 | case PREP_REQ: 186 | state[chan]=PREP_WAIT; 187 | log(Debug) << "The controller requests terminal to prepare for serial data exchange on chan "<< chan 188 | << endlog(); 189 | break; 190 | case PREP_WAIT: 191 | if (!readSB(chan, INIT_ACCEPTED)) { 192 | state[chan]=RUN; 193 | log(Debug) << "The terminal is ready for serial data exchange on chan "<< chan << endlog(); 194 | break; 195 | } 196 | if (trial[chan]>MAX_TRIALS) { 197 | state[chan]=START; 198 | log(Debug) << "Max num of terminal preparation trials reached on chan "<< chan << endlog(); 199 | break; 200 | } else { 201 | state[chan]=PREP_WAIT; 202 | break; 203 | } 204 | case RUN: 205 | if (readSB(chan, PARITY_ERROR)) { 206 | log(Warning) << "Parity error on chan " << chan << "!" << endlog(); 207 | } 208 | if (readSB(chan, FRAMING_ERROR)) { 209 | log(Warning) << "Framing error on chan " << chan << "!" << endlog(); 210 | } 211 | if (readSB(chan, OVERRUN_ERROR)) { 212 | log(Warning) << "Overrun error on chan " << chan << "!" << endlog(); 213 | } 214 | state[chan]=RUN; 215 | break; 216 | default: 217 | state[chan]=START; 218 | } 219 | } 220 | 221 | void SoemEL6022::executeStateActions(unsigned int chan) { 222 | switch (state[chan]) { 223 | case START: 224 | m_outputs[CHANNEL_1]=((out_el6022t*)(m_datap->outputs)); 225 | m_outputs[CHANNEL_2]=((out_el6022t*)(m_datap->outputs + m_datap->Obytes / 2)); 226 | 227 | m_inputs[CHANNEL_1]=((in_el6022t*)(m_datap->inputs)); 228 | m_inputs[CHANNEL_2]=((in_el6022t*)(m_datap->inputs + m_datap->Ibytes / 2)); 229 | 230 | trial[chan] = 0; 231 | m_outputs[chan]->control=0x00; 232 | m_outputs[chan]->output_length=0x00; 233 | 234 | for (unsigned int j = 0; j < RS485_MAX_DATA_LENGTH; j++) { 235 | m_outputs[chan]->buffer_out[j]=0x00; 236 | } 237 | break; 238 | case INIT_REQ: 239 | m_outputs[chan]->control = INIT_REQUEST; 240 | break; 241 | case INIT_WAIT: 242 | trial[chan]++; 243 | break; 244 | case PREP_REQ: 245 | trial[chan]=0; 246 | m_outputs[chan]->control = 0x00; 247 | break; 248 | case PREP_WAIT: 249 | trial[chan]++; 250 | break; 251 | case RUN: 252 | port_running.write(true); 253 | 254 | if (port_in.read(msg_out) == NewData) { 255 | for (unsigned int i = 0; i < msg_out.channels[chan].datasize; i++) { 256 | if ((uint8)bytesOut[chan].size() >= MAX_OUT_QUEUE_SIZE) { 257 | log(Warning) << "Max out queue size reached on " << chan << ". Throwing away old bytes..." << endlog(); 258 | bytesOut[chan].pop(); 259 | } 260 | bytesOut[chan].push(msg_out.channels[chan].datapacket[i]); 261 | } 262 | } 263 | 264 | write(chan); 265 | 266 | if (read(chan)) { 267 | port_out.write(msg_in); 268 | port_rx_ready.write(true); 269 | } else { 270 | port_rx_ready.write(false); 271 | } 272 | break; 273 | } 274 | } 275 | 276 | bool SoemEL6022::read(unsigned int chan) { 277 | if (readSB(chan, RECEIVE_REQUEST)!=readCB(chan, RECEIVE_ACCEPTED)) { 278 | uint8 length = m_inputs[chan]->input_length; 279 | 280 | msg_in.channels[chan].datapacket.clear(); 281 | msg_in.channels[chan].datapacket.resize(length); 282 | 283 | for (unsigned int i = 0; i < length; i++) { 284 | msg_in.channels[chan].datapacket[i] = m_inputs[chan]->buffer_in[i]; 285 | } 286 | msg_in.channels[chan].datasize = length; 287 | 288 | log(Debug) << "Read " << (uint16) length << " bytes on " << chan << ": "; 289 | for (unsigned int i = 0; i < length; i++) { 290 | log(Debug) << (unsigned int) m_inputs[chan]->buffer_in[i] << " "; 291 | } 292 | log(Debug) << endlog(); 293 | 294 | m_outputs[chan]->control = m_outputs[chan]->control ^ RECEIVE_ACCEPTED; // acknowledge that the new data is received 295 | 296 | return true; 297 | } 298 | return false; 299 | } 300 | 301 | bool SoemEL6022::write(unsigned int chan) { 302 | if (readCB(chan, TRANSMIT_REQUEST)==readSB(chan, TRANSMIT_ACCEPTED)) { 303 | uint8 length = 0; 304 | 305 | while ((!bytesOut[chan].empty()) && (length < RS485_MAX_DATA_LENGTH)) { 306 | m_outputs[chan]->buffer_out[length] = bytesOut[chan].front(); 307 | bytesOut[chan].pop(); 308 | length++; 309 | } 310 | 311 | if (length == 0) return false; 312 | 313 | m_outputs[chan]->output_length = length; 314 | 315 | log(Debug) << "Written " << (uint16) length << " bytes on " << chan << ": "; 316 | for (unsigned int i = 0; i < length; i++) { 317 | log(Debug) << (unsigned int) m_outputs[chan]->buffer_out[i] << " "; 318 | } 319 | log(Debug) << endlog(); 320 | 321 | m_outputs[chan]->control = m_outputs[chan]->control ^ TRANSMIT_REQUEST; // request transmitting the new data 322 | 323 | return true; 324 | } 325 | return false; 326 | } 327 | 328 | bool SoemEL6022::readSB(unsigned int chan, uint8 bitmask) { 329 | return (m_inputs[chan]->status & bitmask)==bitmask; 330 | } 331 | 332 | bool SoemEL6022::readCB(unsigned int chan, uint8 bitmask) { 333 | return (m_outputs[chan]->control & bitmask)==bitmask; 334 | } 335 | 336 | namespace { 337 | soem_master::SoemDriver* createSoemEL6022(ec_slavet* mem_loc) { 338 | return new SoemEL6022(mem_loc); 339 | } 340 | 341 | REGISTER_SOEM_DRIVER(EL6022, createSoemEL6022) 342 | 343 | } 344 | 345 | } 346 | 347 | -------------------------------------------------------------------------------- /soem_beckhoff_drivers/src/soem_el6022.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Sava Marinkov Sat Feb 19 12:50:00 CET 2011 soem_el6xxx.h 3 | 4 | soem_el6022.h - Header for Beckhoff 2-channel serial interfaces RS232/RS422/RS485, D-sub connection 5 | ------------------- 6 | begin : Sat February 19 2011 7 | copyright : (C) 2011 Sava Marinkov 8 | email : s.marinkov@student.tue.nl 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #ifndef SOEM_EL6022_H 29 | #define SOEM_EL6022_H 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include "COE_config.h" 38 | 39 | #define CHANNEL_1 0 40 | #define CHANNEL_2 1 41 | #define CHANNEL_NUM 2 42 | #define MAX_TRIALS 30 43 | #define MAX_OUT_QUEUE_SIZE 220 44 | #define RS485_MAX_DATA_LENGTH 22 45 | 46 | //CONTROL MASKS 47 | #define TRANSMIT_REQUEST 0x01 48 | #define RECEIVE_ACCEPTED 0x02 49 | #define INIT_REQUEST 0x04 50 | #define SEND_CONTINUOUS 0x08 51 | 52 | //STATUS MASKS 53 | #define TRANSMIT_ACCEPTED 0x01 54 | #define RECEIVE_REQUEST 0x02 55 | #define INIT_ACCEPTED 0x04 56 | #define BUFFER_FULL 0x08 57 | #define PARITY_ERROR 0x10 58 | #define FRAMING_ERROR 0x20 59 | #define OVERRUN_ERROR 0x40 60 | 61 | typedef enum RS485_BAUDRATE { 62 | RS485_300_BAUD = 1, 63 | RS485_600_BAUD, 64 | RS485_1200_BAUD, 65 | RS485_2400_BAUD, 66 | RS485_4800_BAUD, 67 | RS485_9600_BAUD, 68 | RS485_19200_BAUD, 69 | RS485_38400_BAUD, 70 | RS485_57600_BAUD, 71 | RS485_115200_BAUD 72 | } RS485_BAUDRATE; 73 | 74 | typedef enum RS485_DATA_FRAME { 75 | RS485_7B_EP_1S = 1, 76 | RS485_7B_OP_1S, 77 | RS485_8B_NP_1S, 78 | RS485_8B_EP_1S, 79 | RS485_8B_OP_1S, 80 | RS485_7B_EP_2S, 81 | RS485_7B_OP_2S, 82 | RS485_8B_NP_2S, 83 | RS485_8B_EP_2S, 84 | RS485_8B_OP_2S 85 | } RS485_DATA_FRAME; 86 | 87 | typedef enum RS485_HANDSHAKE { 88 | XON_XOFF_DISABLE = 0, 89 | XON_XOFF_ENABLE 90 | } RS485_HANDSHAKE; 91 | 92 | typedef enum RS485_DUPLEX { 93 | RS485_FULL_DUPLEX = 0, 94 | RS485_HALF_DUPLEX 95 | } RS485_DUPLEX; 96 | 97 | typedef enum state_el6022t { 98 | START, 99 | INIT_REQ, 100 | INIT_WAIT, 101 | PREP_REQ, 102 | PREP_WAIT, 103 | RUN 104 | } state_el6022t; 105 | 106 | typedef struct PACKED { 107 | uint8 control; 108 | uint8 output_length; 109 | uint8 buffer_out[RS485_MAX_DATA_LENGTH]; 110 | } out_el6022t; 111 | 112 | typedef struct PACKED { 113 | uint8 status; 114 | uint8 input_length; 115 | uint8 buffer_in[RS485_MAX_DATA_LENGTH]; 116 | } in_el6022t; 117 | 118 | using namespace RTT; 119 | namespace soem_beckhoff_drivers { 120 | 121 | class SoemEL6022 : public soem_master::SoemDriver 122 | { 123 | 124 | public: 125 | SoemEL6022(ec_slavet* mem_loc); 126 | ~SoemEL6022() 127 | {}; 128 | 129 | void update(); 130 | bool configure(); 131 | 132 | bool readSB(unsigned int chan, uint8 bitmask); 133 | bool readCB(unsigned int chan, uint8 bitmask); 134 | 135 | private: 136 | 137 | void updateState(unsigned int chan); 138 | void executeStateActions(unsigned int chan); 139 | bool read(unsigned int chan); 140 | bool write(unsigned int chan); 141 | 142 | out_el6022t* m_outputs[CHANNEL_NUM]; 143 | in_el6022t* m_inputs[CHANNEL_NUM]; 144 | 145 | CommMsgBig msg_out; //terminal sends this msg to rs485 device 146 | CommMsgBig msg_in; //terminal receives this msg from rs485 device 147 | bool rxReady, txReady; 148 | 149 | RTT::OutputPort port_out; 150 | RTT::InputPort port_in; 151 | RTT::OutputPort port_rx_ready; 152 | RTT::OutputPort port_running; 153 | 154 | std::vector m_params; 155 | std::queue bytesOut[CHANNEL_NUM]; 156 | 157 | state_el6022t state[CHANNEL_NUM]; 158 | 159 | unsigned int trial[CHANNEL_NUM]; 160 | }; 161 | 162 | } 163 | #endif 164 | -------------------------------------------------------------------------------- /soem_core/CATKIN_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/orocos/rtt_soem/97d0cb9d4fa608a9acf8f49a177e617217ccc5c2/soem_core/CATKIN_IGNORE -------------------------------------------------------------------------------- /soem_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | find_path(RTNET_INCLUDE_DIR NAMES rtnet.h PATH_SUFFIXES rtnet/include) 26 | option(ENABLE_RTNET "Enable RTnet over Xenomai." ON) 27 | if(RTNET_INCLUDE_DIR AND ENABLE_RTNET) 28 | message("Found RTNET: ${RTNET_INCLUDE_DIR}") 29 | include_directories(${RTNET_INCLUDE_DIR}) 30 | add_definitions(-DHAVE_RTNET) 31 | # try Xenomai 32 | execute_process(COMMAND xeno-config --skin=posix --cflags OUTPUT_VARIABLE XENO_CFLAGS OUTPUT_STRIP_TRAILING_WHITESPACE) 33 | execute_process(COMMAND xeno-config --skin=posix --ldflags OUTPUT_VARIABLE XENO_LDFLAGS OUTPUT_STRIP_TRAILING_WHITESPACE) 34 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${XENO_CFLAGS}") 35 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} ${XENO_LDFLAGS}") 36 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} ${XENO_LDFLAGS} -lnative") 37 | endif() 38 | 39 | rosbuild_add_library(${PROJECT_NAME} build/soem_core/src/ethercatbase.c build/soem_core/src/ethercatcoe.c build/soem_core/src/ethercatconfig.c build/soem_core/src/ethercatfoe.c build/soem_core/src/ethercatsoe.c build/soem_core/src/ethercatmain.c build/soem_core/src/ethercatprint.c build/soem_core/src/nicdrv.c build/soem_core/src/ethercatdc.c) 40 | 41 | include_directories(${PROJECT_SOURCE_DIR}/build/soem_core/src) 42 | #rosbuild_add_boost_directories() 43 | #rosbuild_link_boost(${PROJECT_NAME} thread) 44 | rosbuild_add_executable(slaveinfo build/soem_core/src/slaveinfo.c) 45 | target_link_libraries(slaveinfo ${PROJECT_NAME}) 46 | 47 | rosbuild_add_executable(eepromtool build/soem_core/src/eepromtool.c) 48 | target_link_libraries(eepromtool ${PROJECT_NAME}) -------------------------------------------------------------------------------- /soem_core/Makefile: -------------------------------------------------------------------------------- 1 | TARBALL = SOEM1.2.5.tar.bz2 2 | TARBALL_URL = http://download.berlios.de/soem/SOEM1.2.5.tar.bz2 3 | SOURCE_DIR = build/soem_core 4 | INITIAL_DIR = build/SOEM1.2.5 5 | UNPACK_CMD = tar xjf 6 | MD5SUM_FILE = SOEM1.2.5.tar.bz2.md5sum 7 | TARBALL_PATCH=rtnet.patch 8 | 9 | all: $(SOURCE_DIR)/unpacked 10 | 11 | include $(shell rospack find mk)/download_unpack_build.mk 12 | 13 | unpack: $(SOURCE_DIR) 14 | 15 | wipe: clean 16 | -rm -rf $(SOURCE_DIR) 17 | -rm -rf ${TARBALL} 18 | 19 | EXTRA_CMAKE_FLAGS=-DENABLE_RTNET=OFF 20 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /soem_core/SOEM1.2.5.tar.bz2.md5sum: -------------------------------------------------------------------------------- 1 | 222c3003d047766463e567f7a49981f9 SOEM1.2.5.tar.bz2 2 | -------------------------------------------------------------------------------- /soem_core/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b SOEM is an open source EtherCAT master library written in c. Its primary target is Linux but can be adapted to other OS and embedded systems. 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /soem_core/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | SOEM is an open source EtherCAT master library written in c. Its 5 | primary target is Linux but can be adapted to other OS and 6 | embedded systems. (http://developer.berlios.de/projects/soem/) 7 | 8 | This package contains the original soem c code provided by the Technische Universiteit Eindhoven. 9 | 10 | 11 | Arthur Ketels and M.J.G. van den Molengraft; packaged by Ruben Smits 12 | GPL+linking exception 13 | 14 | http://developer.berlios.de/projects/soem 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /soem_core/rtnet.patch: -------------------------------------------------------------------------------- 1 | --- src/nicdrv.c 2011-05-09 12:36:45.000000000 +0200 2 | +++ src/nicdrv.c 2012-04-23 10:59:50.000000000 +0200 3 | @@ -64,16 +64,20 @@ 4 | * compensate. If needed the packets from interface A are resend through interface B. 5 | * This layer if fully transparent for the higher layers. 6 | */ 7 | - 8 | -#include 9 | +#ifdef HAVE_RTNET 10 | +#include 11 | +#else 12 | #include 13 | -#include 14 | #include 15 | +#include 16 | +#include 17 | +#endif 18 | + 19 | +#include 20 | #include 21 | #include 22 | #include 23 | #include 24 | -#include 25 | #include 26 | #include 27 | #include 28 | @@ -81,6 +85,7 @@ 29 | #include "ethercattype.h" 30 | #include "nicdrv.h" 31 | 32 | + 33 | /** Redundancy modes */ 34 | enum 35 | { 36 | @@ -176,9 +181,15 @@ 37 | /** second MAC word is used for identification */ 38 | #define RX_SEC secMAC[1] 39 | 40 | +#ifdef HAVE_RTNET 41 | pthread_mutex_t ec_getindex_mutex = PTHREAD_MUTEX_INITIALIZER; 42 | pthread_mutex_t ec_tx_mutex = PTHREAD_MUTEX_INITIALIZER; 43 | pthread_mutex_t ec_rx_mutex = PTHREAD_MUTEX_INITIALIZER; 44 | +#else 45 | +pthread_mutex_t ec_getindex_mutex; 46 | +pthread_mutex_t ec_tx_mutex; 47 | +pthread_mutex_t ec_rx_mutex; 48 | +#endif 49 | 50 | /** Basic setup to connect NIC to socket. 51 | * @param[in] ifname = Name of NIC device, f.e. "eth0" 52 | @@ -194,6 +205,10 @@ 53 | struct sockaddr_ll sll; 54 | int *psock; 55 | 56 | + pthread_mutex_init( &ec_getindex_mutex, 0 ); 57 | + pthread_mutex_init( &ec_tx_mutex, 0 ); 58 | + pthread_mutex_init( &ec_rx_mutex, 0 ); 59 | + 60 | rval = 0; 61 | if (secondary) 62 | { 63 | @@ -208,31 +223,73 @@ 64 | } 65 | /* we use RAW packet socket, with packet type ETH_P_ECAT */ 66 | *psock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ECAT)); 67 | + if(*psock<0){ 68 | + printf("creation of socket failed:%d\n",*psock); 69 | + return 0; 70 | + } 71 | + /* connect socket to NIC by name */ 72 | + strcpy(ifr.ifr_name, ifname); 73 | + r = ioctl(*psock, SIOCGIFINDEX, &ifr); 74 | + if(r<0){ 75 | + printf("getting socket index failed:%d, %s\n",r,strerror(-r)); 76 | + } 77 | + 78 | + 79 | timeout.tv_sec = 0; 80 | timeout.tv_usec = 1; 81 | - 82 | +#ifdef HAVE_RTNET 83 | + r = ioctl(*psock,RTNET_RTIOC_TIMEOUT,&timeout); 84 | + if(r<0){ 85 | + printf("setting socket timeout failed:%d, %s\n",r,strerror(-r)); 86 | + } 87 | +#else 88 | + 89 | r = setsockopt(*psock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); 90 | + if(r<0){ 91 | + printf("setting socket options rcvtimeo failed:%d, %s\n",r,strerror(-r)); 92 | + } 93 | + 94 | r = setsockopt(*psock, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)); 95 | + if(r<0){ 96 | + printf("setting socket options sndtimeo failed:%d, %s\n",r,strerror(-r)); 97 | + } 98 | + 99 | i = 1; 100 | r = setsockopt(*psock, SOL_SOCKET, SO_DONTROUTE, &i, sizeof(i)); 101 | - /* connect socket to NIC by name */ 102 | - strcpy(ifr.ifr_name, ifname); 103 | - r = ioctl(*psock, SIOCGIFINDEX, &ifr); 104 | + if(r<0){ 105 | + printf("setting socket options dontroute failed:%d, %s\n",r,strerror(-r)); 106 | + } 107 | + 108 | +#endif 109 | + 110 | + 111 | ifindex = ifr.ifr_ifindex; 112 | +#ifdef HAVE_RTNET 113 | strcpy(ifr.ifr_name, ifname); 114 | ifr.ifr_flags = 0; 115 | /* reset flags of NIC interface */ 116 | r = ioctl(*psock, SIOCGIFFLAGS, &ifr); 117 | + if(r<0){ 118 | + printf("resetting socket flags failed:%d, %s\n",r,strerror(-r)); 119 | + } 120 | /* set flags of NIC interface, here promiscuous and broadcast */ 121 | - ifr.ifr_flags = ifr.ifr_flags || IFF_PROMISC || IFF_BROADCAST; 122 | + ifr.ifr_flags = ifr.ifr_flags | IFF_PROMISC | IFF_BROADCAST; 123 | r = ioctl(*psock, SIOCGIFFLAGS, &ifr); 124 | + if(r<0){ 125 | + printf("setting socket flags failed:%d, %s\n",r,strerror(-r)); 126 | + } 127 | +#endif 128 | + 129 | /* bind socket to protocol, in this case RAW EtherCAT */ 130 | sll.sll_family = AF_PACKET; 131 | sll.sll_ifindex = ifindex; 132 | sll.sll_protocol = htons(ETH_P_ECAT); 133 | r = bind(*psock, (struct sockaddr *)&sll, sizeof(sll)); 134 | + if(r<0){ 135 | + printf("binding socket failed:%d, %s\n",r,strerror(-r)); 136 | + } 137 | /* get flags */ 138 | - fl = fcntl(*psock, F_GETFL, 0); 139 | + //fl = fcntl(*psock, F_GETFL, 0); 140 | /* set nodelay option, so make socket non-blocking */ 141 | // r = fcntl(*psock, F_SETFL, fl | O_NDELAY); 142 | /* setup ethernet headers in tx buffers so we don't have to repeat it */ 143 | @@ -244,7 +301,7 @@ 144 | ec_setupheader(&ec_txbuf2); 145 | ec_errcnt = ec_incnt = 0; 146 | if (r == 0) rval = 1; 147 | - 148 | + 149 | return rval; 150 | } 151 | 152 | diff -uar src/slaveinfo.c src/slaveinfo.c 153 | --- src/slaveinfo.c 2010-03-07 15:48:41.000000000 +0100 154 | +++ src/slaveinfo.c 2011-05-16 14:28:49.476931001 +0200 155 | @@ -22,6 +22,10 @@ 156 | #include "ethercatdc.h" 157 | #include "ethercatprint.h" 158 | 159 | +#ifdef HAVE_RTNET 160 | +#include 161 | +#endif 162 | + 163 | char IOmap[4096]; 164 | ec_ODlistt ODlist; 165 | ec_OElistt OElist; 166 | @@ -281,6 +286,9 @@ 167 | 168 | if (argc > 1) 169 | { 170 | +#ifdef HAVE_RTNET 171 | + mlockall(MCL_CURRENT | MCL_FUTURE); 172 | +#endif 173 | if ((argc > 2) && (strncmp(argv[2], "-sdo", sizeof("-sdo")) == 0)) printSDO = TRUE; 174 | /* start slaveinfo */ 175 | slaveinfo(argv[1]); 176 | -------------------------------------------------------------------------------- /soem_core/soem.patch: -------------------------------------------------------------------------------- 1 | --- src/nicdrv.c 2010-04-10 16:06:30.000000000 +0200 2 | +++ src/nicdrv.c 2011-04-21 10:55:35.000000000 +0200 3 | @@ -208,31 +208,49 @@ 4 | } 5 | /* we use RAW packet socket, with packet type ETH_P_ECAT */ 6 | *psock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ECAT)); 7 | + if(*psock<0) 8 | + printf("Failed to create socket: %s\n",strerror(-(*psock))); 9 | timeout.tv_sec = 0; 10 | timeout.tv_usec = 1000; 11 | 12 | r = setsockopt(*psock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); 13 | + if(r<0) 14 | + printf("Failed to set option recvtimeo: %s\n",strerror(-r)); 15 | r = setsockopt(*psock, SOL_SOCKET, SO_SNDTIMEO, &timeout, sizeof(timeout)); 16 | + if(r<0) 17 | + printf("Failed to set option sndtimeo: %s\n",strerror(-r)); 18 | i = 1; 19 | r = setsockopt(*psock, SOL_SOCKET, SO_DONTROUTE, &i, sizeof(i)); 20 | + if(r<0) 21 | + printf("Failed to set option dontroute: %s\n",strerror(-r)); 22 | + 23 | /* connect socket to NIC by name */ 24 | strcpy(ifr.ifr_name, ifname); 25 | r = ioctl(*psock, SIOCGIFINDEX, &ifr); 26 | + if(r<0) 27 | + printf("Failed to get socket index: %s\n",strerror(-r)); 28 | ifindex = ifr.ifr_ifindex; 29 | strcpy(ifr.ifr_name, ifname); 30 | ifr.ifr_flags = 0; 31 | /* reset flags of NIC interface */ 32 | r = ioctl(*psock, SIOCGIFFLAGS, &ifr); 33 | + if(r<0) 34 | + printf("Failed to get socket flags: %s\n",strerror(-r)); 35 | /* set flags of NIC interface, here promiscuous and broadcast */ 36 | - ifr.ifr_flags = ifr.ifr_flags || IFF_PROMISC || IFF_BROADCAST; 37 | - r = ioctl(*psock, SIOCGIFFLAGS, &ifr); 38 | + ifr.ifr_flags = ifr.ifr_flags | IFF_PROMISC | IFF_BROADCAST; 39 | + r = ioctl(*psock, SIOCSIFFLAGS, &ifr); 40 | + if(r<0) 41 | + printf("Failed to set socket flags: %s\n",strerror(-r)); 42 | /* bind socket to protocol, in this case RAW EtherCAT */ 43 | sll.sll_family = AF_PACKET; 44 | sll.sll_ifindex = ifindex; 45 | sll.sll_protocol = htons(ETH_P_ECAT); 46 | r = bind(*psock, (struct sockaddr *)&sll, sizeof(sll)); 47 | + if(r<0) 48 | + printf("Failed to bind socket: %s\n",strerror(-r)); 49 | + 50 | /* get flags */ 51 | - fl = fcntl(*psock, F_GETFL, 0); 52 | + //fl = fcntl(*psock, F_GETFL, 0); 53 | /* set nodelay option, so make socket non-blocking */ 54 | // r = fcntl(*psock, F_SETFL, fl | O_NDELAY); 55 | /* setup ethernet headers in tx buffers so we don't have to repeat it */ 56 | -------------------------------------------------------------------------------- /soem_core/trinamic.patch: -------------------------------------------------------------------------------- 1 | *** src/ethercatconfiglist.h.2 2010-09-24 16:00:44.000000000 +0200 2 | --- src/ethercatconfiglist.h 2010-09-21 15:38:19.000000000 +0200 3 | *************** 4 | *** 83,87 **** 5 | --- 83,88 ---- 6 | {/*Man=*/0x00000002,/*ID=*/0x101a3052,/*Name=*/"EL4122" ,/*dtype=*/5,/*Ibits=*/ 0,/*Obits=*/32,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00000022,/*FM0ac*/1,/*FM1ac*/0}, 7 | {/*Man=*/0x00000002,/*ID=*/0x10243052,/*Name=*/"EL4132" ,/*dtype=*/5,/*Ibits=*/ 0,/*Obits=*/32,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00000022,/*FM0ac*/1,/*FM1ac*/0}, 8 | {/*Man=*/0x00000002,/*ID=*/0x13ed3052,/*Name=*/"EL5101" ,/*dtype=*/7,/*Ibits=*/40,/*Obits=*/24,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/1,/*FM1ac*/1}, 9 | + {/*Man=*/0x00000286,/*ID=*/0x00000011,/*Name=*/"Trinamic ",/*dtype=*/7,/*Ibits=*/320,/*Obits=*/320,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/1,/*FM1ac*/1}, 10 | {/*Man=*/EC_CONFIGEND,/*ID=*/0x00000000,/*Name=*/"" ,/*dtype=*/0,/*Ibits=*/ 0,/*Obits=*/ 0} 11 | }; 12 | -------------------------------------------------------------------------------- /soem_ebox/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # The find_package macro for Orocos-RTT works best with 3 | # cmake >= 2.6.3 4 | # 5 | cmake_minimum_required(VERSION 2.6.3) 6 | 7 | # 8 | # This creates a standard cmake project. You may extend this file with 9 | # any cmake macro you see fit. 10 | # 11 | project(soem_ebox) 12 | 13 | #uncomment if you have defined messages 14 | if(NOT ORO_USE_ROSBUILD) 15 | find_package(catkin REQUIRED message_generation rtt_roscomm) 16 | add_message_files( FILES 17 | EBOXAnalog.msg 18 | EBOXDigital.msg 19 | EBOXOut.msg 20 | EBOXPWM.msg 21 | ) 22 | 23 | generate_messages() 24 | endif() 25 | 26 | 27 | # Set the CMAKE_PREFIX_PATH in case you're not using Orocos through ROS 28 | # for helping these find commands find RTT. 29 | find_package(OROCOS-RTT REQUIRED ${RTT_HINTS}) 30 | 31 | # Defines the orocos_* cmake macros. See that file for additional 32 | # documentation. 33 | include(${OROCOS-RTT_USE_FILE_PATH}/UseOROCOS-RTT.cmake) 34 | 35 | if(ORO_USE_ROSBUILD) 36 | rosbuild_genmsg() 37 | rosbuild_include(rtt_rosnode GenerateRTTtypekit) 38 | endif() 39 | 40 | ros_generate_rtt_typekit(soem_ebox) 41 | 42 | 43 | find_package(soem REQUIRED) 44 | include_directories(${soem_INCLUDE_DIRS}) 45 | 46 | orocos_use_package(soem_master) 47 | 48 | # 49 | # Components, types and plugins. 50 | # 51 | orocos_plugin(soem_ebox src/soem_ebox.cpp) # ...only one plugin function per library ! 52 | target_link_libraries(soem_ebox ${soem_LIBRARIES}) 53 | # 54 | 55 | # Generates and installs our orocos-soem_ebox-.pc and manifest.xml file 56 | # 57 | if(NOT ORO_USE_ROSBUILD) 58 | add_dependencies(soem_ebox ${PROJECT_NAME}_generate_messages_cpp) 59 | endif() 60 | 61 | orocos_generate_package() 62 | -------------------------------------------------------------------------------- /soem_ebox/Makefile: -------------------------------------------------------------------------------- 1 | # This Makefile is here for 'rosmake' like systems. In case you don't use 2 | # ROS, it will create a build directory for you and build the package with 3 | # default settings. It will install it at the same location as the RTT is installed. 4 | ifdef ROS_ROOT 5 | include $(shell rospack find mk)/cmake.mk 6 | else 7 | $(warning This Makefile builds this package with default settings) 8 | all: 9 | mkdir -p build 10 | cd build ; cmake .. -DINSTALL_PATH=orocos && make 11 | echo -e "\n Now do 'make install' to install this package.\n" 12 | install: all 13 | cd build ; make install 14 | endif -------------------------------------------------------------------------------- /soem_ebox/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This package contains the components of the soem_ebox package 4 | 5 | Ruben Smits 6 | BSD 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /soem_ebox/msg/EBOXAnalog.msg: -------------------------------------------------------------------------------- 1 | float32[2] analog 2 | -------------------------------------------------------------------------------- /soem_ebox/msg/EBOXDigital.msg: -------------------------------------------------------------------------------- 1 | bool[8] digital 2 | -------------------------------------------------------------------------------- /soem_ebox/msg/EBOXOut.msg: -------------------------------------------------------------------------------- 1 | bool[2] trigger 2 | bool[8] digital 3 | float32[2] analog 4 | uint32 timestamp 5 | int32[2] encoder -------------------------------------------------------------------------------- /soem_ebox/msg/EBOXPWM.msg: -------------------------------------------------------------------------------- 1 | float32[2] pwm -------------------------------------------------------------------------------- /soem_ebox/package.xml: -------------------------------------------------------------------------------- 1 | 2 | soem_ebox 3 | 0.1.1 4 | 5 | This package contains the components of the soem_ebox package 6 | 7 | 8 | Ruben Smits 9 | 10 | BSD 11 | 12 | http://wiki.ros.org/soem_master 13 | 14 | 15 | Ruben Smits 16 | 17 | catkin 18 | 19 | rtt 20 | soem 21 | soem_master 22 | message_generation 23 | rtt_roscomm 24 | 25 | rtt 26 | soem 27 | soem_master 28 | message_runtime 29 | rtt_roscomm 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /soem_ebox/src/soem_ebox.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | soem_ebox.cpp - description 3 | ------------------- 4 | begin : Tue February 15 2011 5 | copyright : (C) 2011 Ruben Smits 6 | email : first.last@mech.kuleuven.be 7 | 8 | *************************************************************************** 9 | * This library is free software; you can redistribute it and/or * 10 | * modify it under the terms of the GNU Lesser General Public * 11 | * License as published by the Free Software Foundation; either * 12 | * version 2.1 of the License, or (at your option) any later version. * 13 | * * 14 | * This library is distributed in the hope that it will be useful, * 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 17 | * Lesser General Public License for more details. * 18 | * * 19 | * You should have received a copy of the GNU Lesser General Public * 20 | * License along with this library; if not, write to the Free Software * 21 | * Foundation, Inc., 59 Temple Place, * 22 | * Suite 330, Boston, MA 02111-1307 USA * 23 | * * 24 | ***************************************************************************/ 25 | #include "soem_ebox.h" 26 | #include 27 | #include 28 | 29 | namespace soem_ebox 30 | { 31 | 32 | SoemEBox::SoemEBox(ec_slavet* mem_loc) : 33 | soem_master::SoemDriver(mem_loc) 34 | { 35 | this->m_service->doc("Services for SMF Ketels E/BOX"); 36 | this->m_service->addOperation("readAnalog", &SoemEBox::readAnalog, this, 37 | RTT::OwnThread).doc("Read analog in value (in Volts)").arg("chan", 38 | "channel to read (shold be 0 or 1)"); 39 | this->m_service->addOperation("checkBit", &SoemEBox::checkBit, this, 40 | RTT::OwnThread).doc("check value of Digital In").arg("bit", 41 | "input to check"); 42 | this->m_service->addOperation("readEncoder", &SoemEBox::readEncoder, this, 43 | RTT::OwnThread).doc("Read Encoder value (in ticks)").arg("chan", 44 | "channel to read"); 45 | this->m_service->addOperation("writeAnalog", &SoemEBox::writeAnalog, this, 46 | RTT::OwnThread).doc( 47 | "Set the value of the analog output chan to value (in Volts)").arg( 48 | "chan", "output channel to set").arg("value", "value to set"); 49 | this->m_service->addOperation("setBit", &SoemEBox::setBit, this, 50 | RTT::OwnThread).doc("Set the digital output bit to value").arg( 51 | "bit", "digital output to set").arg("value", "value to set"); 52 | this->m_service->addOperation("writePWM", &SoemEBox::writePWM, this, 53 | RTT::OwnThread).doc("Set the PWM channel to value (0..1)").arg( 54 | "chan", "PWM channel to set").arg("value", "value to set"); 55 | this->m_service->addOperation("armTrigger", &SoemEBox::armTrigger, this, 56 | RTT::OwnThread).doc("Arm the trigger of encoder chan").arg("chan", 57 | "Encoder to trigger"); 58 | this->m_service->addOperation("readTrigger", &SoemEBox::readTrigger, this, 59 | RTT::OwnThread).doc("Read the trigger value of encoder chan").arg("chan", 60 | "Channel to read"); 61 | this->m_service->addOperation("writeTriggerValue", &SoemEBox::writeTriggerValue, this, 62 | RTT::OwnThread); 63 | 64 | 65 | 66 | 67 | this->m_service->addPort("Measurements", port_input); 68 | this->m_service->addPort("AnalogIn", port_output_analog); 69 | this->m_service->addPort("DigitalIn", port_output_digital); 70 | this->m_service->addPort("PWMIn", port_output_pwm); 71 | 72 | //Initialize output 73 | m_output.analog[0] = 0; 74 | m_output.analog[1] = 0; 75 | m_output.digital = 0; 76 | m_output.pwm[0] = 0; 77 | m_output.pwm[1] = 0; 78 | m_output.control = 0; 79 | 80 | } 81 | 82 | bool SoemEBox::configure() 83 | { 84 | return true; 85 | } 86 | 87 | void SoemEBox::update() 88 | { 89 | m_input = *((in_eboxt*) (m_datap->inputs)); 90 | EBOXOut out_msg; 91 | std::bitset < 8 > bit_tmp; 92 | bit_tmp = m_input.status; 93 | 94 | for (unsigned int i = 0; i < 2; i++) 95 | { 96 | out_msg.analog[i] = (float) m_input.analog[i] 97 | * (float) EBOX_AIN_COUNTSTOVOLTS; 98 | out_msg.encoder[i] = m_input.encoder[i]; 99 | out_msg.trigger[i] = bit_tmp[i]; 100 | } 101 | 102 | bit_tmp = m_input.digital; 103 | for (unsigned int i = 0; i < 8; i++) 104 | out_msg.digital[i] = bit_tmp.test(i); 105 | 106 | out_msg.timestamp = m_input.timestamp; 107 | port_input.write(out_msg); 108 | 109 | EBOXAnalog analog_msg; 110 | if (port_output_analog.read(analog_msg) == NewData) 111 | for (unsigned int i = 0; i < 2; i++) 112 | writeAnalog(i, analog_msg.analog[i]); 113 | 114 | EBOXDigital digital_msg; 115 | if (port_output_digital.read(digital_msg) == NewData) 116 | { 117 | for (unsigned int i = 0; i < 8; i++) 118 | bit_tmp.set(i, (digital_msg.digital[i] != 0)); 119 | m_output.digital = bit_tmp.to_ulong(); 120 | } 121 | 122 | EBOXPWM pwm_msg; 123 | if (port_output_pwm.read(pwm_msg) == NewData) 124 | for (unsigned int i = 0; i < 2; i++) 125 | writePWM(i, pwm_msg.pwm[i]); 126 | 127 | *(out_eboxt*) (m_datap->outputs) = m_output; 128 | } 129 | 130 | double SoemEBox::readAnalog(unsigned int chan) 131 | { 132 | if (checkChannelRange(chan)) 133 | return (double) m_input.analog[chan] * (double) EBOX_AIN_COUNTSTOVOLTS; 134 | else 135 | return 0.0; 136 | } 137 | 138 | bool SoemEBox::checkBit(unsigned int bit) 139 | { 140 | if (checkBitRange(bit)) 141 | return std::bitset<8> (m_input.digital).test(bit); 142 | else 143 | return false; 144 | } 145 | 146 | int SoemEBox::readEncoder(unsigned int chan) 147 | { 148 | if (checkChannelRange(chan)) 149 | return m_input.encoder[chan]; 150 | else 151 | return false; 152 | } 153 | 154 | int SoemEBox::readTrigger(unsigned int chan) 155 | { 156 | if (checkChannelRange(chan)) 157 | { 158 | if ( std::bitset<8> (m_input.status).test(chan) ) 159 | { 160 | return 1; 161 | } else { 162 | return 0; 163 | } 164 | } else { 165 | return -1; 166 | } 167 | } 168 | 169 | bool SoemEBox::writeAnalog(unsigned int chan, double value) 170 | { 171 | if (checkChannelRange(chan)) 172 | { 173 | int sign = (value > 0) - (value < 0); 174 | m_output.analog[chan] = sign * ceil(std::min(fabs(value) 175 | / (double) EBOX_AOUT_MAX * EBOX_AOUT_COUNTS, 176 | (double) EBOX_AOUT_COUNTS)); 177 | return true; 178 | } 179 | return false; 180 | } 181 | 182 | bool SoemEBox::setBit(unsigned int bit, bool value) 183 | { 184 | if (checkBitRange(bit)) 185 | { 186 | std::bitset < 8 > tmp(m_output.digital); 187 | tmp.set(bit, value); 188 | m_output.digital = tmp.to_ulong(); 189 | return true; 190 | } 191 | return false; 192 | } 193 | bool SoemEBox::writePWM(unsigned int chan, double value) 194 | { 195 | if (checkChannelRange(chan)) 196 | { 197 | m_output.pwm[chan] = (int16)(value * EBOX_PWM_MAX); 198 | return true; 199 | } 200 | return false; 201 | } 202 | 203 | bool SoemEBox::armTrigger(unsigned int chan) 204 | { 205 | if (checkChannelRange(chan)) 206 | { 207 | std::bitset < 8 > tmp(m_output.control); 208 | tmp.set(chan); 209 | m_output.control = tmp.to_ulong(); 210 | return true; 211 | } 212 | return false; 213 | } 214 | 215 | bool SoemEBox::writeTriggerValue(unsigned int chan, bool value ) 216 | { 217 | if ( checkChannelRange( chan ) ) 218 | { 219 | std::bitset < 8 > tmp(m_output.control); 220 | if ( value ) 221 | { 222 | tmp.set( chan ); 223 | } 224 | else 225 | { 226 | tmp.reset( chan ); 227 | } 228 | m_output.control = tmp.to_ulong(); 229 | return true; 230 | } 231 | 232 | return false; 233 | } 234 | 235 | 236 | namespace 237 | { 238 | soem_master::SoemDriver* createSoemEBox(ec_slavet* mem_loc) 239 | { 240 | return new SoemEBox(mem_loc); 241 | } 242 | const bool registered1 = 243 | soem_master::SoemDriverFactory::Instance().registerDriver("E/BOX", 244 | createSoemEBox); 245 | }//namespace 246 | }//namespace 247 | 248 | #include 249 | 250 | extern "C" 251 | { 252 | bool loadRTTPlugin(RTT::TaskContext* c) 253 | { 254 | return true; 255 | } 256 | } 257 | -------------------------------------------------------------------------------- /soem_ebox/src/soem_ebox.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | soem_ebox.h - description 3 | ------------------- 4 | begin : Tue February 15 2011 5 | copyright : (C) 2011 Ruben Smits 6 | email : first.last@mech.kuleuven.be 7 | 8 | *************************************************************************** 9 | * This library is free software; you can redistribute it and/or * 10 | * modify it under the terms of the GNU Lesser General Public * 11 | * License as published by the Free Software Foundation; either * 12 | * version 2.1 of the License, or (at your option) any later version. * 13 | * * 14 | * This library is distributed in the hope that it will be useful, * 15 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 16 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 17 | * Lesser General Public License for more details. * 18 | * * 19 | * You should have received a copy of the GNU Lesser General Public * 20 | * License along with this library; if not, write to the Free Software * 21 | * Foundation, Inc., 59 Temple Place, * 22 | * Suite 330, Boston, MA 02111-1307 USA * 23 | * * 24 | ***************************************************************************/ 25 | 26 | #ifndef SOEM_EBOX_H 27 | #define SOEM_EBOX_H 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | using namespace RTT; 40 | 41 | namespace soem_ebox 42 | { 43 | #define EBOX_AOUT_COUNTS 32767 44 | #define EBOX_AOUT_MAX 10 45 | #define EBOX_AIN_COUNTSTOVOLTS 10/100000 46 | #define EBOX_PWM_MAX 2000 47 | 48 | class SoemEBox: public soem_master::SoemDriver 49 | { 50 | 51 | /** 52 | * Output buffer 53 | */ 54 | typedef struct PACKED 55 | { 56 | /** 57 | * Control bits: 58 | * bit 0: arm index trigger of encoder 1 59 | * bit 1: arm index trigger of encoder 2 60 | */ 61 | uint8 control; 62 | /** 63 | * Digital outputs 64 | */ 65 | uint8 digital; 66 | /** 67 | * Analog output: 68 | * range: +-10V mapped to +-32767 69 | */ 70 | int16 analog[2]; 71 | /** 72 | * PWM outputs: 73 | * 0-100% duty cycle is mapped to 2000 counts (clipped) 74 | */ 75 | int16 pwm[2]; 76 | } out_eboxt; 77 | 78 | /** 79 | * Input buffer 80 | */ 81 | typedef struct PACKED 82 | { 83 | /** 84 | * Status bits: 85 | * bit 0: index of encoder 1 triggered 86 | * bit 1: index of encoder 2 triggered 87 | */ 88 | uint8 status; 89 | /** 90 | * Internal cycle counter: 91 | * after 255 -> 0 92 | */ 93 | uint8 counter; 94 | /** 95 | * Digital inputs 96 | */ 97 | uint8 digital; 98 | /** 99 | * Analog input: 100 | * range: +-10V mapped to +-100000 counts 101 | * When filter parameter n > 1 the range is n*100000 (remark: value is sum of n measurements, not mean) 102 | */ 103 | int32 analog[2]; 104 | /** 105 | * Timestamp of analog inputs: 106 | * 32 LSB of the EtherCAT time fo the last ADC trigger 107 | */ 108 | uint32 timestamp; 109 | /** 110 | * Encoder inputs: 111 | * count range is +-2147483648 (remark: not overflow protected!!!) 112 | */ 113 | int32 encoder[2]; 114 | } in_eboxt; 115 | 116 | public: 117 | SoemEBox(ec_slavet* mem_loc); 118 | ~SoemEBox() 119 | { 120 | } 121 | ; 122 | 123 | void update(); 124 | bool configure(); 125 | 126 | private: 127 | /** 128 | * Read Analog In value (in V) 129 | * @param chan channel to read (should be 0 or 1) 130 | * @return analog voltage value 131 | */ 132 | double readAnalog(unsigned int chan); 133 | /** 134 | * check value of Digital In 135 | * @param bit input to check 136 | * @return status of the digital input 137 | */ 138 | bool checkBit(unsigned int bit); 139 | /** 140 | * Read Encoder value (in ticks) 141 | * @param chan channel to read 142 | * @return counted ticks since last reset 143 | */ 144 | int readEncoder(unsigned int chan); 145 | 146 | /** 147 | * Set the value of the analog output chan to value (in Volts) 148 | * @param chan output channel to set 149 | * @param value value to set 150 | * @return true if succeeded, false otherwise 151 | */ 152 | bool writeAnalog(unsigned int chan, double value); 153 | /** 154 | * Set the digital output bit to value 155 | * @param bit digital output to set 156 | * @param value value to set 157 | * @return true if succeeded, false otherwise 158 | */ 159 | bool setBit(unsigned int bit, bool value); 160 | /** 161 | * Set the PWM channel to value (0..1) 162 | * @param chan PWM channel to set 163 | * @param value value to set 164 | * @return true if succeeded, false otherwise 165 | */ 166 | bool writePWM(unsigned int chan, double value); 167 | 168 | /** 169 | * Arm the trigger of the Encoder (reset to zero at next I pulse) 170 | * @param chan encoder trigger to arm 171 | * @return true if succeeded, false otherwise 172 | */ 173 | bool armTrigger(unsigned int chan); 174 | 175 | int readTrigger(unsigned int chan); 176 | 177 | bool writeTriggerValue(unsigned int chan,bool value); 178 | 179 | inline bool checkChannelRange(unsigned int chan) 180 | { 181 | Logger::In(this->getName()); 182 | if (chan > 2) 183 | { 184 | log(Error) << "Channel value " << chan 185 | << "to big, chan should be 0 or 1" << endlog(); 186 | return false; 187 | } 188 | return true; 189 | } 190 | 191 | inline bool checkBitRange(unsigned int bit) 192 | { 193 | Logger::In(this->getName()); 194 | if (bit > 7) 195 | { 196 | log(Error) << "Bit " << bit << "to big, bit should be 0..7" 197 | << endlog(); 198 | return false; 199 | } 200 | return true; 201 | } 202 | 203 | out_eboxt m_output; 204 | in_eboxt m_input; 205 | 206 | OutputPort port_input; 207 | InputPort port_output_analog; 208 | InputPort port_output_pwm; 209 | InputPort port_output_digital; 210 | 211 | };//class 212 | 213 | }//namespace 214 | #endif 215 | -------------------------------------------------------------------------------- /soem_master/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | 3 | project(soem_master) 4 | 5 | # Set the CMAKE_PREFIX_PATH in case you're not using Orocos through 6 | # ROS for helping these find commands find RTT. 7 | find_package(OROCOS-RTT REQUIRED ${RTT_HINTS} ) 8 | find_package(catkin QUIET) 9 | 10 | # Defines the orocos_* cmake macros. See that file for additional 11 | # documentation. 12 | include(${OROCOS-RTT_USE_FILE_PATH}/UseOROCOS-RTT.cmake) 13 | 14 | find_package(soem REQUIRED) 15 | include_directories(${soem_INCLUDE_DIRS}) 16 | 17 | #common commands for building c++ executables and libraries 18 | orocos_library(soem_driver_factory soem_driver_factory.cpp) 19 | orocos_component(soem_master soem_master_component.cpp) 20 | target_link_libraries(soem_master soem_driver_factory ${soem_LIBRARIES}) 21 | 22 | orocos_plugin(soem_plugin soem_plugin.cpp) 23 | 24 | orocos_install_headers( soem_driver.h soem_driver_factory.h) 25 | 26 | install(PROGRAMS setcap.sh DESTINATION lib/${PROJECT_NAME}) 27 | 28 | orocos_generate_package( INCLUDE_DIRS ..) 29 | 30 | -------------------------------------------------------------------------------- /soem_master/Makefile: -------------------------------------------------------------------------------- 1 | EXTRA_CMAKE_FLAGS += -DORO_USE_ROSBUILD=True 2 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /soem_master/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b soem_master is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /soem_master/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | soem_master contains a C++ wrapper around soem_core, a factory object to register and create drivers and a RTT component that will automatically create the drivers and their services for all the slave for which a driver is known. 5 | 6 | 7 | Ruben Smits 8 | BSD 9 | 10 | http://ros.org/wiki/soem_master 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /soem_master/package.xml: -------------------------------------------------------------------------------- 1 | 2 | soem_master 3 | 0.1.1 4 | 5 | soem_master contains a C++ wrapper around soem_core, a factory object to register and create drivers and a RTT component that will automatically create the drivers and their services for all the slave for which a driver is known. 6 | 7 | 8 | Ruben Smits 9 | 10 | LGPL 11 | 12 | http://wiki.ros.org/soem_master 13 | 14 | 15 | Ruben Smits 16 | 17 | catkin 18 | 19 | rtt 20 | soem 21 | rtt 22 | soem 23 | 24 | 25 | -------------------------------------------------------------------------------- /soem_master/setcap.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | sudo setcap cap_net_raw+ep `which deployer-$OROCOS_TARGET` 4 | if [ $? -ne 0 ]; then 5 | echo "Failed to give deployer-$OROCOS_TARGET raw network capabilities" 6 | exit $? 7 | fi 8 | sudo setcap cap_net_raw+ep `which rttlua-$OROCOS_TARGET` 9 | if [ $? -ne 0 ]; then 10 | echo "Failed to give rttlua-$OROCOS_TARGET raw network capabilities" 11 | exit $? 12 | fi 13 | echo "Successfully gave deployer-$OROCOS_TARGET and rttlua-$OROCOS_TARGET raw network capabilities" 14 | 15 | exit 0 16 | -------------------------------------------------------------------------------- /soem_master/soem_driver.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:26:15 CET 2010 soem_driver_factory.cpp 3 | 4 | soem_driver_factory.cpp - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #ifndef _SOEM_DRIVER_H_ 29 | #define _SOEM_DRIVER_H_ 30 | 31 | #define REGISTER_SOEM_DRIVER( NAME, FACTORY_METHOD )\ 32 | const bool registered_ ## NAME = \ 33 | soem_master::SoemDriverFactory::Instance().registerDriver( #NAME, FACTORY_METHOD ); 34 | 35 | extern "C" 36 | { 37 | #include 38 | #include 39 | #include 40 | #include 41 | } 42 | 43 | #include 44 | #include 45 | 46 | #include 47 | 48 | template 49 | inline std::string to_string(const T& t, std::ios_base & (*f)(std::ios_base&)) 50 | { 51 | std::stringstream ss; 52 | ss << f << t; 53 | return ss.str(); 54 | } 55 | ; 56 | 57 | namespace soem_master 58 | { 59 | 60 | class SoemDriver 61 | { 62 | public: 63 | virtual ~SoemDriver() 64 | { 65 | m_service->clear(); 66 | } 67 | ; 68 | 69 | const std::string& getName() const 70 | { 71 | return m_name; 72 | } 73 | 74 | RTT::Service::shared_ptr provides() 75 | { 76 | return RTT::Service::shared_ptr(m_service); 77 | } 78 | ; 79 | 80 | virtual void update()=0; 81 | virtual bool configure() 82 | { 83 | return true; 84 | } 85 | ; 86 | 87 | virtual bool start() 88 | { 89 | return true; 90 | }; 91 | 92 | virtual bool requestState( ec_state state){ 93 | m_datap->state = state; 94 | ec_writestate(m_slave_nr); 95 | ec_statecheck(m_slave_nr,state,EC_TIMEOUTSTATE); 96 | return m_datap->state == state; 97 | }; 98 | 99 | virtual bool checkState( ec_state state){ 100 | ec_statecheck(m_slave_nr,state,EC_TIMEOUTSTATE); 101 | return m_datap->state == state; 102 | }; 103 | 104 | virtual ec_state getState(){ 105 | return (ec_state)(m_datap->state); 106 | }; 107 | 108 | protected: 109 | SoemDriver(ec_slavet* mem_loc) : 110 | m_datap(mem_loc), m_name("Slave_" + to_string(m_datap->configadr, 111 | std::hex)), m_service(new RTT::Service(m_name)),m_slave_nr(m_datap->configadr & 0x0f) 112 | { 113 | m_service->addOperation("requestState",&SoemDriver::requestState,this).doc("request slave state").arg("state","Desired state"); 114 | m_service->addOperation("checkState",&SoemDriver::checkState,this).doc("check the slaves state").arg("state","state value to check"); 115 | m_service->addOperation("getState",&SoemDriver::getState,this).doc("request slave state"); 116 | m_service->addOperation("configure",&SoemDriver::configure,this).doc("Configure slave"); 117 | } 118 | ; 119 | ec_slavet* m_datap; 120 | std::string m_name; 121 | RTT::Service::shared_ptr m_service; 122 | 123 | unsigned int m_slave_nr; 124 | }; 125 | } 126 | #endif 127 | -------------------------------------------------------------------------------- /soem_master/soem_driver_factory.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:26:15 CET 2010 soem_driver_factory.cpp 3 | 4 | soem_driver_factory.cpp - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #include "soem_driver_factory.h" 29 | 30 | namespace soem_master 31 | { 32 | using namespace RTT; 33 | bool SoemDriverFactory::registerDriver(std::string name, 34 | CreateDriverCallBack create_fn) 35 | { 36 | Logger::In in("SoemDriverFactor"); 37 | log(Info) << "Registering driver for " << name << endlog(); 38 | return m_factory_map.insert(FactoryMap::value_type(name, create_fn)).second; 39 | } 40 | 41 | SoemDriver* SoemDriverFactory::createDriver(ec_slavet* mem_loc) 42 | { 43 | FactoryMap::const_iterator it = m_factory_map.find(std::string( 44 | mem_loc->name)); 45 | if (it == m_factory_map.end()) 46 | { 47 | return NULL; 48 | } 49 | return (it->second)(mem_loc); 50 | } 51 | 52 | void SoemDriverFactory::displayAvailableDrivers() 53 | { 54 | Logger::In in("SoemDriverFactory"); 55 | log(Info) << "Following SOEM drivers are registered: \n"<< endlog(); 56 | for (FactoryMap::const_iterator it = m_factory_map.begin(); it 57 | != m_factory_map.end(); ++it) 58 | { 59 | log(Info) << "\t" << it->first << endlog(); 60 | } 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /soem_master/soem_driver_factory.h: -------------------------------------------------------------------------------- 1 | /*************************************************************************** 2 | tag: Ruben Smits Tue Nov 16 09:26:15 CET 2010 soem_driver_factory.cpp 3 | 4 | soem_driver_factory.h - description 5 | ------------------- 6 | begin : Tue November 16 2010 7 | copyright : (C) 2010 Ruben Smits 8 | email : first.last@mech.kuleuven.be 9 | 10 | *************************************************************************** 11 | * This library is free software; you can redistribute it and/or * 12 | * modify it under the terms of the GNU Lesser General Public * 13 | * License as published by the Free Software Foundation; either * 14 | * version 2.1 of the License, or (at your option) any later version. * 15 | * * 16 | * This library is distributed in the hope that it will be useful, * 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * 19 | * Lesser General Public License for more details. * 20 | * * 21 | * You should have received a copy of the GNU Lesser General Public * 22 | * License along with this library; if not, write to the Free Software * 23 | * Foundation, Inc., 59 Temple Place, * 24 | * Suite 330, Boston, MA 02111-1307 USA * 25 | * * 26 | ***************************************************************************/ 27 | 28 | #ifndef SOEM_DRIVER_FACTORY_H 29 | #define SOEM_DRIVER_FACTORY_H 30 | 31 | #include 32 | #include 33 | #include "soem_driver.h" 34 | 35 | namespace soem_master 36 | { 37 | 38 | class SoemDriverFactory 39 | { 40 | public: 41 | static SoemDriverFactory& Instance() 42 | { 43 | static SoemDriverFactory soem_driver_factory; 44 | return soem_driver_factory; 45 | } 46 | 47 | typedef SoemDriver* (*CreateDriverCallBack)(ec_slavet*); 48 | 49 | bool registerDriver(std::string name, CreateDriverCallBack createFn); 50 | 51 | SoemDriver* createDriver(ec_slavet* mem_loc); 52 | void displayAvailableDrivers(); 53 | 54 | private: 55 | typedef std::map FactoryMap; 56 | FactoryMap m_factory_map; 57 | 58 | SoemDriverFactory() 59 | { 60 | } 61 | ; 62 | SoemDriverFactory(const SoemDriverFactory&); 63 | SoemDriverFactory& operator=(const SoemDriverFactory&); 64 | ~SoemDriverFactory() 65 | { 66 | } 67 | ; 68 | }; 69 | } 70 | #endif 71 | -------------------------------------------------------------------------------- /soem_master/soem_master_component.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2010 Ruben Smits 2 | 3 | // Author: Ruben Smits 4 | // Maintainer: Ruben Smits 5 | 6 | // This library is free software; you can redistribute it and/or 7 | // modify it under the terms of the GNU Lesser General Public 8 | // License as published by the Free Software Foundation; either 9 | // version 2.1 of the License, or (at your option) any later version. 10 | 11 | // This library is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | // Lesser General Public License for more details. 15 | 16 | // You should have received a copy of the GNU Lesser General Public 17 | // License along with this library; if not, write to the Free Software 18 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | extern "C" 21 | { 22 | #include "ethercattype.h" 23 | #include "nicdrv.h" 24 | #include "ethercatbase.h" 25 | #include "ethercatmain.h" 26 | #include "ethercatconfig.h" 27 | #include "ethercatdc.h" 28 | #include "ethercatcoe.h" 29 | #include "ethercatprint.h" 30 | } 31 | 32 | #include 33 | 34 | #include "soem_driver_factory.h" 35 | #include "soem_master_component.h" 36 | 37 | #include "rtt/Component.hpp" 38 | 39 | ORO_CREATE_COMPONENT( soem_master::SoemMasterComponent ) 40 | 41 | namespace soem_master 42 | { 43 | 44 | using namespace RTT; 45 | 46 | SoemMasterComponent::SoemMasterComponent(const std::string& name) : 47 | TaskContext(name, PreOperational) 48 | { 49 | this->addProperty("ifname", prop_ifname1="eth0").doc( 50 | "interface to which the ethercat device is connected"); 51 | this->addProperty("ifname2", prop_ifname2="eth1").doc( 52 | "Second (redundant) interface to which the ethercat device is connected"); 53 | this->addProperty("redundant", prop_redundant=false).doc( 54 | "Whether to use a redundant nic"); 55 | SoemDriverFactory& driver_factory = SoemDriverFactory::Instance(); 56 | this->addOperation("displayAvailableDrivers", 57 | &SoemDriverFactory::displayAvailableDrivers, &driver_factory).doc( 58 | "display all available drivers for the soem master"); 59 | //this->addOperation("start",&TaskContext::start,this,RTT::OwnThread); 60 | } 61 | 62 | SoemMasterComponent::~SoemMasterComponent() 63 | { 64 | } 65 | 66 | bool SoemMasterComponent::configureHook() 67 | { 68 | Logger::In in(this->getName()); 69 | 70 | int ret; 71 | if(prop_redundant && !prop_ifname1.empty() && !prop_ifname2.empty()) 72 | ret = ec_init_redundant((char*)prop_ifname1.c_str(),(char*)prop_ifname2.c_str()); 73 | else 74 | ret = ec_init((char*)prop_ifname1.c_str()); 75 | // initialise SOEM, bind socket to ifname 76 | if ( ret > 0) 77 | { 78 | log(Info) << "ec_init on " << prop_ifname1 << (prop_redundant ? std::string("and ") + prop_ifname2 : "")<<" succeeded." << endlog(); 79 | 80 | //Initialise default configuration, using the default config table (see ethercatconfiglist.h) 81 | if (ec_config_init(FALSE) > 0) 82 | { 83 | while (EcatError) 84 | { 85 | log(Error) << ec_elist2string() << endlog(); 86 | } 87 | 88 | log(Info) << ec_slavecount << " slaves found and configured." 89 | << endlog(); 90 | log(Info) << "Request pre-operational state for all slaves" 91 | << endlog(); 92 | ec_slave[0].state = EC_STATE_PRE_OP; 93 | ec_writestate(0); 94 | // wait for all slaves to reach PRE_OP state 95 | ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); 96 | 97 | for (int i = 1; i <= ec_slavecount; i++) 98 | { 99 | SoemDriver 100 | * driver = SoemDriverFactory::Instance().createDriver( 101 | &ec_slave[i]); 102 | if (driver) 103 | { 104 | m_drivers.push_back(driver); 105 | log(Info) << "Created driver for " << ec_slave[i].name 106 | << ", with address " << ec_slave[i].configadr 107 | << endlog(); 108 | //Adding driver's services to master component 109 | this->provides()->addService(driver->provides()); 110 | log(Info) << "Put configured parameters in the slaves." 111 | << endlog(); 112 | if (!driver->configure()) 113 | return false; 114 | } 115 | else 116 | { 117 | log(Warning) << "Could not create driver for " 118 | << ec_slave[i].name << endlog(); 119 | } 120 | } 121 | 122 | ec_config_map(&m_IOmap); 123 | while (EcatError) 124 | { 125 | log(Error) << ec_elist2string() << endlog(); 126 | } 127 | 128 | for (unsigned int i = 0; i < m_drivers.size(); i++) 129 | if (!m_drivers[i]->start()){ 130 | log(Error)<<"Could not start driver for "<getName()); 239 | if (ec_receive_processdata(EC_TIMEOUTRET) == 0) 240 | { 241 | success = false; 242 | log(Warning) << "receiving data failed" << endlog(); 243 | } 244 | 245 | if (success) 246 | for (unsigned int i = 0; i < m_drivers.size(); i++) 247 | m_drivers[i]->update(); 248 | 249 | if (ec_send_processdata() == 0) 250 | { 251 | success = false; 252 | log(Warning) << "sending process data failed" << endlog(); 253 | } 254 | while (EcatError) 255 | { 256 | log(Error) << ec_elist2string() << endlog(); 257 | } 258 | 259 | 260 | } 261 | 262 | void SoemMasterComponent::cleanupHook() 263 | { 264 | for (unsigned int i = 0; i < m_drivers.size(); i++){ 265 | this->provides()->removeService(m_drivers[i]->provides()->getName()); 266 | delete m_drivers[i]; 267 | } 268 | 269 | //stop SOEM, close socket 270 | ec_close(); 271 | } 272 | }//namespace 273 | -------------------------------------------------------------------------------- /soem_master/soem_master_component.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2010 Ruben Smits 2 | 3 | // Author: Ruben Smits 4 | // Maintainer: Ruben Smits 5 | 6 | // This library is free software; you can redistribute it and/or 7 | // modify it under the terms of the GNU Lesser General Public 8 | // License as published by the Free Software Foundation; either 9 | // version 2.1 of the License, or (at your option) any later version. 10 | 11 | // This library is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | // Lesser General Public License for more details. 15 | 16 | // You should have received a copy of the GNU Lesser General Public 17 | // License along with this library; if not, write to the Free Software 18 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 19 | #ifndef SOEM_MASTER_COMPONENT_H 20 | #define SOEM_MASTER_COMPONENT_H 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | #include "soem_driver.h" 28 | 29 | namespace soem_master 30 | { 31 | 32 | class SoemMasterComponent: public RTT::TaskContext 33 | { 34 | public: 35 | SoemMasterComponent(const std::string& name); 36 | ~SoemMasterComponent(); 37 | 38 | protected: 39 | virtual bool configureHook(); 40 | virtual bool startHook(); 41 | virtual void updateHook(); 42 | virtual void stopHook() 43 | { 44 | } 45 | ; 46 | virtual void cleanupHook(); 47 | 48 | private: 49 | std::string prop_ifname1, prop_ifname2; 50 | bool prop_redundant; 51 | char m_IOmap[4096]; 52 | std::vector m_drivers; 53 | };//class 54 | 55 | }//namespace 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /soem_master/soem_master_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /soem_master/soem_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace RTT; 7 | 8 | /** 9 | * An example plugin which can be loaded in a process. 10 | * Orocos plugins should provide at least these two functions: 11 | */ 12 | bool loadRTTPlugin( RTT::TaskContext* t ) 13 | { 14 | types::GlobalsRepository::shared_ptr globals = types::GlobalsRepository::Instance(); 15 | globals->setValue( new Constant("INIT",EC_STATE_INIT) ); 16 | globals->setValue( new Constant("PREOP",EC_STATE_PRE_OP) ); 17 | globals->setValue( new Constant("SAFEOP",EC_STATE_SAFE_OP) ); 18 | globals->setValue( new Constant("OP",EC_STATE_OPERATIONAL) ); 19 | return true; 20 | } 21 | 22 | std::string getRTTPluginName() 23 | { 24 | return "soem-plugin"; 25 | } 26 | -------------------------------------------------------------------------------- /soem_master/test.ops: -------------------------------------------------------------------------------- 1 | import("soem_master") 2 | import("soem_beckhoff_drivers") 3 | loadComponent("Master","soem_master::SoemMasterComponent") 4 | Master.displayAvailableDrivers() -------------------------------------------------------------------------------- /stack.xml: -------------------------------------------------------------------------------- 1 | 2 | soem 3 | Maintained by Ruben Smits 4 | GPL with runtime exception,BSD,LGPL 5 | 6 | http://ros.org/wiki/soem 7 | 8 | 9 | 10 | 11 | 12 | --------------------------------------------------------------------------------