diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..28105dd --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of +# directories (or patterns, but directories should suffice) that should +# be excluded from the distro. This is not the place to put things that +# should be ignored everywhere, like "build" directories; that happens in +# rosbuild/rosbuild.cmake. Here should be listed packages that aren't +# ready for inclusion in a distro. +# +# This list is combined with the list in rosbuild/rosbuild.cmake. Note +# that CMake 2.6 may be required to ensure that the two lists are combined +# properly. CMake 2.4 seems to have unpredictable scoping rules for such +# variables. +#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) + +rosbuild_make_distribution(0.1.0) diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..a818cca --- /dev/null +++ b/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake_stack.mk \ No newline at end of file diff --git a/soem_beckhoff_drivers/CMakeLists.txt b/soem_beckhoff_drivers/CMakeLists.txt new file mode 100644 index 0000000..b61c573 --- /dev/null +++ b/soem_beckhoff_drivers/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib/orocos/plugins) + +#uncomment if you have defined messages +rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +rosbuild_add_library(${PROJECT_NAME} src/soem_beckhoff_drivers.cpp + src/soem_el1xxx.cpp src/soem_el2xxx.cpp src/soem_el4004.cpp + src/soem_el4034.cpp src/soem_el4032.cpp src/soem_el3062.cpp + src/soem_el5101.cpp src/soem_el4038.cpp) + +#ros_generate_rtt_typekit() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) diff --git a/soem_beckhoff_drivers/Makefile b/soem_beckhoff_drivers/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/soem_beckhoff_drivers/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/soem_beckhoff_drivers/mainpage.dox b/soem_beckhoff_drivers/mainpage.dox new file mode 100644 index 0000000..1c1e7b0 --- /dev/null +++ b/soem_beckhoff_drivers/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b soem_beckhoff_drivers is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/soem_beckhoff_drivers/manifest.xml b/soem_beckhoff_drivers/manifest.xml new file mode 100644 index 0000000..3509c11 --- /dev/null +++ b/soem_beckhoff_drivers/manifest.xml @@ -0,0 +1,21 @@ + + + + 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. + + + Ruben Smits + BSD + + http://ros.org/wiki/soem_beckhoff_drivers + + + + + + + + + + + diff --git a/soem_beckhoff_drivers/msg/AnalogMsg.msg b/soem_beckhoff_drivers/msg/AnalogMsg.msg new file mode 100644 index 0000000..77f4d93 --- /dev/null +++ b/soem_beckhoff_drivers/msg/AnalogMsg.msg @@ -0,0 +1 @@ +float32[] values diff --git a/soem_beckhoff_drivers/msg/DigitalMsg.msg b/soem_beckhoff_drivers/msg/DigitalMsg.msg new file mode 100644 index 0000000..dca6d81 --- /dev/null +++ b/soem_beckhoff_drivers/msg/DigitalMsg.msg @@ -0,0 +1 @@ +bool[] values diff --git a/soem_beckhoff_drivers/msg/EncoderInMsg.msg b/soem_beckhoff_drivers/msg/EncoderInMsg.msg new file mode 100644 index 0000000..b454dde --- /dev/null +++ b/soem_beckhoff_drivers/msg/EncoderInMsg.msg @@ -0,0 +1,6 @@ +uint8 status +uint16 value +uint16 latch +uint32 frequency +uint16 period +uint16 window diff --git a/soem_beckhoff_drivers/msg/EncoderMsg.msg b/soem_beckhoff_drivers/msg/EncoderMsg.msg new file mode 100644 index 0000000..6f98048 --- /dev/null +++ b/soem_beckhoff_drivers/msg/EncoderMsg.msg @@ -0,0 +1 @@ +uint16 value diff --git a/soem_beckhoff_drivers/msg/EncoderOutMsg.msg b/soem_beckhoff_drivers/msg/EncoderOutMsg.msg new file mode 100644 index 0000000..18c207b --- /dev/null +++ b/soem_beckhoff_drivers/msg/EncoderOutMsg.msg @@ -0,0 +1,2 @@ +uint8 control +uint16 outvalue diff --git a/soem_beckhoff_drivers/msg/PSUMsg.msg b/soem_beckhoff_drivers/msg/PSUMsg.msg new file mode 100644 index 0000000..8359210 --- /dev/null +++ b/soem_beckhoff_drivers/msg/PSUMsg.msg @@ -0,0 +1,2 @@ +bool power_ok +bool overload \ No newline at end of file diff --git a/soem_beckhoff_drivers/src/COE_config.h b/soem_beckhoff_drivers/src/COE_config.h new file mode 100644 index 0000000..36479e4 --- /dev/null +++ b/soem_beckhoff_drivers/src/COE_config.h @@ -0,0 +1,26 @@ +#ifndef COE_CONFIG_H +#define COE_CONFIG_H + +extern "C"{ +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatconfig.h" +#include "ethercatcoe.h" +#include "ethercatdc.h" +#include "ethercatprint.h" +} + + + typedef struct{ + uint16 index; + uint8 subindex; + uint8 size; + int param; + string name; + string description; + }parameter; + + +#endif diff --git a/soem_beckhoff_drivers/src/soem_beckhoff_drivers.cpp b/soem_beckhoff_drivers/src/soem_beckhoff_drivers.cpp new file mode 100644 index 0000000..a1c4ad1 --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_beckhoff_drivers.cpp @@ -0,0 +1,7 @@ +#include + +extern "C" { +bool loadRTTPlugin(RTT::TaskContext* c){ + return true; +} +} diff --git a/soem_beckhoff_drivers/src/soem_el1xxx.cpp b/soem_beckhoff_drivers/src/soem_el1xxx.cpp new file mode 100644 index 0000000..3c13da3 --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el1xxx.cpp @@ -0,0 +1,80 @@ +/*! +* @author: Ruben Smits and Koen Buys +* @description: Driver for Beckhoff EL1124 module, 4x5V digital inputs +* @version: 9 April 2010 - 1.0 +* @license: LGPL (see Berlios - SOEM) +*/ +#include "soem_el1xxx.h" +#include + +namespace soem_beckhoff_drivers{ + + SoemEL1xxx::SoemEL1xxx(ec_slavet* mem_loc): + soem_master::SoemDriver(mem_loc), + port_(this->getName()+"_bits") + { + size_=mem_loc->Ibits; + service_->doc(std::string("Services for Beckhoff ")+std::string(datap_->name)+std::string(" Dig. Input module")); + service_->addOperation("isOn",&SoemEL1xxx::isOn,this).doc("Check if bit i is on").arg("i","bit nr"); + service_->addOperation("isOff",&SoemEL1xxx::isOff,this).doc("Check if bit i is off").arg("i","bit nr"); + service_->addOperation("readBit",&SoemEL1xxx::readBit,this).doc("Read value of bit i").arg("i","bit nr"); + service_->addConstant("size",size_); + msg_.values.resize(size_); + //port_.doc("values of the bits"); + port_.setDataSample(msg_); + + // port_.createPortObject("bits"); + } + + void SoemEL1xxx::addPortsToTaskContext(RTT::TaskContext* tc){ + tc->ports()->addPort(port_); + } + + bool SoemEL1xxx::isOn( unsigned int bit) const{ + return readBit(bit); + } + + bool SoemEL1xxx::isOff( unsigned int bit) const{ + return !readBit(bit); + } + + bool SoemEL1xxx::readBit( unsigned int bit) const{ + if(bitinputs))->outbits; + return bits_[bit+datap_->Istartbit]; + } + else{ + //TODO: We should raise somekind of error here + return false; + } + } + + void SoemEL1xxx::updatePorts(){ + bits_= ((out_el1xxxt*)(datap_->inputs))->outbits; + for(unsigned int i=0;iinputs))->outbits; + std::bitset<8> out_bits; + unsigned int j=0; + for(unsigned int i=start_bit;i<=stop_bit;i++) + out_bits.set(j,bits_[datap_->Istartbit+i]); + return out_bits.to_ulong(); + } + } + */ + + namespace { + soem_master::SoemDriver* createSoemEL1xxx(ec_slavet* mem_loc){ + return new SoemEL1xxx(mem_loc); + } + const bool registered1 = soem_master::SoemDriverFactory::Instance().registerDriver("EL1124",createSoemEL1xxx); + const bool registered2 = soem_master::SoemDriverFactory::Instance().registerDriver("EL1144",createSoemEL1xxx); + const bool registered3 = soem_master::SoemDriverFactory::Instance().registerDriver("EL1008",createSoemEL1xxx); + } +} diff --git a/soem_beckhoff_drivers/src/soem_el1xxx.h b/soem_beckhoff_drivers/src/soem_el1xxx.h new file mode 100644 index 0000000..d603c66 --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el1xxx.h @@ -0,0 +1,39 @@ +#ifndef SOEM_EL1xxx_H +#define SOEM_EL1xxx_H + +#include +#include +#include +#include + +namespace soem_beckhoff_drivers{ + + class SoemEL1xxx : public soem_master::SoemDriver + { + + typedef struct PACKED + { + uint8 outbits; + } out_el1xxxt; + + public: + SoemEL1xxx(ec_slavet* mem_loc); + ~SoemEL1xxx(){}; + + bool isOn( unsigned int bit = 0) const; + bool isOff( unsigned int bit = 0) const; + bool readBit( unsigned int bit = 0) const; + + void addPortsToTaskContext(RTT::TaskContext* tc); + void updatePorts(); + + + private: + unsigned int size_; + DigitalMsg msg_; + mutable std::bitset<8> bits_; + RTT::OutputPort port_; +}; + +} +#endif diff --git a/soem_beckhoff_drivers/src/soem_el2xxx.cpp b/soem_beckhoff_drivers/src/soem_el2xxx.cpp new file mode 100644 index 0000000..8bf16a8 --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el2xxx.cpp @@ -0,0 +1,99 @@ +#include "soem_el2xxx.h" +#include + +namespace soem_beckhoff_drivers{ + + SoemEL2xxx::SoemEL2xxx(ec_slavet* mem_loc): + soem_master::SoemDriver(mem_loc), + port_(this->getName()+"_bits") + { + size_=mem_loc->Obits; + service_->doc(std::string("Services for Beckhoff ")+std::string(datap_->name)+std::string(" Dig. Output module")); + + service_->addOperation("switchOn",&SoemEL2xxx::switchOn,this).doc("Switch bit i on").arg("i","bit nr"); + service_->addOperation("switchOff",&SoemEL2xxx::switchOff,this).doc("Switch bit i off").arg("i","bit nr"); + service_->addOperation("setBit",&SoemEL2xxx::setBit,this).doc("Set value of bit i to val").arg("i","bit nr").arg("val","new value for bit"); + service_->addOperation("checkBit",&SoemEL2xxx::checkBit,this).doc("Check value of bit i").arg("i","bit nr"); + service_->addConstant("size",size_); + + msg_.values.resize(size_); + + for(unsigned int i=0;iOstartbit,0); + ((out_el2xxxt*)(datap_->outputs))->outbits=bits_.to_ulong(); + } + + void SoemEL2xxx::addPortsToTaskContext(RTT::TaskContext* tc){ + tc->ports()->addPort(port_).doc("DigitalMsg containting the desired values of _all_ bits"); + } + + void SoemEL2xxx::updatePorts(){ + if(port_.connected()){ + if(port_.read(msg_)==RTT::NewData){ + if(msg_.values.size()outputs))->outbits; + for(unsigned int i=0;iOstartbit,msg_.values[i]); + ((out_el2xxxt*)(datap_->outputs))->outbits=bits_.to_ulong(); + } + } + } + } + + void SoemEL2xxx::setBit(unsigned int bit,bool value){ + if(bitoutputs))->outbits; + bits_.set(bit+datap_->Ostartbit,value); + ((out_el2xxxt*)(datap_->outputs))->outbits=bits_.to_ulong(); + } + } + + void SoemEL2xxx::switchOn(unsigned int n){ + this->setBit(n,true); + } + + void SoemEL2xxx::switchOff(unsigned int n){ + this->setBit(n,false); + } + /* + void SoemEL2xxx::setSequence(unsigned int start_bit,unsigned int stop_bit,unsigned int value){ + if(start_bitoutputs))->outbits; + std::bitset<8> in_bits(value); + for(unsigned int i=start_bit;i<=stop_bit;i++) + bits_.set(i+datap_->Ostartbit,in_bits[i]); + ((out_el2xxxt*)(datap_->outputs))->outbits=bits_.to_ulong(); + } + } + */ + + bool SoemEL2xxx::checkBit(unsigned int bit)const{ + if(bitoutputs))->outbits; + return bits_.test(bit+datap_->Ostartbit); + } + return false; + } + /* + unsigned int SoemEL2xxx::checkSequence(unsigned int start_bit,unsigned int stop_bit)const{ + if(start_bitoutputs))->outbits; + std::bitset<8> out_bits; + unsigned int j=0; + for(unsigned int i=start_bit;i<=stop_bit;i++) + out_bits.set(j,bits_[datap_->Ostartbit+i]); + return out_bits.to_ulong(); + } + } + */ + + namespace { + soem_master::SoemDriver* createSoemEL2xxx(ec_slavet* mem_loc){ + return new SoemEL2xxx(mem_loc); + } + const bool registered0 = soem_master::SoemDriverFactory::Instance().registerDriver("EL2002",createSoemEL2xxx); + const bool registered1 = soem_master::SoemDriverFactory::Instance().registerDriver("EL2004",createSoemEL2xxx); + const bool registered2 = soem_master::SoemDriverFactory::Instance().registerDriver("EL2008",createSoemEL2xxx); + const bool registered3 = soem_master::SoemDriverFactory::Instance().registerDriver("EL2124",createSoemEL2xxx); + } +} diff --git a/soem_beckhoff_drivers/src/soem_el2xxx.h b/soem_beckhoff_drivers/src/soem_el2xxx.h new file mode 100644 index 0000000..81d958c --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el2xxx.h @@ -0,0 +1,39 @@ +#ifndef SOEM_EL2XXX_H +#define SOEM_EL2XXX_H + +#include +#include +#include +#include + +namespace soem_beckhoff_drivers{ + + class SoemEL2xxx : public soem_master::SoemDriver + { + + typedef struct PACKED + { + uint8 outbits; + } out_el2xxxt; + + public: + SoemEL2xxx(ec_slavet* mem_loc); + ~SoemEL2xxx(){}; + + void switchOn( unsigned int n ); + void switchOff( unsigned int n ); + void setBit( unsigned int bit, bool value ); + bool checkBit(unsigned int n) const; + + void addPortsToTaskContext(RTT::TaskContext* tc); + void updatePorts(); + + private: + unsigned int size_; + DigitalMsg msg_; + mutable std::bitset<8> bits_; + RTT::InputPort port_; + }; + +} +#endif diff --git a/soem_beckhoff_drivers/src/soem_el3062.cpp b/soem_beckhoff_drivers/src/soem_el3062.cpp new file mode 100644 index 0000000..6abb60e --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el3062.cpp @@ -0,0 +1,222 @@ +#include "soem_el3062.h" +#include +#include +#include + + + +namespace soem_beckhoff_drivers{ + + SoemEL3062::SoemEL3062(ec_slavet* mem_loc): + soem_master::SoemDriver(mem_loc), + size_(2), + raw_range_(32768), + lowest_(0.0), + highest_(10.0), + values_(size_), + raw_values_(size_), + values_port_(this->getName()+"_values"), + raw_values_port_(this->getName()+"_raw_values") + + { + service_->doc(std::string("Services for Beckhoff ")+std::string(datap_->name)+std::string(" module")); + service_->addOperation("rawRead",&SoemEL3062::rawRead,this).doc("Read raw value of channel i").arg("i","channel nr"); + service_->addOperation("read",&SoemEL3062::read,this).doc("Read value to channel i").arg("i","channel nr"); + service_->addOperation("Over_Range",&SoemEL3062::isOverrange,this).doc("For the chanel i : 1 = overrange ; 0 = no overrange ").arg("i","channel nr"); + service_->addOperation("Under_Range",&SoemEL3062::isUnderrange,this).doc("For the chanel i : 1 = Underrange ; 0 = no Underrange ").arg("i","channel nr"); + service_->addOperation("Comp_val_to_lim",&SoemEL3062::CompareV_to_Lim,this).doc("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("i","channel nr").arg("x","Limit nr"); + service_->addOperation("Error",&SoemEL3062::is_error,this).doc("For the chanel i : 1 = error (Overrange or Underrange ; 0 = no error ").arg("i","channel nr"); + service_->addOperation("TxPDO",&SoemEL3062::Tx_PDO,this).doc("For the chanel i :Valid of data of assigned TxPDO (0=valid, 1=invalid) ").arg("i","channel nr"); + service_->addOperation("Toggle",&SoemEL3062::Tx_PDO_Toggle,this).doc("The TxPDO Toggle is toggled by the slave, if the data of the corresponding PDO was updated").arg("i","channel nr"); + + resolution_=((highest_-lowest_)/(double)raw_range_); + + service_->addConstant("raw_range",raw_range_); + service_->addConstant("resolution",resolution_); + service_->addConstant("lowest",lowest_); + service_->addConstant("highest",highest_); + + + msg_.values.resize(size_); + raw_msg_.values.resize(size_); + + } + + + void SoemEL3062::addPortsToTaskContext(RTT::TaskContext* tc){ + + tc->ports()->addPort(values_port_).doc("AnalogMsg contain the read values of _all_ channels"); + tc->ports()->addPort(raw_values_port_).doc("AnalogMsg containing the read values of _all_ channels"); + + // New properties : Component Configuration : Can be completed + // Need to be complete for a specific usage : See Beckof : http://www.beckhoff.com/EL3062/ + //see documentation (.xchm) / Commissionning / Object description and parameterization + + parameter temp; + + /// For parametrisation : uncomment the following code and complete it with using the document above + /* temp.description="Enable user scale"; + temp.index=0x8010; + temp.subindex=0x01; + temp.name="E_user_scl"; + temp.size=1; + temp.param=1; + params.push_back(temp); + + temp.description="Limite1ch1"; + temp.index=0x8010; + temp.subindex=0x13; + temp.name="Limite1ch1"; + temp.size=2; + temp.param=9174; + params.push_back(temp); + + temp.description="Limite2ch1"; + temp.index=0x8010; + temp.subindex=0x14; + temp.name="Limite2ch1"; + temp.size=2; + temp.param=24247; + params.push_back(temp);*/ + + + // Adding proprietes to the Soem_master_Component. + for(unsigned int i=0;iaddProperty(this->getName()+params[i].name,params[i].param).doc(params[i].description); + } + + + // Configuration of the slave + int val; + + for(unsigned int i=0;iproperties()->getProperty(this->getName()+params[i].name); + val=temp_val.get(); + + //assigning parameters + + ec_SDOwrite (((datap_->configadr)&0x0F),params[i].index,params[i].subindex,FALSE,params[i].size,&val,EC_TIMEOUTRXM); + } + + } + + + void SoemEL3062::updatePorts(){ + if(raw_values_port_.connected()){ + + raw_msg_.values[0]=((out_el3062t*)(datap_->inputs))->val_ch1; + raw_msg_.values[1]=((out_el3062t*)(datap_->inputs))->val_ch2; + } + + if(values_port_.connected()){ + + msg_.values[0]=(((out_el3062t*)(datap_->inputs))->val_ch1*resolution_); + msg_.values[1]=(((out_el3062t*)(datap_->inputs))->val_ch2*resolution_); + } + } + + + + //rawRead : read the raw value of the input ///////////////////////////////////////////////////// + int SoemEL3062::rawRead( unsigned int chan ){ + if(chaninputs))->val_ch1; + if (chan==1) return ((out_el3062t*)(datap_->inputs))->val_ch2; + } + return -1; + } + + int SoemEL3062::read_param( unsigned int chan ){ + if(chaninputs))->param_ch1; + if (chan==1) return ((out_el3062t*)(datap_->inputs))->param_ch2; + } + return -1; + } + + + //read: read the value of one of the 2 channels in Volts /////////////////////////// + double SoemEL3062::read( unsigned int chan){ + + if(chaninputs))->val_ch1*resolution_); + if (chan==1) return (((out_el3062t*)(datap_->inputs))->val_ch2*resolution_); + } + return -1; + + } + + + //Checking overrange//////////////////////////////////////////////////////////////// + + bool SoemEL3062::isOverrange( unsigned int chan) + { + ch_par=read_param(chan); + return ch_par[1]; + } + + //Checking Underrange//////////////////////////////////////////////////////////////// + + bool SoemEL3062::isUnderrange( unsigned int chan) + { + ch_par=read_param(chan); + return ch_par[0]; + } + + // Comparing the Value of Channel chan with its own limits (1/2) + int SoemEL3062::CompareV_to_Lim( unsigned int chan, unsigned int Lim_num){ + + if ((Lim_num<3) & (chan>2; + break; + case 2 : + return ((read_param(chan))&&(0x30))>>4; + break; + default : return-1; + break; + } + } + return -1; + } + + // Checking for errors + bool SoemEL3062:: is_error( unsigned int chan ){ + ch_par=read_param(chan); + return ch_par[6]; + } + + //TxPDO + bool SoemEL3062:: Tx_PDO( unsigned int chan ){ + ch_par=read_param(chan); + return ch_par[14]; + } + + // TxPDO Toggle + bool SoemEL3062:: Tx_PDO_Toggle( unsigned int chan ){ + ch_par=read_param(chan); + return ch_par[15]; + } + + + + namespace { + soem_master::SoemDriver* createSoemEL3062(ec_slavet* mem_loc){ + return new SoemEL3062(mem_loc); + } + const bool registered0 = soem_master::SoemDriverFactory::Instance().registerDriver("EL3062",createSoemEL3062); + + } + + +}//namespace + + + diff --git a/soem_beckhoff_drivers/src/soem_el3062.h b/soem_beckhoff_drivers/src/soem_el3062.h new file mode 100644 index 0000000..ea2a852 --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el3062.h @@ -0,0 +1,72 @@ +#ifndef SOEM_EL3062_H +#define SOEM_EL3062_H + +#include +#include +#include +#include +#include +#include +#include "COE_config.h" + + +namespace soem_beckhoff_drivers{ + + class SoemEL3062 : public soem_master::SoemDriver + { + + typedef struct PACKED + { + int16 param_ch1; + int16 val_ch1; + + int16 param_ch2; + int16 val_ch2; + } out_el3062t; + + + + public: + SoemEL3062(ec_slavet* mem_loc); + ~SoemEL3062(){}; + + int rawRead( unsigned int chan ); + double read( unsigned int chan); + bool isOverrange( unsigned int chan = 0) ; + bool isUnderrange( unsigned int chan = 0); + int CompareV_to_Lim( unsigned int chan=0 , unsigned int Lim_num=0); + int read_param( unsigned int chan ); + bool is_error( unsigned int chan ); + bool Tx_PDO( unsigned int chan); + bool Tx_PDO_Toggle( unsigned int chan); + + void addPortsToTaskContext(RTT::TaskContext* tc); + void updatePorts(); + + private: + + + // Property ////// + RTT::Property temp_val; + + const unsigned int size_; + const unsigned int raw_range_; + const double lowest_; + const double highest_; + double resolution_; + mutable std::bitset<16> ch_par; + AnalogMsg msg_; + AnalogMsg raw_msg_; + std::vector values_; + std::vector raw_values_; + + //Ports/////////// + RTT::OutputPort values_port_; + + RTT::OutputPort raw_values_port_; + + std::vector params; + }; + +} +#endif diff --git a/soem_beckhoff_drivers/src/soem_el4004.cpp b/soem_beckhoff_drivers/src/soem_el4004.cpp new file mode 100644 index 0000000..7f7a5dd --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el4004.cpp @@ -0,0 +1,96 @@ +#include "soem_el4004.h" +#include + +namespace soem_beckhoff_drivers{ + + SoemEL4004::SoemEL4004(ec_slavet* mem_loc): + soem_master::SoemDriver(mem_loc), + size_(4), + raw_range_(32768), + lowest_(0.0), + highest_(10.0), + values_(size_), + raw_values_(size_), + values_port_(this->getName()+"_values"), + raw_values_port_(this->getName()+"_raw_values") + { + + service_->doc(std::string("Services for Beckhoff ")+std::string(datap_->name)+std::string(" module")); + service_->addOperation("rawWrite",&SoemEL4004::rawWrite,this).doc("Write raw value to channel i").arg("i","channel nr").arg("value","raw value"); + service_->addOperation("rawRead",&SoemEL4004::rawRead,this).doc("Read raw value of channel i").arg("i","channel nr"); + service_->addOperation("write",&SoemEL4004::write,this).doc("Write value to channel i").arg("i","channel nr").arg("value","value"); + service_->addOperation("read",&SoemEL4004::read,this).doc("Read value to channel i").arg("i","channel nr"); + + resolution_=((highest_-lowest_)/(double)raw_range_); + + service_->addConstant("raw_range",raw_range_); + service_->addConstant("resolution",resolution_); + service_->addConstant("lowest",lowest_); + service_->addConstant("highest",highest_); + + + msg_.values.resize(size_); + raw_msg_.values.resize(size_); + + } + + void SoemEL4004::addPortsToTaskContext(RTT::TaskContext* tc){ + tc->ports()->addPort(values_port_).doc("AnalogMsg containing the desired values of _all_ channels"); + tc->ports()->addPort(raw_values_port_).doc("AnalogMsg containing the desired values of _all_ channels"); + } + + void SoemEL4004::updatePorts(){ + if(raw_values_port_.connected()){ + if(raw_values_port_.read(raw_msg_)==RTT::NewData) + if(raw_msg_.values.size()outputs))->values[i]=raw_msg_.values[i]; + } + if(values_port_.connected()){ + if(values_port_.read(msg_)==RTT::NewData) + if(msg_.values.size()outputs))->values[i]=msg_.values[i]/resolution_; + } + } + + bool SoemEL4004::rawWrite( unsigned int chan, int value ){ + if(chanoutputs))->values[chan]=value; + return true; + } + return false; + } + int SoemEL4004::rawRead( unsigned int chan ){ + if(chanoutputs))->values[chan]; + return -1; + } + + bool SoemEL4004::write( unsigned int chan, double value ){ + if(chanoutputs))->values[chan]=(int)(value/resolution_); + return true; + } + return false; + } + + double SoemEL4004::read( unsigned int chan){ + if(chanoutputs))->values[chan]*resolution_; + return -1; + } + + namespace { + soem_master::SoemDriver* createSoemEL4004(ec_slavet* mem_loc){ + return new SoemEL4004(mem_loc); + } + const bool registered0 = soem_master::SoemDriverFactory::Instance().registerDriver("EL4004",createSoemEL4004); + + } + + +}//namespace + + + diff --git a/soem_beckhoff_drivers/src/soem_el4004.h b/soem_beckhoff_drivers/src/soem_el4004.h new file mode 100644 index 0000000..8c833b7 --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el4004.h @@ -0,0 +1,49 @@ +#ifndef SOEM_EL4004_H +#define SOEM_EL4004_H + +#include +#include +#include +#include + +namespace soem_beckhoff_drivers{ + + class SoemEL4004 : public soem_master::SoemDriver + { + + typedef struct PACKED + { + uint16 values[4]; + } out_el4004t; + + public: + SoemEL4004(ec_slavet* mem_loc); + ~SoemEL4004(){}; + + bool rawWrite( unsigned int chan, int value ); + int rawRead( unsigned int chan ); + + bool write( unsigned int chan, double value ); + double read( unsigned int chan); + + + void addPortsToTaskContext(RTT::TaskContext* tc); + void updatePorts(); + + private: + const unsigned int size_; + const unsigned int raw_range_; + const double lowest_; + const double highest_; + double resolution_; + + AnalogMsg msg_; + AnalogMsg raw_msg_; + std::vector values_; + std::vector raw_values_; + RTT::InputPort values_port_; + RTT::InputPort raw_values_port_; + }; + +} +#endif diff --git a/soem_beckhoff_drivers/src/soem_el4032.cpp b/soem_beckhoff_drivers/src/soem_el4032.cpp new file mode 100644 index 0000000..7f9437f --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el4032.cpp @@ -0,0 +1,123 @@ +#include "soem_el4032.h" +#include + +namespace soem_beckhoff_drivers{ + + + SoemEL4032::SoemEL4032(ec_slavet* mem_loc): + soem_master::SoemDriver(mem_loc), + size_(2), + raw_range_(65536), //32768 + lowest_(-10.0), + highest_(10.0), + values_(size_), + raw_values_(size_), + values_port_(this->getName()+"_values"), + raw_values_port_(this->getName()+"_raw_values") + { + + service_->doc(std::string("Services for Beckhoff ")+std::string(datap_->name)+std::string(" module")); + service_->addOperation("rawWrite",&SoemEL4032::rawWrite,this).doc("Write raw value to channel i").arg("i","channel nr").arg("value","raw value"); + service_->addOperation("rawRead",&SoemEL4032::rawRead,this).doc("Read raw value of channel i").arg("i","channel nr"); + service_->addOperation("write",&SoemEL4032::write,this).doc("Write value to channel i").arg("i","channel nr").arg("value","value"); + service_->addOperation("read",&SoemEL4032::read,this).doc("Read value to channel i").arg("i","channel nr"); + + resolution_=((highest_-lowest_)/(double)raw_range_); + + service_->addConstant("raw_range",raw_range_); + service_->addConstant("resolution",resolution_); + service_->addConstant("lowest",lowest_); + service_->addConstant("highest",highest_); + + + msg_.values.resize(size_); + raw_msg_.values.resize(size_); + + for(unsigned int i=0;ioutputs))->values[i]=0; + } + + } + + void SoemEL4032::addPortsToTaskContext(RTT::TaskContext* tc){ + tc->ports()->addPort(values_port_).doc("AnalogMsg containing the desired values of _all_ channels"); + tc->ports()->addPort(raw_values_port_).doc("AnalogMsg containing the desired values of _all_ channels"); + } + + void SoemEL4032::updatePorts(){ + + if(raw_values_port_.connected()){ + if(raw_values_port_.read(raw_msg_)==RTT::NewData) + { + if(raw_msg_.values.size()<=size_) + { + for(unsigned int i=0;ioutputs))->values[i]=raw_msg_.values[i]; + } + } + } + } + if(values_port_.connected()){ + if(values_port_.read(msg_)==RTT::NewData) + { + if(msg_.values.size()<=size_) + { + for(unsigned int i=0;ioutputs))->values[i]=msg_.values[i]/resolution_; + } + } + } + } + } + + + bool SoemEL4032::rawWrite( unsigned int chan, unsigned int value ){ + if(chanoutputs))->values[chan]=value; + return true; + } + } + return false; + } + + int SoemEL4032::rawRead( unsigned int chan ){ + if(chanoutputs))->values[chan]; + return -1; + } + + bool SoemEL4032::write( unsigned int chan, double value ){ + if(chan= lowest_){ + ((out_el4032t*)(datap_->outputs))->values[chan]=(int)(value/resolution_); + return true; + } + } + return false; + } + + double SoemEL4032::read( unsigned int chan){ + if(chanoutputs))->values[chan]*resolution_; + return -1; + } + + namespace { + soem_master::SoemDriver* createSoemEL4032(ec_slavet* mem_loc){ + return new SoemEL4032(mem_loc); + } + + const bool registered0 = soem_master::SoemDriverFactory::Instance().registerDriver("EL4032",createSoemEL4032); + + } + + +}//namespace + + + diff --git a/soem_beckhoff_drivers/src/soem_el4032.h b/soem_beckhoff_drivers/src/soem_el4032.h new file mode 100644 index 0000000..99ea00b --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el4032.h @@ -0,0 +1,49 @@ +#ifndef SOEM_EL4032_H +#define SOEM_EL4032_H + +#include +#include +#include +#include + +namespace soem_beckhoff_drivers{ + + class SoemEL4032 : public soem_master::SoemDriver + { + + typedef struct PACKED + { + uint16 values[2]; + } out_el4032t; + + public: + SoemEL4032(ec_slavet* mem_loc); + ~SoemEL4032(){}; + + bool rawWrite( unsigned int chan, unsigned int value ); + int rawRead( unsigned int chan ); + + bool write( unsigned int chan, double value ); + double read( unsigned int chan); + + + void addPortsToTaskContext(RTT::TaskContext* tc); + void updatePorts(); + + private: + const unsigned int size_; + const unsigned int raw_range_; + const double lowest_; + const double highest_; + double resolution_; + + AnalogMsg msg_; + AnalogMsg raw_msg_; + std::vector values_; + std::vector raw_values_; + RTT::InputPort values_port_; + RTT::InputPort raw_values_port_; + }; + +} +#endif diff --git a/soem_beckhoff_drivers/src/soem_el4034.cpp b/soem_beckhoff_drivers/src/soem_el4034.cpp new file mode 100644 index 0000000..526f9c1 --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el4034.cpp @@ -0,0 +1,106 @@ +#include "soem_el4034.h" +#include + +namespace soem_beckhoff_drivers{ + SoemEL4034::SoemEL4034(ec_slavet* mem_loc): + soem_master::SoemDriver(mem_loc), + size_(4), + raw_range_(65536), //32768 + lowest_(-10.0), + highest_(10.0), + values_(size_), + raw_values_(size_), + values_port_(this->getName()+"_values"), + raw_values_port_(this->getName()+"_raw_values") + { + + service_->doc(std::string("Services for Beckhoff ")+std::string(datap_->name)+std::string(" module")); + service_->addOperation("rawWrite",&SoemEL4034::rawWrite,this).doc("Write raw value to channel i").arg("i","channel nr").arg("value","raw value"); + service_->addOperation("rawRead",&SoemEL4034::rawRead,this).doc("Read raw value of channel i").arg("i","channel nr"); + service_->addOperation("write",&SoemEL4034::write,this).doc("Write value to channel i").arg("i","channel nr").arg("value","value"); + service_->addOperation("read",&SoemEL4034::read,this).doc("Read value to channel i").arg("i","channel nr"); + + resolution_=((highest_-lowest_)/(double)raw_range_); + + service_->addConstant("raw_range",raw_range_); + service_->addConstant("resolution",resolution_); + service_->addConstant("lowest",lowest_); + service_->addConstant("highest",highest_); + + + msg_.values.resize(size_); + raw_msg_.values.resize(size_); + + for(unsigned int i=0;ioutputs))->values[i]=0; + } + + } + + void SoemEL4034::addPortsToTaskContext(RTT::TaskContext* tc){ + tc->ports()->addPort(values_port_).doc("AnalogMsg containing the desired values of _all_ channels"); + tc->ports()->addPort(raw_values_port_).doc("AnalogMsg containing the desired values of _all_ channels"); + } + + void SoemEL4034::updatePorts(){ + if(raw_values_port_.connected()){ + if(raw_values_port_.read(raw_msg_)==RTT::NewData) + if(raw_msg_.values.size()<=size_) + for(unsigned int i=0;ioutputs))->values[i]=raw_msg_.values[i]; + } + if(values_port_.connected()){ + if(values_port_.read(msg_)==RTT::NewData) + if(msg_.values.size()<=size_) + for(unsigned int i=0;ioutputs))->values[i]=msg_.values[i]/resolution_; + } + } + + + bool SoemEL4034::rawWrite( unsigned int chan, unsigned int value ){ + if(chanoutputs))->values[chan]=value; + return true; + } + } + return false; + } + + int SoemEL4034::rawRead( unsigned int chan ){ + if(chanoutputs))->values[chan]; + return -1; + } + + bool SoemEL4034::write( unsigned int chan, double value ){ + if(chan= lowest_){ + ((out_el4034t*)(datap_->outputs))->values[chan]=(int)(value/resolution_); + return true; + } + } + return false; + } + + double SoemEL4034::read( unsigned int chan){ + if(chanoutputs))->values[chan]*resolution_; + return -1; + } + + namespace { + soem_master::SoemDriver* createSoemEL4034(ec_slavet* mem_loc){ + return new SoemEL4034(mem_loc); + } + const bool registered0 = soem_master::SoemDriverFactory::Instance().registerDriver("EL4034",createSoemEL4034); + } + + +}//namespace + + + diff --git a/soem_beckhoff_drivers/src/soem_el4034.h b/soem_beckhoff_drivers/src/soem_el4034.h new file mode 100644 index 0000000..2c89ba2 --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el4034.h @@ -0,0 +1,49 @@ +#ifndef SOEM_EL4034_H +#define SOEM_EL4034_H + +#include +#include +#include +#include + +namespace soem_beckhoff_drivers{ + + class SoemEL4034 : public soem_master::SoemDriver + { + + typedef struct PACKED + { + uint16 values[4]; + } out_el4034t; + + public: + SoemEL4034(ec_slavet* mem_loc); + ~SoemEL4034(){}; + + bool rawWrite( unsigned int chan, unsigned int value ); + int rawRead( unsigned int chan ); + + bool write( unsigned int chan, double value ); + double read( unsigned int chan); + + + void addPortsToTaskContext(RTT::TaskContext* tc); + void updatePorts(); + + private: + const unsigned int size_; + const unsigned int raw_range_; + const double lowest_; + const double highest_; + double resolution_; + + AnalogMsg msg_; + AnalogMsg raw_msg_; + std::vector values_; + std::vector raw_values_; + RTT::InputPort values_port_; + RTT::InputPort raw_values_port_; + }; + +} +#endif diff --git a/soem_beckhoff_drivers/src/soem_el4038.cpp b/soem_beckhoff_drivers/src/soem_el4038.cpp new file mode 100644 index 0000000..9ebb514 --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el4038.cpp @@ -0,0 +1,106 @@ +#include "soem_el4038.h" +#include + +namespace soem_beckhoff_drivers{ + SoemEL4038::SoemEL4038(ec_slavet* mem_loc): + soem_master::SoemDriver(mem_loc), + size_(8), + raw_range_(65536), //32768 + lowest_(-10.0), + highest_(10.0), + values_(size_), + raw_values_(size_), + values_port_(this->getName()+"_values"), + raw_values_port_(this->getName()+"_raw_values") + { + + service_->doc(std::string("Services for Beckhoff ")+std::string(datap_->name)+std::string(" Analog output module")); + service_->addOperation("rawWrite",&SoemEL4038::rawWrite,this).doc("Write raw value to channel i").arg("i","channel nr").arg("value","raw value"); + service_->addOperation("rawRead",&SoemEL4038::rawRead,this).doc("Read raw value of channel i").arg("i","channel nr"); + service_->addOperation("write",&SoemEL4038::write,this).doc("Write value to channel i").arg("i","channel nr").arg("value","value"); + service_->addOperation("read",&SoemEL4038::read,this).doc("Read value to channel i").arg("i","channel nr"); + + resolution_=((highest_-lowest_)/(double)raw_range_); + + service_->addConstant("raw_range",raw_range_); + service_->addConstant("resolution",resolution_); + service_->addConstant("lowest",lowest_); + service_->addConstant("highest",highest_); + + + msg_.values.resize(size_); + raw_msg_.values.resize(size_); + + for(unsigned int i=0;ioutputs))->values[i]=0; + } + + } + + void SoemEL4038::addPortsToTaskContext(RTT::TaskContext* tc){ + tc->ports()->addPort(values_port_).doc("AnalogMsg containing the desired values of _all_ channels"); + tc->ports()->addPort(raw_values_port_).doc("AnalogMsg containing the desired values of _all_ channels"); + } + + void SoemEL4038::updatePorts(){ + if(raw_values_port_.connected()){ + if(raw_values_port_.read(raw_msg_)==RTT::NewData) + if(raw_msg_.values.size()<=size_) + for(unsigned int i=0;ioutputs))->values[i]=raw_msg_.values[i]; + } + if(values_port_.connected()){ + if(values_port_.read(msg_)==RTT::NewData) + if(msg_.values.size()<=size_) + for(unsigned int i=0;ioutputs))->values[i]=msg_.values[i]/resolution_; + } + } + + + bool SoemEL4038::rawWrite( unsigned int chan, unsigned int value ){ + if(chanoutputs))->values[chan]=value; + return true; + } + } + return false; + } + + int SoemEL4038::rawRead( unsigned int chan ){ + if(chanoutputs))->values[chan]; + return -1; + } + + bool SoemEL4038::write( unsigned int chan, double value ){ + if(chan= lowest_){ + ((out_el4038t*)(datap_->outputs))->values[chan]=(int)(value/resolution_); + return true; + } + } + return false; + } + + double SoemEL4038::read( unsigned int chan){ + if(chanoutputs))->values[chan]*resolution_; + return -1; + } + + namespace { + soem_master::SoemDriver* createSoemEL4038(ec_slavet* mem_loc){ + return new SoemEL4038(mem_loc); + } + const bool registered0 = soem_master::SoemDriverFactory::Instance().registerDriver("EL4038",createSoemEL4038); + } + + +}//namespace + + + diff --git a/soem_beckhoff_drivers/src/soem_el4038.h b/soem_beckhoff_drivers/src/soem_el4038.h new file mode 100644 index 0000000..f6417ad --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el4038.h @@ -0,0 +1,49 @@ +#ifndef SOEM_EL4038_H +#define SOEM_EL4034_H + +#include +#include +#include +#include + +namespace soem_beckhoff_drivers{ + + class SoemEL4038 : public soem_master::SoemDriver + { + + typedef struct PACKED + { + uint16 values[8]; + } out_el4038t; + + public: + SoemEL4038(ec_slavet* mem_loc); + ~SoemEL4038(){}; + + bool rawWrite( unsigned int chan, unsigned int value ); + int rawRead( unsigned int chan ); + + bool write( unsigned int chan, double value ); + double read( unsigned int chan); + + + void addPortsToTaskContext(RTT::TaskContext* tc); + void updatePorts(); + + private: + const unsigned int size_; + const unsigned int raw_range_; + const double lowest_; + const double highest_; + double resolution_; + + AnalogMsg msg_; + AnalogMsg raw_msg_; + std::vector values_; + std::vector raw_values_; + RTT::InputPort values_port_; + RTT::InputPort raw_values_port_; + }; + +} +#endif diff --git a/soem_beckhoff_drivers/src/soem_el5101.cpp b/soem_beckhoff_drivers/src/soem_el5101.cpp new file mode 100644 index 0000000..a38490d --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el5101.cpp @@ -0,0 +1,104 @@ +#include "soem_el5101.h" +#include + +namespace soem_beckhoff_drivers{ + + SoemEL5101::SoemEL5101(ec_slavet* mem_loc): + soem_master::SoemDriver(mem_loc), + values_port_(this->getName()+"_value") + { + service_->doc(std::string("Services for Beckhoff ")+std::string(datap_->name)+std::string(" Encoder module")); + service_->addOperation("read",&SoemEL5101::read,this).doc("Read in value of the encoder"); + //service_->addOperation("read_out",&SoemEL5101::read_out,this).doc("Read out value of the encoder"); + //service_->addOperation("write_out",&SoemEL5101::write_out,this).doc("Write the initial value").arg("value","Value"); + //service_->addOperation("control",&SoemEL5101::control,this).doc("Read control of the encoder"); + //service_->addOperation("status",&SoemEL5101::status,this).doc("Read status of the encoder"); + } + + // void SoemEL5101::addConfigureToTaskContext(RTT::TaskContext* tc){ + //tc->ports()->addPort(values_port_).doc("Uint msg containing the values of the encoder"); + //tc->ports()->addPort(values_port_out_).doc("AnalogMsg containing the values of the encoder"); + // tc->addProperty("",); + //} + + void SoemEL5101::addPortsToTaskContext(RTT::TaskContext* tc){ + tc->ports()->addPort(values_port_).doc("Uint msg containing the value of the encoder"); + + parameter temp; + temp.description="Essai description"; + temp.index=1000; + temp.subindex=0; + temp.name=this->getName()+"essai"; + temp.size=1; + temp.param=10; + + params.push_back(temp); + + // propriete. + if(params.empty()) // Si le tableau est vide. + { + cout << "Le tableau est vide" << endl; + } + else + { + for(unsigned int i(0);iaddProperty(params[i].name,params[i].param).doc(params[i].description); + } + } + + + // std::cout << this.properties()->getProperty(this->getName()+"_testparam") < + //tc->addP + //RTT::Property param(this->getName()+"_TEST2","description de ma proprietee"); + //tc->addProperty(param); + //tc->addProperty(this->getName()+"_param",test.param).doc("TEST description de ma proprietee"); + + // tc->getProvider("marshalling")->writeProperties("essai.xml"); + // tc->addProperty(); + //std::cout << tc->properties()->getProperty(this->getName()+"_testparam")<properties()->getProperty(this->getName()+"_testparam2")<properties()->getProperty(this->getName()+"_testparam3")<inputs))->invalue; + } + + /*double SoemEL5101::read_out( void){ + return ((out_el5101t*)(datap_->outputs))->outvalue; + } + + int SoemEL5101::write_out( uint value){ + ((out_el5101t*)(datap_->outputs))->outvalue=value; + return 1; + //return ((out_el5101t*)(datap_->outputs))->outvalue; + } + + unsigned int SoemEL5101::status( void){ + return ((in_el5101t*)(datap_->inputs))->status; + } + + unsigned int SoemEL5101::control( void){ + return ((out_el5101t*)(datap_->outputs))->control; + } +*/ + namespace { + soem_master::SoemDriver* createSoemEL5101(ec_slavet* mem_loc){ + return new SoemEL5101(mem_loc); + } + const bool registered0 = soem_master::SoemDriverFactory::Instance().registerDriver("EL5101",createSoemEL5101); + + } + +}//namespace + + + diff --git a/soem_beckhoff_drivers/src/soem_el5101.h b/soem_beckhoff_drivers/src/soem_el5101.h new file mode 100644 index 0000000..8143042 --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el5101.h @@ -0,0 +1,66 @@ +#ifndef SOEM_EL5101_H +#define SOEM_EL5101_H + +#include +#include +#include +#include +#include +#include + + +namespace soem_beckhoff_drivers{ + + class SoemEL5101 : public soem_master::SoemDriver + { + + typedef struct PACKED + { + uint8 control; + uint16 outvalue; + } out_el5101t; + + typedef struct PACKED + { + uint8 status; + uint16 invalue; + uint16 latch; + uint32 frequency; + uint16 period; + uint16 window; + } in_el5101t; + + typedef struct{ + uint16 index; + uint8 subindex; + uint8 size; + int param; + string name; + string description; + } parameter; + + public: + SoemEL5101(ec_slavet* mem_loc); + ~SoemEL5101(){}; + + //bool write( unsigned int chan, double value ); + double read(void); + /*double read_out(void); + int write_out(uint); + unsigned int control(void); + unsigned int status(void);*/ + + void addPortsToTaskContext(RTT::TaskContext* tc); + void updatePorts(); + + private: + + EncoderMsg msg_; + std::vector values_in_; + RTT::OutputPort values_port_; + RTT::Property propriete; + std::vector params; + }; + +} +#endif diff --git a/soem_core/CMakeLists.txt b/soem_core/CMakeLists.txt new file mode 100644 index 0000000..3e07cce --- /dev/null +++ b/soem_core/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +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) +target_link_libraries(${PROJECT_NAME} pthread rt m c) +include_directories(${PROJECT_SOURCE_DIR}/build/soem_core/src) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) diff --git a/soem_core/Makefile b/soem_core/Makefile new file mode 100644 index 0000000..66bbb02 --- /dev/null +++ b/soem_core/Makefile @@ -0,0 +1,18 @@ +TARBALL = SOEM1.2.4.tar.bz2 +TARBALL_URL = http://download.berlios.de/soem/SOEM1.2.4.tar.bz2 +SOURCE_DIR = build/soem_core +INITIAL_DIR = build/SOEM1.2.4 +UNPACK_CMD = tar xjf +MD5SUM_FILE = SOEM1.2.4.tar.bz2.md5sum + +all: $(SOURCE_DIR)/unpacked + +include $(shell rospack find mk)/download_unpack_build.mk + +unpack: $(SOURCE_DIR) + +wipe: clean + -rm -rf $(SOURCE_DIR) + -rm -rf ${TARBALL} + +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/soem_core/SOEM1.2.4.tar.bz2.md5sum b/soem_core/SOEM1.2.4.tar.bz2.md5sum new file mode 100644 index 0000000..2ae439d --- /dev/null +++ b/soem_core/SOEM1.2.4.tar.bz2.md5sum @@ -0,0 +1 @@ +7a7ec04a17597cea50e302a411b7dc46 SOEM1.2.4.tar.bz2 diff --git a/soem_core/mainpage.dox b/soem_core/mainpage.dox new file mode 100644 index 0000000..b7f41e3 --- /dev/null +++ b/soem_core/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\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. + + + + +\section codeapi Code API + + + + +*/ diff --git a/soem_core/manifest.xml b/soem_core/manifest.xml new file mode 100644 index 0000000..d37de4d --- /dev/null +++ b/soem_core/manifest.xml @@ -0,0 +1,21 @@ + + + + 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. (http://developer.berlios.de/projects/soem/) + + This package contains the original soem c code provided by the Technische Universiteit Eindhoven. + + + Arthur Ketels and M.J.G. van den Molengraft; packaged by Ruben Smits + GPL+linking exception + + http://developer.berlios.de/projects/soem + + + + + + + diff --git a/soem_master/CMakeLists.txt b/soem_master/CMakeLists.txt new file mode 100644 index 0000000..1d7fec3 --- /dev/null +++ b/soem_master/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +rosbuild_add_library(${PROJECT_NAME} src/soem_driver_factory.cpp src/soem_master.cpp src/soem_master_component.cpp) +set_target_properties(${PROJECT_NAME} PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib/orocos) + +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) diff --git a/soem_master/Makefile b/soem_master/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/soem_master/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/soem_master/include/soem_master/soem_driver.h b/soem_master/include/soem_master/soem_driver.h new file mode 100644 index 0000000..af84c37 --- /dev/null +++ b/soem_master/include/soem_master/soem_driver.h @@ -0,0 +1,57 @@ +#ifndef SOEM_DRIVER_H +#define SOEM_DRIVER_H + +extern "C"{ +#include +#include +} + + +#include +#include + +#include + +template +inline std::string to_string (const T& t,std::ios_base & (*f)(std::ios_base&)){ + std::stringstream ss; + ss << f << t; + return ss.str(); +}; + +namespace soem_master{ + + + class SoemDriver + { + public: + ~SoemDriver(){ + service_->clear(); + }; + + const std::string& getName() const{ + return name_; + } + + RTT::Service::shared_ptr provides(){ + return RTT::Service::shared_ptr(service_); + }; + + virtual void addPortsToTaskContext(RTT::TaskContext* tc)=0; + + virtual void updatePorts()=0; + + protected: + SoemDriver(ec_slavet* mem_loc) : + datap_(mem_loc), + name_("Slave_"+to_string(datap_->configadr,std::hex)), + service_(new RTT::Service(name_)) + { + }; + ec_slavet* datap_; + std::string name_; + RTT::Service::shared_ptr service_; + + }; +} +#endif diff --git a/soem_master/include/soem_master/soem_driver_factory.h b/soem_master/include/soem_master/soem_driver_factory.h new file mode 100644 index 0000000..0821bf5 --- /dev/null +++ b/soem_master/include/soem_master/soem_driver_factory.h @@ -0,0 +1,33 @@ +#ifndef SOEM_DRIVER_FACTORY_H +#define SOEM_DRIVER_FACTORY_H + +#include +#include +#include "soem_driver.h" + +namespace soem_master{ + + class SoemDriverFactory{ + public: + static SoemDriverFactory& Instance(){ + static SoemDriverFactory soem_driver_factory; + return soem_driver_factory; + } + + typedef SoemDriver* (*CreateDriverCallBack)(ec_slavet*); + + bool registerDriver(std::string name,CreateDriverCallBack createFn); + + SoemDriver* createDriver(ec_slavet* mem_loc); + + private: + typedef std::map FactoryMap; + FactoryMap factory_map_; + + SoemDriverFactory(){}; + SoemDriverFactory(const SoemDriverFactory&); + SoemDriverFactory& operator=(const SoemDriverFactory&); + ~SoemDriverFactory(){}; + }; +} +#endif diff --git a/soem_master/mainpage.dox b/soem_master/mainpage.dox new file mode 100644 index 0000000..ac9f2c0 --- /dev/null +++ b/soem_master/mainpage.dox @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b soem_master is ... + + + + +\section codeapi Code API + + + + +*/ diff --git a/soem_master/manifest.xml b/soem_master/manifest.xml new file mode 100644 index 0000000..caae95d --- /dev/null +++ b/soem_master/manifest.xml @@ -0,0 +1,22 @@ + + + + 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. + + + Ruben Smits + BSD + + http://ros.org/wiki/soem_master + + + + + + + + + + + + diff --git a/soem_master/soem_master.xml.xacro b/soem_master/soem_master.xml.xacro new file mode 100644 index 0000000..7de3539 --- /dev/null +++ b/soem_master/soem_master.xml.xacro @@ -0,0 +1,18 @@ + + + +$(find soem_master)/lib/orocos +$(find soem_beckhoff_drivers)/lib/orocos + + + + 0.001 + 0 + ORO_SCHED_RT + + 0 + 0 + + + + diff --git a/soem_master/soem_master_test.launch b/soem_master/soem_master_test.launch new file mode 100644 index 0000000..705d400 --- /dev/null +++ b/soem_master/soem_master_test.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/soem_master/src/soem_driver_factory.cpp b/soem_master/src/soem_driver_factory.cpp new file mode 100644 index 0000000..75f1652 --- /dev/null +++ b/soem_master/src/soem_driver_factory.cpp @@ -0,0 +1,17 @@ +#include +#include +namespace soem_master{ + + bool SoemDriverFactory::registerDriver(std::string name, CreateDriverCallBack create_fn){ + return factory_map_.insert(FactoryMap::value_type(name,create_fn)).second; + } + + SoemDriver* SoemDriverFactory::createDriver(ec_slavet* mem_loc){ + FactoryMap::const_iterator it = factory_map_.find(std::string(mem_loc->name)); + if (it == factory_map_.end()){ + //not found + throw std::runtime_error("Unknown Driver Name"); + } + return (it->second)(mem_loc); + } +} diff --git a/soem_master/src/soem_master.cpp b/soem_master/src/soem_master.cpp new file mode 100644 index 0000000..7f91662 --- /dev/null +++ b/soem_master/src/soem_master.cpp @@ -0,0 +1,245 @@ +/* +* This is the Ethercat master class based on the SOEM package +* +* @author: Koen Buys, Ruben Smits +* +* +* +*/ + +extern "C"{ +#include "ethercattype.h" +#include "nicdrv.h" +#include "ethercatbase.h" +#include "ethercatmain.h" +#include "ethercatconfig.h" +#include "ethercatcoe.h" +#include "ethercatdc.h" +#include "ethercatprint.h" +} + +#include +#include "soem_master.h" +#include +#include + +namespace soem_master{ + + SoemMaster::SoemMaster(){} + + SoemMaster::~SoemMaster(){ + //this->close(); + } + + bool SoemMaster::init(const char *ifname){ + + /* initialise SOEM, bind socket to ifname */ + if (ec_init(ifname)){ + printf("ec_init on %s succeeded.\n",ifname); + /* find and auto-config slaves */ + if ( ec_config(TRUE, &IOmap_) > 0 ) + { + //copy nr of slaves to class variable + nr_slaves_=ec_slavecount; + printf("%d slaves found and configured.\n",ec_slavecount); + /* wait for all slaves to reach SAFE_OP state */ + ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); + if (ec_slave[0].state != EC_STATE_SAFE_OP ) + { + printf("Not all slaves reached safe operational state.\n"); + ec_readstate(); + //If not all slaves operational find out which one + for(int i = 1; i<=ec_slavecount ; i++) + { + if(ec_slave[i].state != EC_STATE_SAFE_OP) + { + printf("Slave %d State=%2x StatusCode=%4x : %s\n", + i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); + } + } + } + + //Configure distributed clock + ec_configdc(); + //Read the state of all slaves + ec_readstate(); + + printf("Request operational state for all slaves\n"); + ec_slave[0].state = EC_STATE_OPERATIONAL; + /* request OP state for all slaves */ + ec_writestate(0); + /* wait for all slaves to reach OP state */ + ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); + if (ec_slave[0].state == EC_STATE_OPERATIONAL ) + { + printf("Operational state reached for all slaves.\n"); + } + else{ + printf("Not all slaves reached operational state. \n"); + return false; + } + + this->createDrivers(); + } else + { + printf("No slaves found!\n"); + return false; + } + return true; + } + else + { + printf("No socket connection on %s\nExcecute as root\n",ifname); + return false; + } + } + + void SoemMaster::close(){ + //delete all drivers + for (unsigned int i=0;i& SoemMaster::getDrivers(){ + return drivers_; + } + + + +/* +void SoemMaster::printstate(){ + for(uint cnt = 1 ; cnt <= ec_slavecount ; cnt++) + { + printf("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n", + cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, + ec_slave[cnt].state, ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); + if (ec_slave[cnt].hasdc) printf(" DCParentport:%d\n", ec_slave[cnt].parentport); + printf(" Activeports:%d.%d.%d.%d\n", (ec_slave[cnt].activeports & 0x01) > 0 , + (ec_slave[cnt].activeports & 0x02) > 0 , + (ec_slave[cnt].activeports & 0x04) > 0 , + (ec_slave[cnt].activeports & 0x08) > 0 ); + printf(" Configured address: %4.4x\n", ec_slave[cnt].configadr); + printf(" Man: %8.8x ID: %8.8x Rev: %8.8x\n", (int)ec_slave[cnt].eep_man, (int)ec_slave[cnt].eep_id, (int)ec_slave[cnt].eep_rev); + printf(" SM0 A:%4.4x L:%4d F:%8.8x\n", ec_slave[cnt].SM[0].StartAddr, ec_slave[cnt].SM[0].SMlength, (int)ec_slave[cnt].SM[0].SMflags); + printf(" SM1 A:%4.4x L:%4d F:%8.8x\n", ec_slave[cnt].SM[1].StartAddr, ec_slave[cnt].SM[1].SMlength, (int)ec_slave[cnt].SM[1].SMflags); + printf(" SM2 A:%4.4x L:%4d F:%8.8x\n", ec_slave[cnt].SM[2].StartAddr, ec_slave[cnt].SM[2].SMlength, (int)ec_slave[cnt].SM[2].SMflags); + printf(" SM3 A:%4.4x L:%4d F:%8.8x\n", ec_slave[cnt].SM[3].StartAddr, ec_slave[cnt].SM[3].SMlength, (int)ec_slave[cnt].SM[3].SMflags); + printf(" SM types 0:%d 1:%d 2:%d 3:%d\n", ec_slave[cnt].SMtype[0], ec_slave[cnt].SMtype[1], ec_slave[cnt].SMtype[2], ec_slave[cnt].SMtype[3]); + printf(" FMMU0 Ls:%8.8x Ll:%4d Lsb:%d Leb:%d Ps:%4.4x Psb:%d Ty:%2.2x Act:%2.2x\n", + (int)ec_slave[cnt].FMMU[0].LogStart, ec_slave[cnt].FMMU[0].LogLength, ec_slave[cnt].FMMU[0].LogStartbit, + ec_slave[cnt].FMMU[0].LogEndbit, ec_slave[cnt].FMMU[0].PhysStart, ec_slave[cnt].FMMU[0].PhysStartBit, + ec_slave[cnt].FMMU[0].FMMUtype, ec_slave[cnt].FMMU[0].FMMUactive); + printf(" FMMU1 Ls:%8.8x Ll:%4d Lsb:%d Leb:%d Ps:%4.4x Psb:%d Ty:%2.2x Act:%2.2x\n", + (int)ec_slave[cnt].FMMU[1].LogStart, ec_slave[cnt].FMMU[1].LogLength, ec_slave[cnt].FMMU[1].LogStartbit, + ec_slave[cnt].FMMU[1].LogEndbit, ec_slave[cnt].FMMU[1].PhysStart, ec_slave[cnt].FMMU[1].PhysStartBit, + ec_slave[cnt].FMMU[1].FMMUtype, ec_slave[cnt].FMMU[1].FMMUactive); + printf(" FMMU2 Ls:%8.8x Ll:%4d Lsb:%d Leb:%d Ps:%4.4x Psb:%d Ty:%2.2x Act:%2.2x\n", + (int)ec_slave[cnt].FMMU[2].LogStart, ec_slave[cnt].FMMU[2].LogLength, ec_slave[cnt].FMMU[2].LogStartbit, + ec_slave[cnt].FMMU[2].LogEndbit, ec_slave[cnt].FMMU[2].PhysStart, ec_slave[cnt].FMMU[2].PhysStartBit, + ec_slave[cnt].FMMU[2].FMMUtype, ec_slave[cnt].FMMU[2].FMMUactive); + printf(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n", + ec_slave[cnt].FMMU0func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU3func); + printf(" MBX length: %d MBX protocols : %2.2x\n", ec_slave[cnt].mbx_l, ec_slave[cnt].mbx_proto); + ssigen = ec_siifind(cnt, ECT_SII_GENERAL); + // SII general section + if (ssigen) + { + ec_slave[cnt].CoEdetails = ec_siigetbyte(cnt, ssigen + 0x07); + ec_slave[cnt].FoEdetails = ec_siigetbyte(cnt, ssigen + 0x08); + ec_slave[cnt].EoEdetails = ec_siigetbyte(cnt, ssigen + 0x09); + ec_slave[cnt].SoEdetails = ec_siigetbyte(cnt, ssigen + 0x0a); + if((ec_siigetbyte(cnt, ssigen + 0x0d) & 0x02) > 0) + { + ec_slave[cnt].blockLRW = 1; + ec_slave[0].blockLRW++; + } + ec_slave[cnt].Ebuscurrent = ec_siigetbyte(cnt, ssigen + 0x0e); + ec_slave[cnt].Ebuscurrent += ec_siigetbyte(cnt, ssigen + 0x0f) << 8; + ec_slave[0].Ebuscurrent += ec_slave[cnt].Ebuscurrent; + } + printf(" CoE details: %2.2x FoE details: %2.2x EoE details: %2.2x SoE details: %2.2x\n", + ec_slave[cnt].CoEdetails, ec_slave[cnt].FoEdetails, ec_slave[cnt].EoEdetails, ec_slave[cnt].SoEdetails); + printf(" Ebus current: %d[mA]\n only LRD/LWR:%d\n", + ec_slave[cnt].Ebuscurrent, ec_slave[cnt].blockLRW); + if ((ec_slave[cnt].mbx_proto & 0x04) && printSDO) + { + ODlist.Entries = 0; + memset(&ODlist, 0, sizeof(ODlist)); + if( ec_readODlist(cnt, &ODlist)) + { + printf(" CoE Object Description found, %d entries.\n",ODlist.Entries); + for( i = 0 ; i < ODlist.Entries ; i++) + { + ec_readODdescription(i, &ODlist); + while(EcatError) + { + printf("%s", ec_elist2string()); + } + printf(" Index: %4.4x Datatype: %4.4x Objectcode: %2.2x Name: %s\n", + ODlist.Index[i], ODlist.DataType[i], ODlist.ObjectCode[i], ODlist.Name[i]); + memset(&OElist, 0, sizeof(OElist)); + ec_readOE(i, &ODlist, &OElist); + while(EcatError) + { + printf("%s", ec_elist2string()); + } + for( j = 0 ; j < ODlist.MaxSub[i]+1 ; j++) + { + if ((OElist.DataType[j] > 0) && (OElist.BitLength[j] > 0)) + { + printf(" Sub: %2.2x Datatype: %4.4x Bitlength: %4.4x Obj.access: %4.4x Name: %s\n", + j, OElist.DataType[j], OElist.BitLength[j], OElist.ObjAccess[j], OElist.Name[j]); + if ((OElist.ObjAccess[j] & 0x0007)) + { + dtype = OElist.DataType[j]; + printf(" Value :%s\n", SDO2string(cnt, ODlist.Index[i], j, OElist.DataType[j])); + } + } + } + } + } + else + { + while(EcatError) + { + printf("%s", ec_elist2string()); + } + } + } + } +} +*/ + +}//namespace soem_master diff --git a/soem_master/src/soem_master.h b/soem_master/src/soem_master.h new file mode 100644 index 0000000..2369a6d --- /dev/null +++ b/soem_master/src/soem_master.h @@ -0,0 +1,65 @@ +/* +* This is the Ethercat master class based on the SOEM package +* +* @author: Koen Buys, Ruben Smits +* +* +* +*/ +#ifndef SOEM_MASTER_H +#define SOEM_MASTER_H + +extern "C"{ +#include +#include +} + +#include + +#include + +namespace soem_master{ + + +class SoemMaster +{ + int nr_slaves_; //contains the number of found slave on the EC bus + char IOmap_[4096]; + + public: + + SoemMaster(); + ~SoemMaster(); + /** + * Initialise the master and bind the socket to ifname + * + * @param ifname ethernet interface used to connect to ethercat slaves + * + * @return error code + */ + bool init(const char *ifname); + /** + * send updated data from all drivers to the slaves + * + */ + bool send(); + /** + * receive data from slaves and update drivers data + * + */ + bool receive(); + void update(); + void close(); + //void printstate(); + //should be protected, only public for developping + + const std::vector& getDrivers(); + + protected: + void createDrivers(); + std::vector drivers_; + +}; +} + +#endif diff --git a/soem_master/src/soem_master_component.cpp b/soem_master/src/soem_master_component.cpp new file mode 100644 index 0000000..cbb1aa6 --- /dev/null +++ b/soem_master/src/soem_master_component.cpp @@ -0,0 +1,63 @@ +// Copyright (C) 2008 Ruben Smits + +// Author: Ruben Smits +// Maintainer: Ruben Smits + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "soem_master_component.h" + +#include "ocl/Component.hpp" +ORO_CREATE_COMPONENT( soem_master::SoemMasterComponent ) + +namespace soem_master{ + + using namespace RTT; + + SoemMasterComponent::SoemMasterComponent(const std::string& name) : + TaskContext(name,PreOperational), + ifname_("eth0") + { + this->addProperty("ifname",ifname_).doc("interface to which the ethercat device is connected"); + } + + SoemMasterComponent::~SoemMasterComponent(){ + } + + bool SoemMasterComponent::configureHook(){ + if (!soem_master_.init(ifname_.c_str())) + return false; + + const std::vector& drivers = soem_master_.getDrivers(); + + for(unsigned int i=0;iprovides()->addService(drivers[i]->provides()); + drivers[i]->addPortsToTaskContext(this); + } + return true; + } + + void SoemMasterComponent::updateHook(){ + const std::vector& drivers = soem_master_.getDrivers(); + soem_master_.update(); + for(unsigned int i=0;iupdatePorts(); + } + + void SoemMasterComponent::cleanupHook(){ + soem_master_.close(); + } + +}//namespace diff --git a/soem_master/src/soem_master_component.h b/soem_master/src/soem_master_component.h new file mode 100644 index 0000000..2f755ef --- /dev/null +++ b/soem_master/src/soem_master_component.h @@ -0,0 +1,53 @@ +// Copyright (C) 2008 Ruben Smits + +// Author: Ruben Smits +// Maintainer: Ruben Smits + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +#ifndef SOEM_MASTER_COMPONENT_H +#define SOEM_MASTER_COMPONENT_H + +#include +#include + +#include "soem_master.h" + +#include + +namespace soem_master{ + + class SoemMasterComponent : public RTT::TaskContext + { + public: + SoemMasterComponent(const std::string& name); + ~SoemMasterComponent(); + + protected: + virtual bool configureHook(); + virtual bool startHook(){return true;}; + virtual void updateHook(); + virtual void stopHook(){}; + virtual void cleanupHook(); + + SoemMaster soem_master_; + + private: + std::string ifname_; + + };//class + +}//namespace + +#endif diff --git a/stack.xml b/stack.xml new file mode 100644 index 0000000..900c582 --- /dev/null +++ b/stack.xml @@ -0,0 +1,10 @@ + + soem + Maintained by Ruben Smits + GPL with runtime exception,BSD,LGPL + + http://ros.org/wiki/soem + + + +