From 1e6250a27b7606190068b01aee9a257151a81fd2 Mon Sep 17 00:00:00 2001 From: Sergio Portoles Date: Wed, 28 Oct 2015 20:48:13 +0100 Subject: [PATCH] Support for EL3255 module Drivers for Beckhoff module EL3255: EL3255 5Ch. potentiometer measurement with sensor supply. Based on EL3104 driver of Ruben Smits. --- soem_beckhoff_drivers/CMakeLists.txt | 3 + soem_beckhoff_drivers/src/soem_el3255.cpp | 211 ++++++++++++++++++++++ soem_beckhoff_drivers/src/soem_el3255.h | 121 +++++++++++++ 3 files changed, 335 insertions(+) create mode 100644 soem_beckhoff_drivers/src/soem_el3255.cpp create mode 100644 soem_beckhoff_drivers/src/soem_el3255.h diff --git a/soem_beckhoff_drivers/CMakeLists.txt b/soem_beckhoff_drivers/CMakeLists.txt index 8da5c85..9359198 100644 --- a/soem_beckhoff_drivers/CMakeLists.txt +++ b/soem_beckhoff_drivers/CMakeLists.txt @@ -62,6 +62,8 @@ orocos_plugin(soem_el30xx src/soem_beckhoff_drivers.cpp src/soem_el30xx.cpp) target_link_libraries(soem_el30xx ${soem_LIBRARIES}) orocos_plugin(soem_el3104 src/soem_beckhoff_drivers.cpp src/soem_el3104.cpp) target_link_libraries(soem_el3104 ${soem_LIBRARIES}) +orocos_plugin(soem_el3255 src/soem_beckhoff_drivers.cpp src/soem_el3255.cpp) +target_link_libraries(soem_el3255 ${soem_LIBRARIES}) if(NOT ORO_USE_ROSBUILD) add_dependencies(soem_el1xxx ${PROJECT_NAME}_generate_messages_cpp) @@ -72,5 +74,6 @@ if(NOT ORO_USE_ROSBUILD) add_dependencies(soem_el6022 ${PROJECT_NAME}_generate_messages_cpp) add_dependencies(soem_el30xx ${PROJECT_NAME}_generate_messages_cpp) add_dependencies(soem_el3104 ${PROJECT_NAME}_generate_messages_cpp) + add_dependencies(soem_el3255 ${PROJECT_NAME}_generate_messages_cpp) endif() orocos_generate_package() diff --git a/soem_beckhoff_drivers/src/soem_el3255.cpp b/soem_beckhoff_drivers/src/soem_el3255.cpp new file mode 100644 index 0000000..117685d --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el3255.cpp @@ -0,0 +1,211 @@ +/*************************************************************************** + tag: Sergio Portoles Tue Oct 27 13:36:50 CET 2015 soem_el3255.cpp + Based on EL3104 of Ruben Smits + + soem_el3255.cpp - description + ------------------- + begin : Tue October 27 2015 + copyright : (C) 2015 Sergio Portoles + email : first.last@kuleuven.be + + *************************************************************************** + * 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., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ + +#include "soem_el3255.h" +#include +#include +#include + +using namespace RTT; + +namespace soem_beckhoff_drivers +{ + +SoemEL3255::SoemEL3255(ec_slavet* mem_loc) : + soem_master::SoemDriver(mem_loc), m_size(5), m_raw_range(65535), m_lowest( + 0.0), m_highest(1.0), m_values(m_size), m_raw_values(m_size),m_params(m_size), + m_values_port("values"), m_raw_values_port("raw_values") + +{ + m_service->doc(std::string("Services for Beckhoff ") + std::string( + m_datap->name) + std::string(" module")); + m_service->addOperation("rawRead", &SoemEL3255::rawRead, this, + RTT::OwnThread).doc("Read raw value of channel i").arg("i", + "channel nr"); + m_service->addOperation("read", &SoemEL3255::read, this, RTT::OwnThread).doc( + "Read value to channel i").arg("i", "channel nr"); + m_service->addOperation("Over_Range", &SoemEL3255::isOverrange, this, + RTT::OwnThread).doc( + "For the channel i : 1 = overrange ; 0 = no overrange ").arg("i", + "channel nr"); + m_service->addOperation("Under_Range", &SoemEL3255::isUnderrange, this, + RTT::OwnThread).doc( + "For the channel i : 1 = Underrange ; 0 = no Underrange ").arg("i", + "channel nr"); + m_service->addOperation("Comp_val_to_lim", &SoemEL3255::checkLimit, + this, RTT::OwnThread).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"); + m_service->addOperation("Error", &SoemEL3255::is_error, this).doc( + "For the channel i : 1 = error (Overrange or Underrange ; 0 = no error ").arg( + "i", "channel nr"); + + m_resolution = ((m_highest - m_lowest) / (double) m_raw_range); + + m_service->addConstant("raw_range", m_raw_range); + m_service->addConstant("resolution", m_resolution); + m_service->addConstant("lowest", m_lowest); + m_service->addConstant("highest", m_highest); + + m_service->addPort(m_values_port).doc( + "AnalogMsg contain the read values of _all_ channels"); + m_service->addPort(m_raw_values_port).doc( + "AnalogMsg containing the read values of _all_ channels"); + + m_msg.values.resize(m_size); + m_raw_msg.values.resize(m_size); +} + +bool SoemEL3255::configure() +{ + return true; +} + +void SoemEL3255::update() +{ + + m_raw_msg.values[0] = ((in_el3225_rtd_standard_t*) (m_datap->inputs))->val_ch1; + m_raw_msg.values[1] = ((in_el3225_rtd_standard_t*) (m_datap->inputs))->val_ch2; + m_raw_msg.values[2] = ((in_el3225_rtd_standard_t*) (m_datap->inputs))->val_ch3; + m_raw_msg.values[3] = ((in_el3225_rtd_standard_t*) (m_datap->inputs))->val_ch4; + m_raw_msg.values[4] = ((in_el3225_rtd_standard_t*) (m_datap->inputs))->val_ch5; + m_raw_values_port.write(m_raw_msg); + + for(int i=0;iinputs))->param_ch1; + m_params[1] = ((in_el3225_rtd_standard_t*) (m_datap->inputs))->param_ch2; + m_params[2] = ((in_el3225_rtd_standard_t*) (m_datap->inputs))->param_ch3; + m_params[3] = ((in_el3225_rtd_standard_t*) (m_datap->inputs))->param_ch4; + m_params[4] = ((in_el3225_rtd_standard_t*) (m_datap->inputs))->param_ch5; + +} + +//rawRead : read the raw value of the input ///////////////////////////////////////////////////// +int SoemEL3255::rawRead(unsigned int chan) +{ + if (chan < m_size) + { + return m_raw_msg.values[chan]; + } + else + log(Error) << "Channel " << chan << " outside of module's range" + << endlog(); + return false; +} + +//read: read the value of one of the 2 channels in Volts /////////////////////////// +double SoemEL3255::read(unsigned int chan) +{ + + if (chan < m_size) + { + return m_msg.values[chan]; + } + else + log(Error) << "Channel " << chan << " is out of the module's range" + << endlog(); + return 0.0; + +} + +//Checking overrange//////////////////////////////////////////////////////////////// + +bool SoemEL3255::isOverrange(unsigned int chan) +{ + if (chan < m_size) + return m_params[chan].test(OVERRANGE); + else + log(Error) << "Channel " << chan << " is out of the module's range" + << endlog(); + return false; +} + +//Checking Underrange//////////////////////////////////////////////////////////////// + +bool SoemEL3255::isUnderrange(unsigned int chan) +{ + if (chan < m_size) + return m_params[chan].test(UNDERRANGE); + else + log(Error) << "Channel " << chan << " is out of the module's range" + << endlog(); + return false; +} + +// Comparing the Value of Channel chan with its own limits (1/2) +bool SoemEL3255::checkLimit(unsigned int chan, unsigned int lim_num) +{ + if (!chan < m_size) + { + log(Error) << "Channel " << chan << " is out of the module's range" + << endlog(); + return false; + } + if (!(lim_num > 0 && lim_num < 3)) + { + log(Error) << "Limit nr " << lim_num << " should be 1 or 2" << endlog(); + return false; + } + if (lim_num == 1) + return m_params[chan].test(LIMIT1SMALLER); + else + return m_params[chan].test(LIMIT2SMALLER); +} + +// Checking for errors +bool SoemEL3255::is_error(unsigned int chan) +{ + if (chan < m_size) + return m_params[chan].test(ERROR); + else + log(Error) << "Channel " << chan << " is out of the module's range" + << endlog(); + return false; +} + +namespace +{ +soem_master::SoemDriver* createSoemEL3255(ec_slavet* mem_loc) +{ + return new SoemEL3255(mem_loc); +} + + +REGISTER_SOEM_DRIVER(EL3255, createSoemEL3255) + +} + +} //namespace soem_beckhoff_drivers + + diff --git a/soem_beckhoff_drivers/src/soem_el3255.h b/soem_beckhoff_drivers/src/soem_el3255.h new file mode 100644 index 0000000..561cf5e --- /dev/null +++ b/soem_beckhoff_drivers/src/soem_el3255.h @@ -0,0 +1,121 @@ +/*************************************************************************** + tag: Sergio Portoles Tue Oct 27 13:36:50 CET 2015 soem_el3255.h + Based on EL3104 of Ruben Smits + + soem_el3255.h - description + ------------------- + begin : Tue October 27 2015 + copyright : (C) 2015 Sergio Portoles + email : first.last@kuleuven.be + + *************************************************************************** + * 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., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ + +#ifndef SOEM_EL3255_H +#define SOEM_EL3255_H + +#include +#include +#include +#include +#include +#include "COE_config.h" + +namespace soem_beckhoff_drivers +{ + +class SoemEL3255: public soem_master::SoemDriver +{ + enum + { + UNDERRANGE = 0, + OVERRANGE, + LIMIT1SMALLER, + LIMIT1HIGHER, + LIMIT2SMALLER, + LIMIT2HIGHER, + ERROR, + ST_1, + ST_2, + ST_3, + ST_4, + ST_5, + ST_6, + SYNC_ERROR, + TXPDO_STATE, + TXPDO_TOGGLE, + }; + + typedef struct + PACKED { + int16 param_ch1; + int16 val_ch1; + int16 param_ch2; + int16 val_ch2; + int16 param_ch3; + int16 val_ch3; + int16 param_ch4; + int16 val_ch4; + int16 param_ch5; + int16 val_ch5; + } in_el3225_rtd_standard_t; + + typedef struct + PACKED { + int16 value; + } in_el3225_rtd_compact_t; + + public: + SoemEL3255(ec_slavet* mem_loc); + ~SoemEL3255() + { + } + ; + + int rawRead(unsigned int chan); + double read(unsigned int chan); + bool isOverrange(unsigned int chan = 0); + bool isUnderrange(unsigned int chan = 0); + bool checkLimit(unsigned int chan = 0, unsigned int Lim_num = 0); + bool is_error(unsigned int chan); + + void update(); + bool configure(); + + private: + + const unsigned int m_size; + const unsigned int m_raw_range; + const double m_lowest; + const double m_highest; + double m_resolution; + std::vector > m_params; + AnalogMsg m_msg; + AnalogMsg m_raw_msg; + std::vector m_values; + std::vector m_raw_values; + + //Ports/////////// + RTT::OutputPort m_values_port; + RTT::OutputPort m_raw_values_port; + + }; // End of class SoemEL3255 + +} // End of namespace soem_beckhoff_drivers + +#endif