diff --git a/ethercat_plugins/available_plugins.md b/ethercat_plugins/available_plugins.md deleted file mode 100644 index f4859aad..00000000 --- a/ethercat_plugins/available_plugins.md +++ /dev/null @@ -1,62 +0,0 @@ -# ethercat_plugins -Collection of plugin implementations of EtherCAT modules for the `ethercat_driver`. - -## Module list and configuration -The list of currently supported EtherCAT modules and the available parameters. All modules have the `alias` and `position` parameter to specify the bus topology. -### Beckhoff -- **Beckhoff_EK1100**: EtherCAT Coupler. -- **Beckhoff_EL1008**: EtherCAT Terminal, 8-channel digital input, 24 V DC, 3 ms. - - parameters: `di.1..8` - requested digital input -- **Beckhoff_EL1018**: EtherCAT Terminal, 8-channel digital input, 24 V DC, 10 us. - - parameters: `di.1..8` - requested digital input -- **Beckhoff_EL2008**: EtherCAT Terminal, 8-channel digital output, 24 V DC, 0.5 A. - - parameters: `do.1..8` - requested digital output -- **Beckhoff_EL2088**: EtherCAT Terminal, 8-channel digital output, 24 V DC, 0.5 A, ground switching. - - parameters: `do.1..8` - requested digital output -- **Beckhoff_EL2124**: EtherCAT Terminal, 4-channel digital output, 5 V DC, 20 mA. - - parameters: `do.1..4` - requested digital output -- **Beckhoff_EL3102**: EtherCAT Terminal, 2-channel analog input, voltage, ±10 V, 16 bit, differential. - - parameters: `ai.1..2` - requested analog input -- **Beckhoff_EL3104**: EtherCAT Terminal, 4-channel analog input, voltage, ±10 V, 16 bit, differential. - - parameters: `ai.1..4` - requested analog input -- **Beckhoff_EL4132**: EtherCAT Terminal, 2-channel analog output, voltage, ±10 V, 16 bit. - - parameters: `ao.1..2` - requested analog output -- **Beckhoff_EL4134**: EtherCAT Terminal, 4-channel analog output, voltage, ±10 V, 16 bit. - - parameters: `ao.1..4` - requested analog output -- **Beckhoff_EL5101**: EtherCAT Terminal, 1-channel encoder interface, incremental, 5 V DC (DIFF RS422, TTL), 1 MHz. - - parameters: - 1. `encoder_position` - current encoder position - 2. `convertion_factor` - encoder tic to unit conversion factor - 3. `encoder_reset` - reset encoder counting (reset on 0->1 switch) - -## Maxon -- **EPOS3**: EPOS3 70/10 EtherCAT, digital positioning controller, 10 A, 11 - 70 VDC - - parameters: - 1. `mode_of_operation`: see [EPOS3 documentation](https://maxonjapan.com/wp-content/uploads/manual/epos/EPOS3_EtherCAT_Firmware_Specification_En.pdf) -> Table 8-116 - -## ATI -- **ATI_FTSensor**: ATI EtherCAT F/T Sensor - - parameters: - 1. `force.[x|y|z].state_interface`: `state_interface` name for force on [x|y|z]-axis - 2. `force.[x|y|z].offset`: data offset for force on [x|y|z]-axis - 3. `torque.[x|y|z].state_interface`: `state_interface` name for torque on [x|y|z]-axis - 4. `torque.[x|y|z].offset`: data offset for torque on [x|y|z]-axis -### Advantech -- **AMAX-5074**: EtherCAT Coupler with ID switch -- **AMAX-5051**: Digital Input Module, 8-channel digital input, 24 V DC, 4 ms. - - parameters: `di.1..8` - requested digital input -- **AMAX-5056**: Sink-type Digital Output Module, 8-channel digital output, 24 V DC, 0.3 A. - - parameters: `do.1..8` - requested digital output - -### Omron - -- **Omron_NX_ECC201_NX_ID5442**: Omron EtherCAT Coupler NX_ECC201 with Input module NX_ID5442. -- **Omron_NX_ECC201_NX_OD5256**: Omron EtherCAT Coupler NX_ECC201 with Output module NX_OD5256. -- **Omron_NX_ECC202_NX_ID5142_1**: Omron EtherCAT Coupler NX_ECC202 with Input module NX_ID5142_1. -- **Omron_NX_ECC202_NX_OD5256**: Omron EtherCAT Coupler NX_ECC202 with Output module NX_OD5256. - -### Schneider Electric - -- **Schneider_ATV320**: Schneider Electric Variable frequency drive. Coupled with VW3A3601 communication card. - - state interfaces: `velocity` - - command interfaces: `velocity` diff --git a/ethercat_plugins/ethercat_advantech_modules/CMakeLists.txt b/ethercat_plugins/ethercat_advantech_modules/CMakeLists.txt deleted file mode 100644 index ab2c1a84..00000000 --- a/ethercat_plugins/ethercat_advantech_modules/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ethercat_advantech_modules) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(ethercat_interface REQUIRED) -find_package(pluginlib REQUIRED) - -file(GLOB_RECURSE PLUGINS_SRC src/*.cpp) -add_library(${PROJECT_NAME} ${PLUGINS_SRC}) -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) -ament_target_dependencies( - ${PROJECT_NAME} - "ethercat_interface" - "pluginlib" -) -pluginlib_export_plugin_description_file(ethercat_interface ethercat_plugins.xml) -# install( -# DIRECTORY include/ -# DESTINATION include -# ) -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(pluginlib REQUIRED) - find_package(ethercat_interface REQUIRED) - - ament_lint_auto_find_test_dependencies() - - ament_add_gmock( - test_load_advantech_plugins - test/test_load_ec_modules.cpp - ) - target_include_directories(test_load_advantech_plugins PRIVATE include) - ament_target_dependencies(test_load_advantech_plugins - pluginlib - ethercat_interface - ) -endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_targets( - export_${PROJECT_NAME} -) -ament_package() diff --git a/ethercat_plugins/ethercat_advantech_modules/ethercat_plugins.xml b/ethercat_plugins/ethercat_advantech_modules/ethercat_plugins.xml deleted file mode 100644 index 26e45bbc..00000000 --- a/ethercat_plugins/ethercat_advantech_modules/ethercat_plugins.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - EtherCAT Terminal, 8-channel digital input, 24 V DC - - - EtherCAT Terminal, 8-channel digital output, 24 V DC - - diff --git a/ethercat_plugins/ethercat_advantech_modules/package.xml b/ethercat_plugins/ethercat_advantech_modules/package.xml deleted file mode 100644 index d9e5ca29..00000000 --- a/ethercat_plugins/ethercat_advantech_modules/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - ethercat_advantech_modules - 1.2.0 - Plugin implementations of EtherCAT Advantech modules - Maciej Bednarczyk - Apache-2.0 - - ament_cmake_ros - - ethercat_interface - pluginlib - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/ethercat_plugins/ethercat_advantech_modules/src/amax5051.cpp b/ethercat_plugins/ethercat_advantech_modules/src/amax5051.cpp deleted file mode 100644 index a4f86e91..00000000 --- a/ethercat_plugins/ethercat_advantech_modules/src/amax5051.cpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class Advantech_AMAX5051 : public ethercat_interface::EcSlave -{ -public: - Advantech_AMAX5051() - : EcSlave(0x000013FE, 0x00035051) - { - std::cerr << "The Advantech_AMAX5051 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Advantech_AMAX5051() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - uint8_t data = 0; - data = EC_READ_U8(domain_address); - digital_inputs_[0] = ((data & 0b00000001) != 0); // bit 0 - digital_inputs_[1] = ((data & 0b00000010) != 0); // bit 1 - digital_inputs_[2] = ((data & 0b00000100) != 0); // bit 2 - digital_inputs_[3] = ((data & 0b00001000) != 0); // bit 3 - digital_inputs_[4] = ((data & 0b00010000) != 0); // bit 4 - digital_inputs_[5] = ((data & 0b00100000) != 0); // bit 5 - digital_inputs_[6] = ((data & 0b01000000) != 0); // bit 6 - digital_inputs_[7] = ((data & 0b10000000) != 0); // bit 7 - for (auto i = 0ul; i < 8; i++) { - if (sii_di_[i] >= 0) { - state_interface_ptr_->at(sii_di_[i]) = digital_inputs_[i]; - } - } - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 8; index++) { - if (paramters_.find("di." + std::to_string(index)) != paramters_.end()) { - if (paramters_.find( - "state_interface/" + paramters_["di." + std::to_string(index)]) != paramters_.end()) - { - sii_di_[index] = std::stoi( - paramters_["state_interface/" + paramters_["di." + std::to_string(index)]]); - } - } - } - return true; - } - -private: - int sii_di_[8] = {-1, -1, -1, -1, -1, -1, -1, -1}; - // digital write values - bool write_data_[8] = {false, false, false, false, false, false, false, false}; - bool digital_inputs_[8]; - - ec_pdo_entry_info_t channels_[1] = { - {0x3001, 0x00, 1}, /* Input */ - }; - ec_pdo_info_t pdos_[1] = { - {0x1a00, 1, channels_ + 0}, /* Channel 1 */ - }; - ec_sync_info_t syncs_[2] = { - {0, EC_DIR_INPUT, 1, pdos_ + 0, EC_WD_DISABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {0}} - }; -}; -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Advantech_AMAX5051, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_advantech_modules/src/amax5056.cpp b/ethercat_plugins/ethercat_advantech_modules/src/amax5056.cpp deleted file mode 100644 index 5c1cb0f4..00000000 --- a/ethercat_plugins/ethercat_advantech_modules/src/amax5056.cpp +++ /dev/null @@ -1,128 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class Advantech_AMAX5056 : public ethercat_interface::EcSlave -{ -public: - Advantech_AMAX5056() - : EcSlave(0x000013FE, 0x00035056) - { - std::cerr << "The Advantech_AMAX5056 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Advantech_AMAX5056() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - uint8_t digital_out_; - digital_out_ = 0; - for (auto i = 0ul; i < 8; i++) { - if (cii_do_[i] >= 0) { - if (!std::isnan(command_interface_ptr_->at(cii_do_[i]))) { - write_data_[i] = (command_interface_ptr_->at(cii_do_[i])) ? true : false; - if (sii_do_[i] >= 0) { - state_interface_ptr_->at(sii_do_[i]) = command_interface_ptr_->at(cii_do_[i]); - } - } - } - } - - // bit masking to get individual input values - digital_out_ += ( write_data_[0] << 0 ); // bit 0 - digital_out_ += ( write_data_[1] << 1 ); // bit 1 - digital_out_ += ( write_data_[2] << 2 ); // bit 2 - digital_out_ += ( write_data_[3] << 3 ); // bit 3 - digital_out_ += ( write_data_[4] << 4 ); // bit 4 - digital_out_ += ( write_data_[5] << 5 ); // bit 5 - digital_out_ += ( write_data_[6] << 6 ); // bit 6 - digital_out_ += ( write_data_[7] << 7 ); // bit 7 - - EC_WRITE_U8(domain_address, digital_out_); - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 8; index++) { - if (paramters_.find("do." + std::to_string(index)) != paramters_.end()) { - if (paramters_.find( - "command_interface/" + paramters_["do." + std::to_string(index)]) != paramters_.end()) - { - cii_do_[index] = std::stoi( - paramters_["command_interface/" + paramters_["do." + std::to_string(index)]]); - } - if (paramters_.find( - "state_interface/" + paramters_["do." + std::to_string(index)]) != paramters_.end()) - { - sii_do_[index] = std::stoi( - paramters_["state_interface/" + paramters_["do." + std::to_string(index)]]); - } - } - } - return true; - } - -private: - int cii_do_[8] = {-1, -1, -1, -1, -1, -1, -1, -1}; - int sii_do_[8] = {-1, -1, -1, -1, -1, -1, -1, -1}; - // digital write values - bool write_data_[8] = {false, false, false, false, false, false, false, false}; - ec_pdo_entry_info_t channels_[8] = { - {0x3101, 0x01, 1}, /* Output */ - {0x3101, 0x02, 1}, /* Output */ - {0x3101, 0x03, 1}, /* Output */ - {0x3101, 0x04, 1}, /* Output */ - {0x3101, 0x05, 1}, /* Output */ - {0x3101, 0x06, 1}, /* Output */ - {0x3101, 0x07, 1}, /* Output */ - {0x3101, 0x08, 1}, /* Output */ - }; - ec_pdo_info_t pdos_[1] = { - {0x1600, 1, channels_ + 0}, /* Channel 1 */ - }; - ec_sync_info_t syncs_[2] = { - {0, EC_DIR_OUTPUT, 1, pdos_ + 0, EC_WD_ENABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {0}} - }; -}; -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Advantech_AMAX5056, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_advantech_modules/test/test_load_ec_modules.cpp b/ethercat_plugins/ethercat_advantech_modules/test/test_load_ec_modules.cpp deleted file mode 100644 index 21b0fbc0..00000000 --- a/ethercat_plugins/ethercat_advantech_modules/test/test_load_ec_modules.cpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2023 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include "ethercat_interface/ec_slave.hpp" - -TEST(TestLoadAdvantech_AMAX5051, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Advantech_AMAX5051")); -} - -TEST(TestLoadAdvantech_AMAX5056, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Advantech_AMAX5056")); -} diff --git a/ethercat_plugins/ethercat_ati_modules/CMakeLists.txt b/ethercat_plugins/ethercat_ati_modules/CMakeLists.txt deleted file mode 100644 index 2ae0261f..00000000 --- a/ethercat_plugins/ethercat_ati_modules/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ethercat_ati_modules) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(ethercat_interface REQUIRED) -find_package(pluginlib REQUIRED) - -file(GLOB_RECURSE PLUGINS_SRC src/*.cpp) -add_library(${PROJECT_NAME} ${PLUGINS_SRC}) -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) -ament_target_dependencies( - ${PROJECT_NAME} - "ethercat_interface" - "pluginlib" -) -pluginlib_export_plugin_description_file(ethercat_interface ethercat_plugins.xml) -# install( -# DIRECTORY include/ -# DESTINATION include -# ) -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(pluginlib REQUIRED) - find_package(ethercat_interface REQUIRED) - - ament_lint_auto_find_test_dependencies() - - ament_add_gmock( - test_load_ati_plugins - test/test_load_ec_modules.cpp - ) - target_include_directories(test_load_ati_plugins PRIVATE include) - ament_target_dependencies(test_load_ati_plugins - pluginlib - ethercat_interface - ) -endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_targets( - export_${PROJECT_NAME} -) -ament_package() diff --git a/ethercat_plugins/ethercat_ati_modules/ethercat_plugins.xml b/ethercat_plugins/ethercat_ati_modules/ethercat_plugins.xml deleted file mode 100644 index 7a27b0ef..00000000 --- a/ethercat_plugins/ethercat_ati_modules/ethercat_plugins.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - ATI EtherCAT F/T Sensor. - - diff --git a/ethercat_plugins/ethercat_ati_modules/package.xml b/ethercat_plugins/ethercat_ati_modules/package.xml deleted file mode 100644 index 65136f30..00000000 --- a/ethercat_plugins/ethercat_ati_modules/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - ethercat_ati_modules - 1.2.0 - Plugin implementations of EtherCAT ATI modules - Maciej Bednarczyk - Apache-2.0 - - ament_cmake_ros - - ethercat_interface - pluginlib - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/ethercat_plugins/ethercat_ati_modules/src/ati_ft_sensor.cpp b/ethercat_plugins/ethercat_ati_modules/src/ati_ft_sensor.cpp deleted file mode 100644 index afe1d0aa..00000000 --- a/ethercat_plugins/ethercat_ati_modules/src/ati_ft_sensor.cpp +++ /dev/null @@ -1,166 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class ATI_FTSensor : public ethercat_interface::EcSlave -{ -public: - ATI_FTSensor() - : EcSlave(0x00000732, 0x26483052) - { - std::cerr << "The ATI_FTSensor plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~ATI_FTSensor() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - if (sii_ft_[index] >= 0) { - double data = static_cast(EC_READ_S32(domain_address)); - state_interface_ptr_->at(sii_ft_[index]) = data / 1e6 - data_offset_[index]; - } - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - if (paramters_.find("force.x.state_interface") != paramters_.end()) { - if (paramters_.find( - "state_interface/" + paramters_["force.x.state_interface"]) != paramters_.end()) - { - sii_ft_[0] = std::stoi( - paramters_["state_interface/" + paramters_["force.x.state_interface"]]); - } - } - if (paramters_.find("force.y.state_interface") != paramters_.end()) { - if (paramters_.find( - "state_interface/" + paramters_["force.y.state_interface"]) != paramters_.end()) - { - sii_ft_[1] = std::stoi( - paramters_["state_interface/" + paramters_["force.y.state_interface"]]); - } - } - if (paramters_.find("force.z.state_interface") != paramters_.end()) { - if (paramters_.find( - "state_interface/" + paramters_["force.z.state_interface"]) != paramters_.end()) - { - sii_ft_[2] = std::stoi( - paramters_["state_interface/" + paramters_["force.z.state_interface"]]); - } - } - if (paramters_.find("torque.x.state_interface") != paramters_.end()) { - if (paramters_.find( - "state_interface/" + paramters_["torque.x.state_interface"]) != paramters_.end()) - { - sii_ft_[3] = std::stoi( - paramters_["state_interface/" + paramters_["torque.x.state_interface"]]); - } - } - if (paramters_.find("torque.y.state_interface") != paramters_.end()) { - if (paramters_.find( - "state_interface/" + paramters_["torque.y.state_interface"]) != paramters_.end()) - { - sii_ft_[4] = std::stoi( - paramters_["state_interface/" + paramters_["torque.y.state_interface"]]); - } - } - if (paramters_.find("torque.z.state_interface") != paramters_.end()) { - if (paramters_.find( - "state_interface/" + paramters_["torque.z.state_interface"]) != paramters_.end()) - { - sii_ft_[5] = std::stoi( - paramters_["state_interface/" + paramters_["torque.z.state_interface"]]); - } - } - - if (paramters_.find("force.x.offset") != paramters_.end()) { - data_offset_[0] = std::stod(paramters_["force.x.offset"]); - } - if (paramters_.find("force.y.offset") != paramters_.end()) { - data_offset_[1] = std::stod(paramters_["force.y.offset"]); - } - if (paramters_.find("force.z.offset") != paramters_.end()) { - data_offset_[2] = std::stod(paramters_["force.z.offset"]); - } - if (paramters_.find("torque.x.offset") != paramters_.end()) { - data_offset_[3] = std::stod(paramters_["torque.x.offset"]); - } - if (paramters_.find("torque.y.offset") != paramters_.end()) { - data_offset_[4] = std::stod(paramters_["torque.y.offset"]); - } - if (paramters_.find("torque.z.offset") != paramters_.end()) { - data_offset_[5] = std::stod(paramters_["torque.z.offset"]); - } - - return true; - } - -private: - int sii_ft_[6] = {-1, -1, -1, -1, -1, -1}; - double data_offset_[6] = {0, 0, 0, 0, 0, 0}; - - ec_pdo_entry_info_t channels_[10] = { - {0x7010, 0x01, 32}, /* Control 1 */ - {0x7010, 0x02, 32}, /* Control 2 */ - {0x6000, 0x01, 32}, /* Fx/Gage0 */ - {0x6000, 0x02, 32}, /* Fy/Gage1 */ - {0x6000, 0x03, 32}, /* Fz/Gage2 */ - {0x6000, 0x04, 32}, /* Tx/Gage3 */ - {0x6000, 0x05, 32}, /* Ty/Gage3 */ - {0x6000, 0x06, 32}, /* Tz/Gage3 */ - {0x6010, 0x00, 32}, /* SubIndex 000 */ - {0x6020, 0x00, 32}, /* SubIndex 000 */ - }; - ec_pdo_info_t pdos_[2] = { - {0x1601, 2, channels_ + 0}, /* DO RxPDO-Map */ - {0x1a00, 8, channels_ + 2}, /* DI TxPDO-Map */ - }; - ec_sync_info_t syncs_[5] = { - {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, - {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, - {2, EC_DIR_OUTPUT, 1, pdos_ + 0, EC_WD_ENABLE}, - {3, EC_DIR_INPUT, 1, pdos_ + 1, EC_WD_DISABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {2, 3, 4, 5, 6, 7}} - }; -}; - -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::ATI_FTSensor, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_ati_modules/test/test_load_ec_modules.cpp b/ethercat_plugins/ethercat_ati_modules/test/test_load_ec_modules.cpp deleted file mode 100644 index b83eb4c5..00000000 --- a/ethercat_plugins/ethercat_ati_modules/test/test_load_ec_modules.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include "ethercat_interface/ec_slave.hpp" - -TEST(TestLoadATI_FTSensor, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/ATI_FTSensor")); -} diff --git a/ethercat_plugins/ethercat_beckhoff_modules/CMakeLists.txt b/ethercat_plugins/ethercat_beckhoff_modules/CMakeLists.txt deleted file mode 100644 index 4deb8f5f..00000000 --- a/ethercat_plugins/ethercat_beckhoff_modules/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ethercat_beckhoff_modules) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(ethercat_interface REQUIRED) -find_package(pluginlib REQUIRED) - -file(GLOB_RECURSE PLUGINS_SRC src/*.cpp) -add_library(${PROJECT_NAME} ${PLUGINS_SRC}) -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) -ament_target_dependencies( - ${PROJECT_NAME} - "ethercat_interface" - "pluginlib" -) -pluginlib_export_plugin_description_file(ethercat_interface ethercat_plugins.xml) -# install( -# DIRECTORY include/ -# DESTINATION include -# ) -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(pluginlib REQUIRED) - find_package(ethercat_interface REQUIRED) - - ament_lint_auto_find_test_dependencies() - - ament_add_gmock( - test_load_beckhoff_plugins - test/test_load_ec_modules.cpp - ) - target_include_directories(test_load_beckhoff_plugins PRIVATE include) - ament_target_dependencies(test_load_beckhoff_plugins - pluginlib - ethercat_interface - ) -endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_targets( - export_${PROJECT_NAME} -) -ament_package() diff --git a/ethercat_plugins/ethercat_beckhoff_modules/ethercat_plugins.xml b/ethercat_plugins/ethercat_beckhoff_modules/ethercat_plugins.xml deleted file mode 100644 index 16dddde2..00000000 --- a/ethercat_plugins/ethercat_beckhoff_modules/ethercat_plugins.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - EtherCAT Coupler. - - - EtherCAT Terminal, 8-channel digital input, 24 V DC, 3 ms. - - - EtherCAT Terminal, 8-channel digital input, 24 V DC, 10 us. - - - EtherCAT Terminal, 8-channel digital output, 24 V DC, 0.5 A. - - - EtherCAT Terminal, 8-channel digital output, 24 V DC, 0.5 A, ground switching. - - - EtherCAT Terminal, 4-channel digital output, 5 V DC, 20 mA. - - - EtherCAT Terminal, 2-channel analog input, voltage, ±10 V, 16 bit, differential. - - - EtherCAT Terminal, 4-channel analog input, voltage, ±10 V, 16 bit, differential. - - - EtherCAT Terminal, 2-channel analog output, voltage, ±10 V, 16 bit. - - - EtherCAT Terminal, 4-channel analog output, voltage, ±10 V, 16 bit. - - - EtherCAT Terminal, 1-channel encoder interface, incremental, 5 V DC (DIFF RS422, TTL), 1 MHz. - - diff --git a/ethercat_plugins/ethercat_beckhoff_modules/package.xml b/ethercat_plugins/ethercat_beckhoff_modules/package.xml deleted file mode 100644 index 2d2eb67b..00000000 --- a/ethercat_plugins/ethercat_beckhoff_modules/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - ethercat_beckhoff_modules - 1.2.0 - Plugin implementations of EtherCAT Beckhoff modules - Maciej Bednarczyk - Apache-2.0 - - ament_cmake_ros - - ethercat_interface - pluginlib - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_ek1100.cpp b/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_ek1100.cpp deleted file mode 100644 index ccc8a225..00000000 --- a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_ek1100.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class Beckhoff_EK1100 : public ethercat_interface::EcSlave -{ -public: - Beckhoff_EK1100() - : EcSlave(0x00000002, 0x044c2c52) - { - std::cerr << "The Beckhoff_EK1100 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Beckhoff_EK1100() {} -}; - -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Beckhoff_EK1100, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el10xx.cpp b/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el10xx.cpp deleted file mode 100644 index 4dad0eb9..00000000 --- a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el10xx.cpp +++ /dev/null @@ -1,221 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class Beckhoff_EL1008 : public ethercat_interface::EcSlave -{ -public: - Beckhoff_EL1008() - : EcSlave(0x00000002, 0x03f03052) - { - std::cerr << "The Beckhoff_EL1008 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Beckhoff_EL1008() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - uint8_t data = 0; - data = EC_READ_U8(domain_address); - bool digital_inputs_[8]; - digital_inputs_[0] = ((data & 0b00000001) != 0); // bit 0 - digital_inputs_[1] = ((data & 0b00000010) != 0); // bit 1 - digital_inputs_[2] = ((data & 0b00000100) != 0); // bit 2 - digital_inputs_[3] = ((data & 0b00001000) != 0); // bit 3 - digital_inputs_[4] = ((data & 0b00010000) != 0); // bit 4 - digital_inputs_[5] = ((data & 0b00100000) != 0); // bit 5 - digital_inputs_[6] = ((data & 0b01000000) != 0); // bit 6 - digital_inputs_[7] = ((data & 0b10000000) != 0); // bit 7 - for (auto i = 0ul; i < 8; i++) { - if (sii_di_[i] >= 0) { - state_interface_ptr_->at(sii_di_[i]) = digital_inputs_[i]; - } - } - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 8; index++) { - if (paramters_.find("di." + std::to_string(index + 1)) != paramters_.end()) { - if (paramters_.find( - "state_interface/" + paramters_["di." + std::to_string(index + 1)]) != paramters_.end()) - { - sii_di_[index] = std::stoi( - paramters_["state_interface/" + paramters_["di." + std::to_string(index + 1)]]); - } - } - } - return true; - } - -private: - int sii_di_[8] = {-1, -1, -1, -1, -1, -1, -1, -1}; - // digital write values - bool write_data_[8] = {false, false, false, false, false, false, false, false}; - - ec_pdo_entry_info_t channels_[8] = { - {0x6000, 0x01, 1}, /* Input */ - {0x6010, 0x01, 1}, /* Input */ - {0x6020, 0x01, 1}, /* Input */ - {0x6030, 0x01, 1}, /* Input */ - {0x6040, 0x01, 1}, /* Input */ - {0x6050, 0x01, 1}, /* Input */ - {0x6060, 0x01, 1}, /* Input */ - {0x6070, 0x01, 1}, /* Input */ - }; - ec_pdo_info_t pdos_[8] = { - {0x1a00, 1, channels_ + 0}, /* Channel 1 */ - {0x1a01, 1, channels_ + 1}, /* Channel 2 */ - {0x1a02, 1, channels_ + 2}, /* Channel 3 */ - {0x1a03, 1, channels_ + 3}, /* Channel 4 */ - {0x1a04, 1, channels_ + 4}, /* Channel 5 */ - {0x1a05, 1, channels_ + 5}, /* Channel 6 */ - {0x1a06, 1, channels_ + 6}, /* Channel 7 */ - {0x1a07, 1, channels_ + 7}, /* Channel 8 */ - }; - ec_sync_info_t syncs_[2] = { - {0, EC_DIR_INPUT, 8, pdos_ + 0, EC_WD_DISABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {0}} - }; -}; - -// -------------------------------------------------------------------------------------- -class Beckhoff_EL1018 : public ethercat_interface::EcSlave -{ -public: - Beckhoff_EL1018() - : EcSlave(0x00000002, 0x03fa3052) - { - std::cerr << "The Beckhoff_EL1018 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Beckhoff_EL1018() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - uint8_t data = 0; - data = EC_READ_U8(domain_address); - bool digital_inputs_[8]; - digital_inputs_[0] = ((data & 0b00000001) != 0); // bit 0 - digital_inputs_[1] = ((data & 0b00000010) != 0); // bit 1 - digital_inputs_[2] = ((data & 0b00000100) != 0); // bit 2 - digital_inputs_[3] = ((data & 0b00001000) != 0); // bit 3 - digital_inputs_[4] = ((data & 0b00010000) != 0); // bit 4 - digital_inputs_[5] = ((data & 0b00100000) != 0); // bit 5 - digital_inputs_[6] = ((data & 0b01000000) != 0); // bit 6 - digital_inputs_[7] = ((data & 0b10000000) != 0); // bit 7 - for (auto i = 0ul; i < 8; i++) { - if (sii_di_[i] >= 0) { - state_interface_ptr_->at(sii_di_[i]) = digital_inputs_[i]; - } - } - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 8; index++) { - if (paramters_.find("di." + std::to_string(index + 1)) != paramters_.end()) { - if (paramters_.find( - "state_interface/" + paramters_["di." + std::to_string(index + 1)]) != paramters_.end()) - { - sii_di_[index] = std::stoi( - paramters_["state_interface/" + paramters_["di." + std::to_string(index + 1)]]); - } - } - } - return true; - } - -private: - int sii_di_[8] = {-1, -1, -1, -1, -1, -1, -1, -1}; - // digital write values - bool write_data_[8] = {false, false, false, false, false, false, false, false}; - - ec_pdo_entry_info_t channels_[8] = { - {0x6000, 0x01, 1}, /* Input */ - {0x6010, 0x01, 1}, /* Input */ - {0x6020, 0x01, 1}, /* Input */ - {0x6030, 0x01, 1}, /* Input */ - {0x6040, 0x01, 1}, /* Input */ - {0x6050, 0x01, 1}, /* Input */ - {0x6060, 0x01, 1}, /* Input */ - {0x6070, 0x01, 1}, /* Input */ - }; - ec_pdo_info_t pdos_[8] = { - {0x1a00, 1, channels_ + 0}, /* Channel 1 */ - {0x1a01, 1, channels_ + 1}, /* Channel 2 */ - {0x1a02, 1, channels_ + 2}, /* Channel 3 */ - {0x1a03, 1, channels_ + 3}, /* Channel 4 */ - {0x1a04, 1, channels_ + 4}, /* Channel 5 */ - {0x1a05, 1, channels_ + 5}, /* Channel 6 */ - {0x1a06, 1, channels_ + 6}, /* Channel 7 */ - {0x1a07, 1, channels_ + 7}, /* Channel 8 */ - }; - ec_sync_info_t syncs_[2] = { - {0, EC_DIR_INPUT, 8, pdos_ + 0, EC_WD_DISABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {0}} - }; -}; -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Beckhoff_EL1008, ethercat_interface::EcSlave) -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Beckhoff_EL1018, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el20xx.cpp b/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el20xx.cpp deleted file mode 100644 index 6ed25f9d..00000000 --- a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el20xx.cpp +++ /dev/null @@ -1,251 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class Beckhoff_EL2008 : public ethercat_interface::EcSlave -{ -public: - Beckhoff_EL2008() - : EcSlave(0x00000002, 0x07d83052) - { - std::cerr << "The Beckhoff_EL2008 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Beckhoff_EL2008() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - uint8_t digital_out_; - digital_out_ = 0; - for (auto i = 0ul; i < 8; i++) { - if (cii_do_[i] >= 0) { - if (!std::isnan(command_interface_ptr_->at(cii_do_[i]))) { - write_data_[i] = (command_interface_ptr_->at(cii_do_[i])) ? true : false; - if (sii_do_[i] >= 0) { - state_interface_ptr_->at(sii_do_[i]) = command_interface_ptr_->at(cii_do_[i]); - } - } - } - } - - // bit masking to get individual input values - digital_out_ += ( write_data_[0] << 0 ); // bit 0 - digital_out_ += ( write_data_[1] << 1 ); // bit 1 - digital_out_ += ( write_data_[2] << 2 ); // bit 2 - digital_out_ += ( write_data_[3] << 3 ); // bit 3 - digital_out_ += ( write_data_[4] << 4 ); // bit 4 - digital_out_ += ( write_data_[5] << 5 ); // bit 5 - digital_out_ += ( write_data_[6] << 6 ); // bit 6 - digital_out_ += ( write_data_[7] << 7 ); // bit 7 - - EC_WRITE_U8(domain_address, digital_out_); - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 8; index++) { - if (paramters_.find("do." + std::to_string(index + 1)) != paramters_.end()) { - if (paramters_.find( - "command_interface/" + paramters_["do." + std::to_string(index + 1)]) != - paramters_.end()) - { - cii_do_[index] = std::stoi( - paramters_["command_interface/" + paramters_["do." + std::to_string(index + 1)]]); - } - if (paramters_.find( - "state_interface/" + paramters_["do." + std::to_string(index + 1)]) != paramters_.end()) - { - sii_do_[index] = std::stoi( - paramters_["state_interface/" + paramters_["do." + std::to_string(index + 1)]]); - } - } - } - return true; - } - -private: - int cii_do_[8] = {-1, -1, -1, -1, -1, -1, -1, -1}; - int sii_do_[8] = {-1, -1, -1, -1, -1, -1, -1, -1}; - // digital write values - bool write_data_[8] = {false, false, false, false, false, false, false, false}; - ec_pdo_entry_info_t channels_[8] = { - {0x7000, 0x01, 1}, /* Output */ - {0x7010, 0x01, 1}, /* Output */ - {0x7020, 0x01, 1}, /* Output */ - {0x7030, 0x01, 1}, /* Output */ - {0x7040, 0x01, 1}, /* Output */ - {0x7050, 0x01, 1}, /* Output */ - {0x7060, 0x01, 1}, /* Output */ - {0x7070, 0x01, 1}, /* Output */ - }; - ec_pdo_info_t pdos_[8] = { - {0x1600, 1, channels_ + 0}, /* Channel 1 */ - {0x1601, 1, channels_ + 1}, /* Channel 2 */ - {0x1602, 1, channels_ + 2}, /* Channel 3 */ - {0x1603, 1, channels_ + 3}, /* Channel 4 */ - {0x1604, 1, channels_ + 4}, /* Channel 5 */ - {0x1605, 1, channels_ + 5}, /* Channel 6 */ - {0x1606, 1, channels_ + 6}, /* Channel 7 */ - {0x1607, 1, channels_ + 7}, /* Channel 8 */ - }; - ec_sync_info_t syncs_[2] = { - {0, EC_DIR_OUTPUT, 8, pdos_ + 0, EC_WD_ENABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {0}} - }; -}; -// -------------------------------------------------------------------------------------- -class Beckhoff_EL2088 : public ethercat_interface::EcSlave -{ -public: - Beckhoff_EL2088() - : EcSlave(0x00000002, 0x08283052) - { - std::cerr << "The Beckhoff_EL2088 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Beckhoff_EL2088() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - uint8_t digital_out_; - digital_out_ = 0; - for (auto i = 0ul; i < 8; i++) { - if (cii_do_[i] >= 0) { - if (!std::isnan(command_interface_ptr_->at(cii_do_[i]))) { - write_data_[i] = (command_interface_ptr_->at(cii_do_[i])) ? true : false; - if (sii_do_[i] >= 0) { - state_interface_ptr_->at(sii_do_[i]) = command_interface_ptr_->at(cii_do_[i]); - } - } - } - } - - // bit masking to get individual input values - digital_out_ += ( write_data_[0] << 0 ); // bit 0 - digital_out_ += ( write_data_[1] << 1 ); // bit 1 - digital_out_ += ( write_data_[2] << 2 ); // bit 2 - digital_out_ += ( write_data_[3] << 3 ); // bit 3 - digital_out_ += ( write_data_[4] << 4 ); // bit 4 - digital_out_ += ( write_data_[5] << 5 ); // bit 5 - digital_out_ += ( write_data_[6] << 6 ); // bit 6 - digital_out_ += ( write_data_[7] << 7 ); // bit 7 - - EC_WRITE_U8(domain_address, digital_out_); - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 8; index++) { - if (paramters_.find("do." + std::to_string(index + 1)) != paramters_.end()) { - if (paramters_.find( - "command_interface/" + paramters_["do." + std::to_string(index + 1)]) != - paramters_.end()) - { - cii_do_[index] = std::stoi( - paramters_["command_interface/" + paramters_["do." + std::to_string(index + 1)]]); - } - if (paramters_.find( - "state_interface/" + paramters_["do." + std::to_string(index + 1)]) != paramters_.end()) - { - sii_do_[index] = std::stoi( - paramters_["state_interface/" + paramters_["do." + std::to_string(index + 1)]]); - } - } - } - return true; - } - -private: - int cii_do_[8] = {-1, -1, -1, -1, -1, -1, -1, -1}; - int sii_do_[8] = {-1, -1, -1, -1, -1, -1, -1, -1}; - // digital write values - bool write_data_[8] = {false, false, false, false, false, false, false, false}; - ec_pdo_entry_info_t channels_[8] = { - {0x7000, 0x01, 1}, /* Output */ - {0x7010, 0x01, 1}, /* Output */ - {0x7020, 0x01, 1}, /* Output */ - {0x7030, 0x01, 1}, /* Output */ - {0x7040, 0x01, 1}, /* Output */ - {0x7050, 0x01, 1}, /* Output */ - {0x7060, 0x01, 1}, /* Output */ - {0x7070, 0x01, 1}, /* Output */ - }; - ec_pdo_info_t pdos_[8] = { - {0x1600, 1, channels_ + 0}, /* Channel 1 */ - {0x1601, 1, channels_ + 1}, /* Channel 2 */ - {0x1602, 1, channels_ + 2}, /* Channel 3 */ - {0x1603, 1, channels_ + 3}, /* Channel 4 */ - {0x1604, 1, channels_ + 4}, /* Channel 5 */ - {0x1605, 1, channels_ + 5}, /* Channel 6 */ - {0x1606, 1, channels_ + 6}, /* Channel 7 */ - {0x1607, 1, channels_ + 7}, /* Channel 8 */ - }; - ec_sync_info_t syncs_[2] = { - {0, EC_DIR_OUTPUT, 8, pdos_ + 0, EC_WD_ENABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {0}} - }; -}; - -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Beckhoff_EL2008, ethercat_interface::EcSlave) -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Beckhoff_EL2088, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el21xx.cpp b/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el21xx.cpp deleted file mode 100644 index c3f2a081..00000000 --- a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el21xx.cpp +++ /dev/null @@ -1,125 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class Beckhoff_EL2124 : public ethercat_interface::EcSlave -{ -public: - Beckhoff_EL2124() - : EcSlave(0x00000002, 0x084c3052) - { - std::cerr << "The Beckhoff_EL2124 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Beckhoff_EL2124() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - uint8_t digital_out_; - digital_out_ = 0; - for (auto i = 0ul; i < 4; i++) { - if (cii_do_[i] >= 0) { - if (command_interface_ptr_->at(cii_do_[i]) == command_interface_ptr_->at(cii_do_[i])) { - write_data_[i] = (command_interface_ptr_->at(cii_do_[i])) ? true : false; - if (sii_do_[i] >= 0) { - state_interface_ptr_->at(sii_do_[i]) = command_interface_ptr_->at(cii_do_[i]); - } - } - } - } - - // bit masking to get individual input values - digital_out_ += ( write_data_[0] << 0 ); // bit 0 - digital_out_ += ( write_data_[1] << 1 ); // bit 1 - digital_out_ += ( write_data_[2] << 2 ); // bit 2 - digital_out_ += ( write_data_[3] << 3 ); // bit 3 - - EC_WRITE_U8(domain_address, digital_out_); - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 4; index++) { - if (paramters_.find("do." + std::to_string(index + 1)) != paramters_.end()) { - if (paramters_.find( - "command_interface/" + paramters_["do." + std::to_string(index + 1)]) != - paramters_.end()) - { - cii_do_[index] = std::stoi( - paramters_["command_interface/" + paramters_["do." + std::to_string(index + 1)]]); - } - if (paramters_.find( - "state_interface/" + paramters_["do." + std::to_string(index + 1)]) != paramters_.end()) - { - sii_do_[index] = std::stoi( - paramters_["state_interface/" + paramters_["do." + std::to_string(index + 1)]]); - } - } - } - return true; - } - -private: - int cii_do_[4] = {-1, -1, -1, -1}; - int sii_do_[4] = {-1, -1, -1, -1}; - // digital write values - bool write_data_[4] = {false, false, false, false}; - ec_pdo_entry_info_t channels_[4] = { - {0x7000, 0x01, 1}, /* Output */ - {0x7010, 0x01, 1}, /* Output */ - {0x7020, 0x01, 1}, /* Output */ - {0x7030, 0x01, 1}, /* Output */ - }; - ec_pdo_info_t pdos_[4] = { - {0x1600, 1, channels_ + 0}, /* Channel 1 */ - {0x1601, 1, channels_ + 1}, /* Channel 2 */ - {0x1602, 1, channels_ + 2}, /* Channel 3 */ - {0x1603, 1, channels_ + 3}, /* Channel 4 */ - }; - ec_sync_info_t syncs_[2] = { - {0, EC_DIR_OUTPUT, 4, pdos_ + 0, EC_WD_ENABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {0}} - }; -}; - -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Beckhoff_EL2124, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el31xx.cpp b/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el31xx.cpp deleted file mode 100644 index c5e7ad22..00000000 --- a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el31xx.cpp +++ /dev/null @@ -1,222 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class Beckhoff_EL3102 : public ethercat_interface::EcSlave -{ -public: - Beckhoff_EL3102() - : EcSlave(0x00000002, 0x0c1e3052) - { - std::cerr << "The Beckhoff_EL3102 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Beckhoff_EL3102() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - if (sii_ai_[index] >= 0) { - double data = static_cast( - EC_READ_S16(domain_address)) / std::numeric_limits::max() * 10; - state_interface_ptr_->at(sii_ai_[index]) = data; - } - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 2; index++) { - if (paramters_.find( - "ai." + std::to_string(index + 1)) != paramters_.end()) - { - if (paramters_.find( - "state_interface/" + paramters_["ai." + std::to_string(index + 1)]) != paramters_.end()) - { - sii_ai_[index] = std::stoi( - paramters_["state_interface/" + paramters_["ai." + std::to_string(index + 1)]]); - } - } - } - return true; - } - -private: - int sii_ai_[2] = {-1, -1}; - - ec_pdo_entry_info_t channels_[4] = { - {0x3101, 0x01, 8}, /* Status */ - {0x3101, 0x02, 16}, /* Value */ - {0x3102, 0x01, 8}, /* Status */ - {0x3102, 0x02, 16}, /* Value */ - }; - ec_pdo_info_t pdos_[2] = { - {0x1a00, 2, channels_ + 0}, /* TxPDO-Map Channel 1 */ - {0x1a01, 2, channels_ + 2}, /* TxPDO-Map Channel 2 */ - }; - ec_sync_info_t syncs_[5] = { - {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, - {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, - {2, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, - {3, EC_DIR_INPUT, 2, pdos_ + 0, EC_WD_DISABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {1, 3}} - }; -}; -// -------------------------------------------------------------------------------------- -class Beckhoff_EL3104 : public ethercat_interface::EcSlave -{ -public: - Beckhoff_EL3104() - : EcSlave(0x00000002, 0x0c203052) - { - std::cerr << "The Beckhoff_EL3104 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Beckhoff_EL3104() {} - virtual void processData(size_t index, uint8_t domain_address) - { - if (sii_ai_[index] >= 0) { - double data = static_cast( - EC_READ_S16(domain_address)) / std::numeric_limits::max() * 10; - state_interface_ptr_->at(sii_ai_[index]) = data; - } - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 4; index++) { - if (paramters_.find("ai." + std::to_string(index + 1)) != paramters_.end()) { - if (paramters_.find( - "state_interface/" + paramters_["ai." + std::to_string(index + 1)]) != paramters_.end()) - { - sii_ai_[index] = std::stoi( - paramters_["state_interface/" + paramters_["ai." + std::to_string(index + 1)]]); - } - } - } - return true; - } - -private: - int sii_ai_[4] = {-1, -1, -1, -1}; - - ec_pdo_entry_info_t channels_[44] = { - {0x6000, 0x01, 1}, /* Underrange */ - {0x6000, 0x02, 1}, /* Overrange */ - {0x6000, 0x03, 2}, /* Limit 1 */ - {0x6000, 0x05, 2}, /* Limit 2 */ - {0x6000, 0x07, 1}, /* Error */ - {0x0000, 0x00, 1}, /* Gap */ - {0x0000, 0x00, 5}, /* Gap */ - {0x6000, 0x0e, 1}, /* Sync error */ - {0x6000, 0x0f, 1}, /* TxPDO State */ - {0x6000, 0x10, 1}, /* TxPDO Toggle */ - {0x6000, 0x11, 16}, /* Value */ - {0x6010, 0x01, 1}, /* Underrange */ - {0x6010, 0x02, 1}, /* Overrange */ - {0x6010, 0x03, 2}, /* Limit 1 */ - {0x6010, 0x05, 2}, /* Limit 2 */ - {0x6010, 0x07, 1}, /* Error */ - {0x0000, 0x00, 1}, /* Gap */ - {0x0000, 0x00, 5}, /* Gap */ - {0x6010, 0x0e, 1}, /* Sync error */ - {0x6010, 0x0f, 1}, /* TxPDO State */ - {0x6010, 0x10, 1}, /* TxPDO Toggle */ - {0x6010, 0x11, 16}, /* Value */ - {0x6020, 0x01, 1}, /* Underrange */ - {0x6020, 0x02, 1}, /* Overrange */ - {0x6020, 0x03, 2}, /* Limit 1 */ - {0x6020, 0x05, 2}, /* Limit 2 */ - {0x6020, 0x07, 1}, /* Error */ - {0x0000, 0x00, 1}, /* Gap */ - {0x0000, 0x00, 5}, /* Gap */ - {0x6020, 0x0e, 1}, /* Sync error */ - {0x6020, 0x0f, 1}, /* TxPDO State */ - {0x6020, 0x10, 1}, /* TxPDO Toggle */ - {0x6020, 0x11, 16}, /* Value */ - {0x6030, 0x01, 1}, /* Underrange */ - {0x6030, 0x02, 1}, /* Overrange */ - {0x6030, 0x03, 2}, /* Limit 1 */ - {0x6030, 0x05, 2}, /* Limit 2 */ - {0x6030, 0x07, 1}, /* Error */ - {0x0000, 0x00, 1}, /* Gap */ - {0x0000, 0x00, 5}, /* Gap */ - {0x6030, 0x0e, 1}, /* Sync error */ - {0x6030, 0x0f, 1}, /* TxPDO State */ - {0x6030, 0x10, 1}, /* TxPDO Toggle */ - {0x6030, 0x11, 16}, /* Value */ - }; - ec_pdo_info_t pdos_[4] = { - {0x1a00, 11, channels_ + 0}, /* AI TxPDO-Map Standard Ch.1 */ - {0x1a02, 11, channels_ + 11}, /* AI TxPDO-Map Standard Ch.2 */ - {0x1a04, 11, channels_ + 22}, /* AI TxPDO-Map Standard Ch.3 */ - {0x1a06, 11, channels_ + 33}, /* AI TxPDO-Map Standard Ch.4 */ - }; - ec_sync_info_t syncs_[2] = { - {3, EC_DIR_INPUT, 4, pdos_ + 0, EC_WD_ENABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {10, 21, 32, 43}} - }; -}; - -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Beckhoff_EL3102, ethercat_interface::EcSlave) -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Beckhoff_EL3104, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el41xx.cpp b/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el41xx.cpp deleted file mode 100644 index 95b39ae1..00000000 --- a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el41xx.cpp +++ /dev/null @@ -1,205 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class Beckhoff_EL4134 : public ethercat_interface::EcSlave -{ -public: - Beckhoff_EL4134() - : EcSlave(0x00000002, 0x10263052) - { - std::cerr << "The Beckhoff_EL4134 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Beckhoff_EL4134() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - if (cii_ao_[index] >= 0) { - double data = command_interface_ptr_->at(cii_ao_[index]); - data = std::isnan(data) ? 0 : data; - if (data > 10) {data = 10;} - if (data < -10) {data = -10;} - if (sii_ao_[index] >= 0) { - state_interface_ptr_->at(sii_ao_[index]) = data; - } - int16_t dac_data = static_cast( - data * static_cast(std::numeric_limits::max()) / 10); - EC_WRITE_S16(domain_address, dac_data); - } - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 4; index++) { - if (paramters_.find("ao." + std::to_string(index + 1)) != paramters_.end()) { - if (paramters_.find( - "command_interface/" + paramters_[ - "ao." + std::to_string(index + 1)]) != paramters_.end()) - { - cii_ao_[index] = std::stoi( - paramters_["command_interface/" + paramters_["ao." + std::to_string(index + 1)]]); - } - if (paramters_.find( - "state_interface/" + paramters_["ao." + std::to_string(index + 1)]) != paramters_.end()) - { - sii_ao_[index] = std::stoi( - paramters_["state_interface/" + paramters_["ao." + std::to_string(index + 1)]]); - } - } - } - return true; - } - -private: - int cii_ao_[4] = {-1, -1, -1, -1}; - int sii_ao_[4] = {-1, -1, -1, -1}; - - ec_pdo_entry_info_t channels_[4] = { - {0x7000, 0x01, 16}, /* Analog output */ - {0x7010, 0x01, 16}, /* Analog output */ - {0x7020, 0x01, 16}, /* Analog output */ - {0x7030, 0x01, 16}, /* Analog output */ - }; - ec_pdo_info_t pdos_[4] = { - {0x1600, 1, channels_ + 0}, /* AO RxPDO-Map OutputsCh.1 */ - {0x1601, 1, channels_ + 1}, /* AO RxPDO-Map OutputsCh.2 */ - {0x1602, 1, channels_ + 2}, /* AO RxPDO-Map OutputsCh.3 */ - {0x1603, 1, channels_ + 3}, /* AO RxPDO-Map OutputsCh.4 */ - }; - ec_sync_info_t syncs_[2] = { - {2, EC_DIR_OUTPUT, 4, pdos_ + 0, EC_WD_ENABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {0, 1, 2, 3}} - }; -}; -//--------------------------------------------------------------------------------------------------------- -class Beckhoff_EL4132 : public ethercat_interface::EcSlave -{ -public: - Beckhoff_EL4132() - : EcSlave(0x00000002, 0x10243052) - { - std::cerr << "The Beckhoff_EL4132 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Beckhoff_EL4132() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - if (cii_ao_[index] >= 0) { - double data = command_interface_ptr_->at(cii_ao_[index]); - data = std::isnan(data) ? 0 : data; - if (data > 10) {data = 10;} - if (data < -10) {data = -10;} - if (sii_ao_[index] >= 0) { - state_interface_ptr_->at(sii_ao_[index]) = data; - } - int16_t dac_data = static_cast( - data * static_cast(std::numeric_limits::max()) / 10); - EC_WRITE_S16(domain_address, dac_data); - } - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 2; index++) { - if (paramters_.find("ao." + std::to_string(index + 1)) != paramters_.end()) { - if (paramters_.find( - "command_interface/" + paramters_["ao." + std::to_string(index + 1)]) != - paramters_.end()) - { - cii_ao_[index] = std::stoi( - paramters_["command_interface/" + paramters_["ao." + std::to_string(index + 1)]]); - } - if (paramters_.find( - "state_interface/" + paramters_["ao." + std::to_string(index + 1)]) != paramters_.end()) - { - sii_ao_[index] = std::stoi( - paramters_["state_interface/" + paramters_["ao." + std::to_string(index + 1)]]); - } - } - } - return true; - } - -private: - int cii_ao_[2] = {-1, -1}; - int sii_ao_[2] = {-1, -1}; - - ec_pdo_entry_info_t channels_[2] = { - {0x3001, 0x01, 16}, /* Analog Output */ - {0x3002, 0x01, 16}, /* Analog Output */ - }; - ec_pdo_info_t pdos_[2] = { - {0x1600, 1, channels_ + 0}, /* AO RxPDO-Map OutputsCh.1 */ - {0x1601, 1, channels_ + 1}, /* AO RxPDO-Map OutputsCh.2 */ - }; - ec_sync_info_t syncs_[2] = { - {2, EC_DIR_OUTPUT, 2, pdos_ + 0, EC_WD_ENABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {0, 1}} - }; -}; - -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Beckhoff_EL4132, ethercat_interface::EcSlave) -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Beckhoff_EL4134, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el5101.cpp b/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el5101.cpp deleted file mode 100644 index 1839d9e2..00000000 --- a/ethercat_plugins/ethercat_beckhoff_modules/src/beckhoff_el5101.cpp +++ /dev/null @@ -1,288 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -#define NULL_CMD ((uint16_t) 0x00) -#define WRITE_REQUEST ((uint16_t) 0x01) - -namespace ethercat_plugins -{ - -class Beckhoff_EL5101 : public ethercat_interface::EcSlave -{ -public: - Beckhoff_EL5101() - : EcSlave(0x00000002, 0x13ed3052) - { - std::cerr << "The Beckhoff_EL5101 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Beckhoff_EL5101() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - switch (index) { - case 0: - control_word_ = EC_READ_U8(domain_address); - control_word_ = transition(state_, control_word_); - if (cii_reset_ >= 0) { - reset_command_ = command_interface_ptr_->at(cii_reset_); - if (sii_reset_ >= 0) { - state_interface_ptr_->at(sii_reset_) = reset_command_; - } - if (reset_command_ == 1.0 && last_reset_command_ != 1.0) { - cmd_ = CMD_WRITE_REQUEST; - } - last_reset_command_ = reset_command_; - } - control_word_ = process_cmd(state_, cmd_, control_word_); - last_control_word_ = control_word_; - EC_WRITE_U8(domain_address, control_word_); - break; - case 1: - if (write_t) { - EC_WRITE_S32(domain_address, target_position_); - write_t = false; - } - break; - case 2: - status_word_ = EC_READ_U16(domain_address); - break; - case 3: - position_ = EC_READ_S32(domain_address); - if (sii_position_ >= 0) { - state_interface_ptr_->at(sii_position_) = - static_cast(position_) * conversion_factor_; - } - break; - case 4: - latch_ = EC_READ_S32(domain_address); - break; - default: - break; - } - - if (index == 4) { - if (static_cast(status_word_) != static_cast(last_status_word_)) { - state_ = deviceState(status_word_); - } - last_status_word_ = status_word_; - } - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - if (paramters_.find("conversion_factor") != paramters_.end()) { - conversion_factor_ = std::stod(paramters_["conversion_factor"]); - } - if (paramters_.find("encoder_reset") != paramters_.end()) { - if (paramters_.find( - "command_interface/" + paramters_["encoder_reset"]) != paramters_.end()) - { - cii_reset_ = std::stoi( - paramters_["command_interface/" + paramters_["encoder_reset"]]); - } - if (paramters_.find( - "state_interface/" + paramters_["encoder_reset"]) != paramters_.end()) - { - sii_reset_ = std::stoi( - paramters_["state_interface/" + paramters_["encoder_reset"]]); - } - } - if (paramters_.find("encoder_position") != paramters_.end()) { - if (paramters_.find( - "state_interface/" + paramters_["encoder_position"]) != paramters_.end()) - { - sii_position_ = std::stoi( - paramters_["state_interface/" + paramters_["encoder_position"]]); - } - } - return true; - } - -private: - int32_t target_position_ = 0; // write - uint8_t control_word_ = 0; // write - uint32_t latch_ = 0; // read - int32_t position_ = 0; // read - uint16_t status_word_ = 0; // read - uint16_t last_status_word_ = -1; - uint8_t last_control_word_ = -1; - double last_reset_command_ = 0.0; - double reset_command_ = 0.0; - bool write_ = false; // write - bool write_t = false; // write - bool error_ = false; // read - uint8_t cmd_ = 0; - uint8_t error_type_ = 0; // read - double conversion_factor_ = 1; - int cii_reset_ = -1; - int sii_reset_ = -1; - int sii_position_ = -1; - - - ec_pdo_entry_info_t channels_[25] = { - {0x7010, 0x01, 1}, - {0x7010, 0x02, 1}, - {0x7010, 0x03, 1}, - {0x7010, 0x04, 1}, - {0x0000, 0x00, 4}, // Gap - {0x0000, 0x00, 8}, // Gap - {0x7010, 0x11, 32}, - {0x6010, 0x01, 1}, - {0x6010, 0x02, 1}, - {0x6010, 0x03, 1}, - {0x6010, 0x04, 1}, - {0x6010, 0x05, 1}, - {0x6010, 0x06, 1}, - {0x6010, 0x07, 1}, - {0x6010, 0x08, 1}, - {0x6010, 0x09, 1}, - {0x6010, 0x0a, 1}, - {0x6010, 0x0b, 1}, - {0x6010, 0x0c, 1}, - {0x6010, 0x0d, 1}, - {0x1c32, 0x20, 1}, - {0x1804, 0x07, 1}, - {0x1804, 0x09, 1}, - {0x6010, 0x11, 32}, - {0x6010, 0x12, 32}, - }; - ec_pdo_info_t pdos_[2] = { - {0x1603, 7, channels_ + 0}, - {0x1a04, 18, channels_ + 7}, - }; - ec_sync_info_t syncs_[5] = { - {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, - {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, - {2, EC_DIR_OUTPUT, 1, pdos_ + 0, EC_WD_DISABLE}, - {3, EC_DIR_INPUT, 1, pdos_ + 1, EC_WD_DISABLE}, - {0xff} - }; - - DomainMap domains_ = { - {0, {0, 6, 7, 23, 24}} - }; - - enum DeviceState - { - STATE_UNDEFINED = 0, - STATE_READY_TO_WRITE = 1, - STATE_NOTREADY_TO_WRITE = 2, - STATE_WRITE = 2, - STATE_FAULT = 3 - }; - - enum CmdOfOperation - { - CMD_NO_CMD = NULL_CMD, - CMD_WRITE_REQUEST = WRITE_REQUEST, - }; - - // returns the control word that will take device from state to next desired state - uint8_t transition(DeviceState state, uint8_t control_word) - { - switch (state) { - case STATE_READY_TO_WRITE: - if (((control_word >> 2) & 1) && !write_) { - control_word = (control_word & 0xfb); - } - return control_word; - default: - break; - } - return control_word; - } - - DeviceState deviceState(uint16_t status_word) - { - if (((status_word ) & 0b01000000) == 0b01000000) { - error_ = true; - error_type_ = 3; - return STATE_FAULT; - } - if ((((status_word ) & 0b00100100) == 0b00100000) ) { - return STATE_READY_TO_WRITE; - } - if (((status_word ) & 0b00100100) == 0b00100100) { - write_ = false; - return STATE_NOTREADY_TO_WRITE; - } - return STATE_UNDEFINED; - } - - uint16_t process_cmd(DeviceState state, uint8_t cmd, uint8_t control_word) - { - if (cmd > CMD_NO_CMD) { - switch (state) { - case STATE_READY_TO_WRITE: - switch (cmd) { - case CMD_WRITE_REQUEST: - if ((control_word >> 2) & 1) { - control_word = (control_word & 0xfb); - break; - } - control_word = (control_word | 0x04); - write_t = true; - write_ = true; - cmd_ = CMD_NO_CMD; - break; - default: - cmd_ = CMD_NO_CMD; - } - break; - case STATE_NOTREADY_TO_WRITE: - switch (cmd) { - case CMD_WRITE_REQUEST: - if ((control_word >> 2) & 1) { - control_word = (control_word & 0xfb); - } - break; - default: - cmd_ = CMD_NO_CMD; - } - break; - default: - cmd_ = CMD_NO_CMD; - break; - } - } - return control_word; - } - - DeviceState state_ = STATE_NOTREADY_TO_WRITE; -}; -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Beckhoff_EL5101, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_beckhoff_modules/test/test_load_ec_modules.cpp b/ethercat_plugins/ethercat_beckhoff_modules/test/test_load_ec_modules.cpp deleted file mode 100644 index 3a2bc8f5..00000000 --- a/ethercat_plugins/ethercat_beckhoff_modules/test/test_load_ec_modules.cpp +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright 2023 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include "ethercat_interface/ec_slave.hpp" - -TEST(TestLoadBeckhoff_EK1100, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Beckhoff_EK1100")); -} - -TEST(TestLoadBeckhoff_EL1008, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Beckhoff_EL1008")); -} - -TEST(TestLoadBeckhoff_EL1018, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Beckhoff_EL1018")); -} - -TEST(TestLoadBeckhoff_EL2008, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Beckhoff_EL2008")); -} - -TEST(TestLoadBeckhoff_EL2088, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Beckhoff_EL2088")); -} - -TEST(TestLoadBeckhoff_EL2124, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Beckhoff_EL2124")); -} - -TEST(TestLoadBeckhoff_EL3102, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Beckhoff_EL3102")); -} - -TEST(TestLoadBeckhoff_EL3104, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Beckhoff_EL3104")); -} - -TEST(TestLoadBeckhoff_EL4132, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Beckhoff_EL4132")); -} - -TEST(TestLoadBeckhoff_EL4134, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Beckhoff_EL4134")); -} - -TEST(TestLoadBeckhoff_EL5101, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Beckhoff_EL5101")); -} diff --git a/ethercat_plugins/ethercat_maxon_drives/CMakeLists.txt b/ethercat_plugins/ethercat_maxon_drives/CMakeLists.txt deleted file mode 100644 index 28e835a4..00000000 --- a/ethercat_plugins/ethercat_maxon_drives/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ethercat_maxon_drives) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(ethercat_interface REQUIRED) -find_package(pluginlib REQUIRED) - -file(GLOB_RECURSE PLUGINS_SRC src/*.cpp) -add_library(${PROJECT_NAME} ${PLUGINS_SRC}) -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) -ament_target_dependencies( - ${PROJECT_NAME} - "ethercat_interface" - "pluginlib" -) -pluginlib_export_plugin_description_file(ethercat_interface ethercat_plugins.xml) -install( - DIRECTORY include/ - DESTINATION include -) -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(pluginlib REQUIRED) - find_package(ethercat_interface REQUIRED) - - ament_lint_auto_find_test_dependencies() - - ament_add_gmock( - test_load_maxon_plugins - test/test_load_ec_modules.cpp - ) - target_include_directories(test_load_maxon_plugins PRIVATE include) - ament_target_dependencies(test_load_maxon_plugins - pluginlib - ethercat_interface - ) -endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_targets( - export_${PROJECT_NAME} -) -ament_package() diff --git a/ethercat_plugins/ethercat_maxon_drives/ethercat_plugins.xml b/ethercat_plugins/ethercat_maxon_drives/ethercat_plugins.xml deleted file mode 100644 index 4a56ddc4..00000000 --- a/ethercat_plugins/ethercat_maxon_drives/ethercat_plugins.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - Maxon EPOS3 motor driver. - - diff --git a/ethercat_plugins/ethercat_maxon_drives/include/ethercat_maxon_drives/maxon_defs.hpp b/ethercat_plugins/ethercat_maxon_drives/include/ethercat_maxon_drives/maxon_defs.hpp deleted file mode 100644 index 81d9f774..00000000 --- a/ethercat_plugins/ethercat_maxon_drives/include/ethercat_maxon_drives/maxon_defs.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2023 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ETHERCAT_MAXON_DRIVES__MAXON_DEFS_HPP_ -#define ETHERCAT_MAXON_DRIVES__MAXON_DEFS_HPP_ - -#include -#include - -enum DeviceState -{ - STATE_UNDEFINED = 0, - STATE_START = 1, - STATE_NOT_READY_TO_SWITCH_ON, - STATE_SWITCH_ON_DISABLED, - STATE_READY_TO_SWITCH_ON, - STATE_SWITCH_ON, - STATE_OPERATION_ENABLED, - STATE_QUICK_STOP_ACTIVE, - STATE_FAULT_REACTION_ACTIVE, - STATE_FAULT -}; - -enum ModeOfOperation -{ - MODE_NO_MODE = 0, - MODE_PROFILED_POSITION = 1, - MODE_PROFILED_VELOCITY = 3, - MODE_PROFILED_TORQUE = 4, - MODE_HOMING = 6, - MODE_INTERPOLATED_POSITION = 7, - MODE_CYCLIC_SYNC_POSITION = 8, - MODE_CYCLIC_SYNC_VELOCITY = 9, - MODE_CYCLIC_SYNC_TORQUE = 10 -}; - -const std::map DEVICE_STATE_STR = { - {STATE_START, "Start"}, - {STATE_NOT_READY_TO_SWITCH_ON, "Not Ready to Switch On"}, - {STATE_SWITCH_ON_DISABLED, "Switch on Disabled"}, - {STATE_READY_TO_SWITCH_ON, "Ready to Switch On"}, - {STATE_SWITCH_ON, "Switch On"}, - {STATE_OPERATION_ENABLED, "Operation Enabled"}, - {STATE_QUICK_STOP_ACTIVE, "Quick Stop Active"}, - {STATE_FAULT_REACTION_ACTIVE, "Fault Reaction Active"}, - {STATE_FAULT, "Fault"} -}; - -#endif // ETHERCAT_MAXON_DRIVES__MAXON_DEFS_HPP_ diff --git a/ethercat_plugins/ethercat_maxon_drives/package.xml b/ethercat_plugins/ethercat_maxon_drives/package.xml deleted file mode 100644 index 888ba9c1..00000000 --- a/ethercat_plugins/ethercat_maxon_drives/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - ethercat_maxon_drives - 1.2.0 - Plugin implementations of EtherCAT Maxon motor drive modules - Maciej Bednarczyk - Apache-2.0 - - ament_cmake_ros - - ethercat_interface - pluginlib - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/ethercat_plugins/ethercat_maxon_drives/src/maxon_epos3.cpp b/ethercat_plugins/ethercat_maxon_drives/src/maxon_epos3.cpp deleted file mode 100644 index 00197f35..00000000 --- a/ethercat_plugins/ethercat_maxon_drives/src/maxon_epos3.cpp +++ /dev/null @@ -1,342 +0,0 @@ -// Copyright 2022 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" -#include "ethercat_maxon_drives/maxon_defs.hpp" - -namespace ethercat_plugins -{ - -class Maxon_EPOS3 : public ethercat_interface::EcSlave -{ -public: - Maxon_EPOS3() - : EcSlave(0x000000fb, 0x64400000) - { - std::cerr << "The Maxon_EPOS3 plugin is depreciated and will be removed in the future." - << "Use the EcCiA402Drive plugin instead." << std::endl; - } - virtual ~Maxon_EPOS3() {} - /** Returns true if Epos3 has reached "operation enabled" state. - * The transition through the state machine is handled automatically. */ - bool initialized() const {return initialized_;} - virtual int assign_activate_dc_sync() {return 0x300;} - - virtual void processData(size_t index, uint8_t * domain_address) - { - // DATA READ WRITE - switch (index) { - case 0: - control_word_ = EC_READ_U16(domain_address); - // initialization sequence - control_word_ = transition(state_, control_word_); - EC_WRITE_U16(domain_address, control_word_); - break; - case 1: - if (isTargetPositionRequired && ( - mode_of_operation_display_ == MODE_CYCLIC_SYNC_POSITION || - mode_of_operation_display_ == MODE_PROFILED_POSITION || - mode_of_operation_display_ == MODE_INTERPOLATED_POSITION )) - { - target_position_ = command_interface_ptr_->at(cii_target_position); - } - EC_WRITE_S32(domain_address, target_position_); - break; - case 2: - if (isTargetVelocityRequired && ( - mode_of_operation_display_ == MODE_CYCLIC_SYNC_VELOCITY || - mode_of_operation_display_ == MODE_PROFILED_VELOCITY )) - { - target_velocity_ = command_interface_ptr_->at(cii_target_velocity); - } - EC_WRITE_S32(domain_address, target_velocity_); - break; - case 3: - if (isTargetTorqueRequired && ( - mode_of_operation_display_ == MODE_CYCLIC_SYNC_TORQUE || - mode_of_operation_display_ == MODE_PROFILED_TORQUE )) - { - target_torque_ = command_interface_ptr_->at(cii_target_torque); - } - EC_WRITE_S16(domain_address, target_torque_); - break; - case 4: - EC_WRITE_S32(domain_address, position_offset_); - break; - case 5: - EC_WRITE_S32(domain_address, velocity_offset_); - break; - case 6: - EC_WRITE_S16(domain_address, torque_offset_); - break; - case 7: - EC_WRITE_S8(domain_address, mode_of_operation_); - break; - case 8: - EC_WRITE_U16(domain_address, dig_input_function_); - break; - case 9: - EC_WRITE_U16(domain_address, touch_probe_function_); - break; - case 10: - status_word_ = EC_READ_U16(domain_address); - state_ = deviceState(status_word_); - break; - case 11: - position_ = EC_READ_S32(domain_address); - if (isPositionRequired) { - state_interface_ptr_->at(sii_position) = position_; - } - break; - case 12: - velocity_ = EC_READ_S32(domain_address); - if (isVelocityRequired) { - state_interface_ptr_->at(sii_velocity) = velocity_; - } - break; - case 13: - torque_ = EC_READ_S16(domain_address); - if (isTorqueRequired) { - state_interface_ptr_->at(sii_torque) = torque_; - } - break; - case 14: - mode_of_operation_display_ = EC_READ_S8(domain_address); - break; - case 15: - break; - case 16: - break; - case 17: - break; - case 18: - break; - default: - std::cout << "WARNING. Epos3 pdo index = " << index << " out of range." << std::endl; - } - // CHECK FOR STATE CHANGE - if (index == 18) { // if last entry in domain - if (status_word_ != last_status_word_) { - state_ = deviceState(status_word_); - // status word change does not necessarily mean state change - // http://ftp.beckhoff.com/download/document/motion/ax2x00_can_manual_en.pdf - // std::bitset<16> temp(status_word_); - // std::cout << "STATUS WORD: " << temp << std::endl; - if (state_ != last_state_) { - std::cout << "STATE: " << DEVICE_STATE_STR.at(state_) - << " with status word :" << status_word_ << std::endl; - } - } - if ((state_ == STATE_OPERATION_ENABLED) && - (last_state_ == STATE_OPERATION_ENABLED)) - { - initialized_ = true; - } else { - initialized_ = false; - } - last_status_word_ = status_word_; - last_state_ = state_; - } - } - - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - if (paramters_.find("mode_of_operation") != paramters_.end()) { - mode_of_operation_ = std::stod(paramters_["mode_of_operation"]); - } - - isPositionRequired = paramters_.find("state_interface/position") != paramters_.end(); - isVelocityRequired = paramters_.find("state_interface/velocity") != paramters_.end(); - isTorqueRequired = paramters_.find("state_interface/effort") != paramters_.end(); - - isTargetPositionRequired = paramters_.find("command_interface/position") != paramters_.end(); - isTargetVelocityRequired = paramters_.find("command_interface/velocity") != paramters_.end(); - isTargetTorqueRequired = paramters_.find("command_interface/effort") != paramters_.end(); - - if (isPositionRequired) { - sii_position = std::stoi(paramters_["state_interface/position"]); - } - if (isVelocityRequired) { - sii_velocity = std::stoi(paramters_["state_interface/velocity"]); - } - if (isTorqueRequired) { - sii_torque = std::stoi(paramters_["state_interface/effort"]); - } - if (isTargetPositionRequired) { - cii_target_position = std::stoi(paramters_["command_interface/position"]); - } - if (isTargetVelocityRequired) { - cii_target_velocity = std::stoi(paramters_["command_interface/velocity"]); - } - if (isTargetTorqueRequired) { - cii_target_torque = std::stoi(paramters_["command_interface/effort"]); - } - - return true; - } - - int32_t target_position_ = 0; // write - int32_t target_velocity_ = 0; // write - int16_t target_torque_ = 0; // write (max torque (max current) = 1000) - uint16_t control_word_ = 0; // write - int8_t mode_of_operation_ = 0; // write (use enum ModeOfOperation for convenience) - int16_t torque_offset_ = 0; // write - int32_t position_offset_ = 0; // write - int32_t velocity_offset_ = 0; // write - uint16_t touch_probe_function_ = 0; // write - uint16_t dig_input_function_ = 0; // write - - int32_t velocity_ = 0; // read - int32_t position_ = 0; // read - int16_t torque_ = 0; // read - uint16_t status_word_ = 0; // read - int8_t mode_of_operation_display_ = 0; // read - - bool isPositionRequired = false; - bool isVelocityRequired = false; - bool isTorqueRequired = false; - - bool isTargetPositionRequired = false; - bool isTargetVelocityRequired = false; - bool isTargetTorqueRequired = false; - - int sii_position; - int sii_velocity; - int sii_torque; - - int cii_target_position; - int cii_target_velocity; - int cii_target_torque; - -private: - uint32_t digital_output_ = 0; // write - uint32_t digital_input_ = 0; // read (must be enabled in Epos3 Motion Studio) - ec_pdo_entry_info_t channels_[19] = { - {0x6040, 0x00, 16}, /* 0 Control word */ - {0x607a, 0x00, 32}, /* 1 Target Position */ - {0x60ff, 0x00, 32}, /* 2 Target Velocity */ - {0x6071, 0x00, 16}, /* 3 Target Torque */ - {0x60b0, 0x00, 32}, /* 4 Position Offset */ - {0x60b1, 0x00, 32}, /* 5 Velocity Offset */ - {0x60b2, 0x00, 16}, /* 6 Torque Offset */ - {0x6060, 0x00, 8}, /* 7 Modes of Operation */ - {0x2078, 0x01, 16}, /* 8 Digital Output Functionalities */ - {0x60b8, 0x00, 16}, /* 9 Touch Probe Function */ - {0x6041, 0x00, 16}, /* 10 Status word */ - {0x6064, 0x00, 32}, /* 11 Position Actual Value */ - {0x606c, 0x00, 32}, /* 12 Velocity Actual Value */ - {0x6077, 0x00, 16}, /* 13 Torque Actual Value */ - {0x6061, 0x00, 8}, /* 14 Mode of operation display */ - {0x2071, 0x01, 16}, /* 15 Digital Input Functionalities State */ - {0x60b9, 0x00, 16}, /* 16 Touch Probe Status */ - {0x60ba, 0x00, 32}, /* 17 Touch Probe Position 1 Positive Value */ - {0x60bb, 0x00, 32}, /* 18 Touch Probe Position 1 Negative Value */ - }; - ec_pdo_info_t pdos_[2] = { - {0x1603, 10, channels_ + 0}, /* 4th receive PDO Mapping */ - {0x1a03, 9, channels_ + 10}, /* 4th transmit PDO Mapping */ - }; - ec_sync_info_t syncs_[5] = { - {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, - {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, - {2, EC_DIR_OUTPUT, 1, pdos_, EC_WD_ENABLE}, - {3, EC_DIR_INPUT, 1, pdos_ + 1, EC_WD_DISABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18}} - }; -//======================================================== -// Epos3 SPECIFIC -//======================================================== - - /** returns device state based upon the status_word */ - DeviceState deviceState(uint16_t status_word) - { - if ((status_word & 0b01001111) == 0b00000000) { - return STATE_NOT_READY_TO_SWITCH_ON; - } else if ((status_word & 0b01001111) == 0b01000000) { - return STATE_SWITCH_ON_DISABLED; - } else if ((status_word & 0b01101111) == 0b00100001) { - return STATE_READY_TO_SWITCH_ON; - } else if ((status_word & 0b01101111) == 0b00100011) { - return STATE_SWITCH_ON; - } else if ((status_word & 0b01101111) == 0b00100111) { - return STATE_OPERATION_ENABLED; - } else if ((status_word & 0b01101111) == 0b00000111) { - return STATE_QUICK_STOP_ACTIVE; - } else if ((status_word & 0b01001111) == 0b00001111) { - return STATE_FAULT_REACTION_ACTIVE; - } else if ((status_word & 0b01001111) == 0b00001000) { - return STATE_FAULT; - } - return STATE_UNDEFINED; - } - /** returns the control word that will take device from state to next desired state */ - uint16_t transition(DeviceState state, uint16_t control_word) - { - switch (state) { - case STATE_START: // -> STATE_NOT_READY_TO_SWITCH_ON (automatic) - return control_word; - case STATE_NOT_READY_TO_SWITCH_ON: // -> STATE_SWITCH_ON_DISABLED (automatic) - return control_word; - case STATE_SWITCH_ON_DISABLED: // -> STATE_READY_TO_SWITCH_ON - return (control_word & 0b01111110) | 0b00000110; - case STATE_READY_TO_SWITCH_ON: // -> STATE_SWITCH_ON - return (control_word & 0b01110111) | 0b00000111; - case STATE_SWITCH_ON: // -> STATE_OPERATION_ENABLED - return (control_word & 0b01111111) | 0b00001111; - case STATE_OPERATION_ENABLED: // -> GOOD - return control_word; - case STATE_QUICK_STOP_ACTIVE: // -> STATE_OPERATION_ENABLED - return (control_word & 0b01111111) | 0b00001111; - case STATE_FAULT_REACTION_ACTIVE: // -> STATE_FAULT (automatic) - return control_word; - case STATE_FAULT: // -> STATE_SWITCH_ON_DISABLED - return (control_word & 0b11111111) | 0b10000000; - default: - break; - } - return control_word; - } - int last_status_word_ = -1; - DeviceState last_state_ = STATE_START; - DeviceState state_ = STATE_START; - bool initialized_ = false; -}; -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Maxon_EPOS3, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_maxon_drives/test/test_load_ec_modules.cpp b/ethercat_plugins/ethercat_maxon_drives/test/test_load_ec_modules.cpp deleted file mode 100644 index fef60afa..00000000 --- a/ethercat_plugins/ethercat_maxon_drives/test/test_load_ec_modules.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include "ethercat_interface/ec_slave.hpp" - -TEST(TestLoadMaxon_EPOS3, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Maxon_EPOS3")); -} diff --git a/ethercat_plugins/ethercat_omron_modules/CMakeLists.txt b/ethercat_plugins/ethercat_omron_modules/CMakeLists.txt deleted file mode 100644 index 281aa246..00000000 --- a/ethercat_plugins/ethercat_omron_modules/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ethercat_omron_modules) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(ethercat_interface REQUIRED) -find_package(pluginlib REQUIRED) - -file(GLOB_RECURSE PLUGINS_SRC src/*.cpp) -add_library(${PROJECT_NAME} ${PLUGINS_SRC}) -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) -ament_target_dependencies( - ${PROJECT_NAME} - "ethercat_interface" - "pluginlib" -) -pluginlib_export_plugin_description_file(ethercat_interface ethercat_plugins.xml) -# install( -# DIRECTORY include/ -# DESTINATION include -# ) -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(pluginlib REQUIRED) - find_package(ethercat_interface REQUIRED) - - ament_lint_auto_find_test_dependencies() - - ament_add_gmock( - test_load_omron_plugins - test/test_load_ec_modules.cpp - ) - target_include_directories(test_load_omron_plugins PRIVATE include) - ament_target_dependencies(test_load_omron_plugins - pluginlib - ethercat_interface - ) -endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_targets( - export_${PROJECT_NAME} -) -ament_package() diff --git a/ethercat_plugins/ethercat_omron_modules/ethercat_plugins.xml b/ethercat_plugins/ethercat_omron_modules/ethercat_plugins.xml deleted file mode 100644 index f9479de7..00000000 --- a/ethercat_plugins/ethercat_omron_modules/ethercat_plugins.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - Omron NX ECC201 NX ID5442 - - - Omron NX ECC201 NX OD5256 - - - Omron NX ECC202 NX ID5142 1 - - - Omron NX ECC202 NX OD5256 - - diff --git a/ethercat_plugins/ethercat_omron_modules/package.xml b/ethercat_plugins/ethercat_omron_modules/package.xml deleted file mode 100644 index 78c63337..00000000 --- a/ethercat_plugins/ethercat_omron_modules/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - ethercat_omron_modules - 1.2.0 - Plugin implementations of EtherCAT Omron modules - Maciej Bednarczyk - Apache-2.0 - - ament_cmake_ros - - ethercat_interface - pluginlib - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/ethercat_plugins/ethercat_omron_modules/src/omron_nx_ecc201_nx_id5442.cpp b/ethercat_plugins/ethercat_omron_modules/src/omron_nx_ecc201_nx_id5442.cpp deleted file mode 100644 index 5014530f..00000000 --- a/ethercat_plugins/ethercat_omron_modules/src/omron_nx_ecc201_nx_id5442.cpp +++ /dev/null @@ -1,130 +0,0 @@ -// Copyright 2023 ICube-Robotics -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class Omron_NX_ECC201_NX_ID5442 : public ethercat_interface::EcSlave -{ -public: - Omron_NX_ECC201_NX_ID5442() - : EcSlave(0x00000083, 0x00000083) - { - std::cerr << - "The Omron_NX_ECC201_NX_ID5442 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Omron_NX_ECC201_NX_ID5442() {} - virtual void processData(size_t /*index*/, uint8_t * domain_address) - { - uint16_t data = 0; - data = EC_READ_U16(domain_address); - bool digital_inputs_[16]; - digital_inputs_[0] = ((data & 0b0000000000000001) != 0); // bit 0 - digital_inputs_[1] = ((data & 0b0000000000000010) != 0); // bit 1 - digital_inputs_[2] = ((data & 0b0000000000000100) != 0); // bit 2 - digital_inputs_[3] = ((data & 0b0000000000001000) != 0); // bit 3 - digital_inputs_[4] = ((data & 0b0000000000010000) != 0); // bit 4 - digital_inputs_[5] = ((data & 0b0000000000100000) != 0); // bit 5 - digital_inputs_[6] = ((data & 0b0000000001000000) != 0); // bit 6 - digital_inputs_[7] = ((data & 0b0000000010000000) != 0); // bit 7 - digital_inputs_[8] = ((data & 0b0000000100000000) != 0); // bit 8 - digital_inputs_[9] = ((data & 0b0000001000000000) != 0); // bit 9 - digital_inputs_[10] = ((data & 0b0000010000000000) != 0); // bit 10 - digital_inputs_[11] = ((data & 0b0000100000000000) != 0); // bit 11 - digital_inputs_[12] = ((data & 0b0001000000000000) != 0); // bit 12 - digital_inputs_[13] = ((data & 0b0010000000000000) != 0); // bit 13 - digital_inputs_[14] = ((data & 0b0100000000000000) != 0); // bit 14 - digital_inputs_[15] = ((data & 0b1000000000000000) != 0); // bit 15 - - for (auto i = 0ul; i < 16; i++) { - if (sii_di_[i] >= 0) { - state_interface_ptr_->at(sii_di_[i]) = digital_inputs_[i]; - } - } - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0; index < 16; index++) { - if (paramters_.find("di." + std::to_string(index)) != paramters_.end()) { - if (paramters_.find( - "state_interface/" + paramters_["di." + std::to_string(index)]) != paramters_.end()) - { - sii_di_[index] = std::stoi( - paramters_["state_interface/" + paramters_["di." + std::to_string(index)]]); - } - } - } - return true; - } - -private: - std::vector sii_di_{std::vector(16, -1)}; - // digital write values - std::vector write_data_{std::vector(16, false)}; - - ec_pdo_entry_info_t channels_[6] = { - {0x7022, 0x01, 16}, - {0x3003, 0x04, 128}, - {0x3006, 0x04, 128}, - {0x2002, 0x01, 8}, - {0x0000, 0x00, 8}, /* Gap */ - {0x6002, 0x01, 16}, - }; - ec_pdo_info_t pdos_[5] = { - {0x1604, 1, channels_ + 0}, - {0x1bf8, 2, channels_ + 1}, - {0x1bff, 1, channels_ + 3}, - {0x1bf4, 1, channels_ + 4}, - {0x1a00, 1, channels_ + 5}, - }; - ec_sync_info_t syncs_[5] = { - {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, - {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, - {2, EC_DIR_OUTPUT, 1, pdos_ + 0, EC_WD_DISABLE}, - {3, EC_DIR_INPUT, 4, pdos_ + 1, EC_WD_DISABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {5}} - }; -}; - -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Omron_NX_ECC201_NX_ID5442, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_omron_modules/src/omron_nx_ecc201_nx_od5256.cpp b/ethercat_plugins/ethercat_omron_modules/src/omron_nx_ecc201_nx_od5256.cpp deleted file mode 100644 index 986664a3..00000000 --- a/ethercat_plugins/ethercat_omron_modules/src/omron_nx_ecc201_nx_od5256.cpp +++ /dev/null @@ -1,134 +0,0 @@ -// Copyright 2023 ICube-Robotics -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class Omron_NX_ECC201_NX_OD5256 : public ethercat_interface::EcSlave -{ -public: - Omron_NX_ECC201_NX_OD5256() - : EcSlave(0x00000083, 0x00000083) - { - std::cerr << - "The Omron_NX_ECC201_NX_OD5256 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Omron_NX_ECC201_NX_OD5256() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - uint16_t digital_out_; - digital_out_ = 0; - for (auto i = 0ul; i < 16; i++) { - if (cii_do_[i] >= 0) { - if (!std::isnan(command_interface_ptr_->at(cii_do_[i]))) { - write_data_[i] = (command_interface_ptr_->at(cii_do_[i])) ? true : false; - if (sii_do_[i] >= 0) { - state_interface_ptr_->at(sii_do_[i]) = command_interface_ptr_->at(cii_do_[i]); - } - } - } - } - - // bit masking to get individual input values - digital_out_ += ( write_data_[0] << 0 ); // bit 0 - digital_out_ += ( write_data_[1] << 1 ); // bit 1 - digital_out_ += ( write_data_[2] << 2 ); // bit 2 - digital_out_ += ( write_data_[3] << 3 ); // bit 3 - digital_out_ += ( write_data_[4] << 4 ); // bit 4 - digital_out_ += ( write_data_[5] << 5 ); // bit 5 - digital_out_ += ( write_data_[6] << 6 ); // bit 6 - digital_out_ += ( write_data_[7] << 7 ); // bit 7 - digital_out_ += ( write_data_[8] << 8 ); // bit 8 - digital_out_ += ( write_data_[9] << 9 ); // bit 9 - digital_out_ += ( write_data_[10] << 10 ); // bit 10 - digital_out_ += ( write_data_[11] << 11 ); // bit 11 - digital_out_ += ( write_data_[12] << 12 ); // bit 12 - digital_out_ += ( write_data_[13] << 13 ); // bit 13 - digital_out_ += ( write_data_[14] << 14 ); // bit 14 - digital_out_ += ( write_data_[15] << 15 ); // bit 15 - - EC_WRITE_U16(domain_address, digital_out_); - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 16; index++) { - if (paramters_.find("do." + std::to_string(index)) != paramters_.end()) { - if (paramters_.find( - "command_interface/" + paramters_["do." + std::to_string(index)]) != paramters_.end()) - { - cii_do_[index] = std::stoi( - paramters_["command_interface/" + paramters_["do." + std::to_string(index)]]); - } - if (paramters_.find( - "state_interface/" + paramters_["do." + std::to_string(index)]) != paramters_.end()) - { - sii_do_[index] = std::stoi( - paramters_["state_interface/" + paramters_["do." + std::to_string(index)]]); - } - } - } - return true; - } - -private: - int cii_do_[16] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; - int sii_do_[16] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; - // digital write values - bool write_data_[16] = {false, false, false, false, false, - false, false, false, false, false, false, false, false, false, false, false}; - - ec_pdo_entry_info_t channels_[1] = { - {0x7022, 0x01, 16}, /* Output */ - }; - ec_pdo_info_t pdos_[1] = { - {0x1604, 1, channels_ + 0}, /* Channel 1 */ - }; - ec_sync_info_t syncs_[2] = { - {2, EC_DIR_OUTPUT, 1, pdos_ + 0, EC_WD_DISABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {0}} - }; -}; - -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Omron_NX_ECC201_NX_OD5256, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_omron_modules/src/omron_nx_ecc202_nx_id5142_1.cpp b/ethercat_plugins/ethercat_omron_modules/src/omron_nx_ecc202_nx_id5142_1.cpp deleted file mode 100644 index 1a8614f8..00000000 --- a/ethercat_plugins/ethercat_omron_modules/src/omron_nx_ecc202_nx_id5142_1.cpp +++ /dev/null @@ -1,132 +0,0 @@ -// Copyright 2023 ICube-Robotics -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class Omron_NX_ECC202_NX_ID5142_1 : public ethercat_interface::EcSlave -{ -public: - Omron_NX_ECC202_NX_ID5142_1() - : EcSlave(0x00000083, 0x000000a6) - { - std::cerr << - "The Omron_NX_ECC202_NX_ID5142_1 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - // 0x01115142 for NX-ID5142-1 and 0x05115142 for NX-ID5142-5 - virtual ~Omron_NX_ECC202_NX_ID5142_1() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - uint16_t data = 0; - data = EC_READ_U16(domain_address); - digital_inputs_[0] = ((data & 0b0000000000000001) != 0); // bit 0 - digital_inputs_[1] = ((data & 0b0000000000000010) != 0); // bit 1 - digital_inputs_[2] = ((data & 0b0000000000000100) != 0); // bit 2 - digital_inputs_[3] = ((data & 0b0000000000001000) != 0); // bit 3 - digital_inputs_[4] = ((data & 0b0000000000010000) != 0); // bit 4 - digital_inputs_[5] = ((data & 0b0000000000100000) != 0); // bit 5 - digital_inputs_[6] = ((data & 0b0000000001000000) != 0); // bit 6 - digital_inputs_[7] = ((data & 0b0000000010000000) != 0); // bit 7 - digital_inputs_[8] = ((data & 0b0000000100000000) != 0); // bit 8 - digital_inputs_[9] = ((data & 0b0000001000000000) != 0); // bit 9 - digital_inputs_[10] = ((data & 0b0000010000000000) != 0); // bit 10 - digital_inputs_[11] = ((data & 0b0000100000000000) != 0); // bit 11 - digital_inputs_[12] = ((data & 0b0001000000000000) != 0); // bit 12 - digital_inputs_[13] = ((data & 0b0010000000000000) != 0); // bit 13 - digital_inputs_[14] = ((data & 0b0100000000000000) != 0); // bit 14 - digital_inputs_[15] = ((data & 0b1000000000000000) != 0); // bit 15 - - for (auto i = 0ul; i < 16; i++) { - if (sii_di_[i] >= 0) { - state_interface_ptr_->at(sii_di_[i]) = digital_inputs_[i]; - } - } - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 16; index++) { - if (paramters_.find( - "di." + std::to_string(index)) != paramters_.end()) - { - if (paramters_.find( - "state_interface/" + paramters_["di." + std::to_string(index)]) != paramters_.end()) - { - sii_di_[index] = std::stoi( - paramters_["state_interface/" + paramters_["di." + std::to_string(index)]]); - } - } - } - return true; - } - -private: - std::vector sii_di_{std::vector(16, -1)}; - // digital write values - std::vector write_data_ = {std::vector(16, false)}; - std::vector digital_inputs_ = {std::vector(16, false)}; - - ec_pdo_entry_info_t channels_[6] = { - {0x7022, 0x01, 16}, - {0x3003, 0x04, 128}, - {0x3006, 0x04, 128}, - {0x2002, 0x01, 8}, - {0x0000, 0x00, 8}, /* Gap */ - {0x6002, 0x01, 16}, - }; - ec_pdo_info_t pdos_[5] = { - {0x1604, 1, channels_ + 0}, - {0x1bf8, 2, channels_ + 1}, - {0x1bff, 1, channels_ + 3}, - {0x1bf4, 1, channels_ + 4}, - {0x1a00, 1, channels_ + 5}, - }; - ec_sync_info_t syncs_[5] = { - {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, - {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, - {2, EC_DIR_OUTPUT, 1, pdos_ + 0, EC_WD_DISABLE}, - {3, EC_DIR_INPUT, 4, pdos_ + 1, EC_WD_DISABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {5}} - }; -}; -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Omron_NX_ECC202_NX_ID5142_1, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_omron_modules/src/omron_nx_ecc202_nx_od5256.cpp b/ethercat_plugins/ethercat_omron_modules/src/omron_nx_ecc202_nx_od5256.cpp deleted file mode 100644 index 2da051bc..00000000 --- a/ethercat_plugins/ethercat_omron_modules/src/omron_nx_ecc202_nx_od5256.cpp +++ /dev/null @@ -1,133 +0,0 @@ -// Copyright 2023 ICube-Robotics -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" - -namespace ethercat_plugins -{ - -class Omron_NX_ECC202_NX_OD5256 : public ethercat_interface::EcSlave -{ -public: - Omron_NX_ECC202_NX_OD5256() - : EcSlave(0x00000083, 0x000000a6) - { - std::cerr << - "The Omron_NX_ECC202_NX_OD5256 plugin is depreciated and will be removed in the future." - << "Use the GenericEcSlave plugin instead." << std::endl; - } - virtual ~Omron_NX_ECC202_NX_OD5256() {} - virtual void processData(size_t index, uint8_t * domain_address) - { - uint16_t digital_out_; - digital_out_ = 0; - for (auto i = 0ul; i < 16; i++) { - if (cii_do_[i] >= 0) { - if (!std::isnan(command_interface_ptr_->at(cii_do_[i]))) { - write_data_[i] = (command_interface_ptr_->at(cii_do_[i])) ? true : false; - if (sii_do_[i] >= 0) { - state_interface_ptr_->at(sii_do_[i]) = command_interface_ptr_->at(cii_do_[i]); - } - } - } - } - - // bit masking to get individual input values - digital_out_ += ( write_data_[0] << 0 ); // bit 0 - digital_out_ += ( write_data_[1] << 1 ); // bit 1 - digital_out_ += ( write_data_[2] << 2 ); // bit 2 - digital_out_ += ( write_data_[3] << 3 ); // bit 3 - digital_out_ += ( write_data_[4] << 4 ); // bit 4 - digital_out_ += ( write_data_[5] << 5 ); // bit 5 - digital_out_ += ( write_data_[6] << 6 ); // bit 6 - digital_out_ += ( write_data_[7] << 7 ); // bit 7 - digital_out_ += ( write_data_[8] << 8 ); // bit 8 - digital_out_ += ( write_data_[9] << 9 ); // bit 9 - digital_out_ += ( write_data_[10] << 10 ); // bit 10 - digital_out_ += ( write_data_[11] << 11 ); // bit 11 - digital_out_ += ( write_data_[12] << 12 ); // bit 12 - digital_out_ += ( write_data_[13] << 13 ); // bit 13 - digital_out_ += ( write_data_[14] << 14 ); // bit 14 - digital_out_ += ( write_data_[15] << 15 ); // bit 15 - - EC_WRITE_U16(domain_address, digital_out_); - } - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return sizeof(syncs_) / sizeof(ec_sync_info_t); - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - for (auto index = 0ul; index < 16; index++) { - if (paramters_.find("do." + std::to_string(index)) != paramters_.end()) { - if (paramters_.find( - "command_interface/" + paramters_["do." + - std::to_string(index)]) != paramters_.end()) - { - cii_do_[index] = std::stoi( - paramters_["command_interface/" + paramters_["do." + std::to_string(index)]]); - } - if (paramters_.find( - "state_interface/" + paramters_["do." + - std::to_string(index)]) != paramters_.end()) - { - sii_do_[index] = std::stoi( - paramters_["state_interface/" + paramters_["do." + std::to_string(index)]]); - } - } - } - return true; - } - -private: - std::vector cii_do_{std::vector(16, -1)}; - std::vector sii_do_{std::vector(16, -1)}; - // digital write values - std::vector write_data_{std::vector(16, false)}; - - ec_pdo_entry_info_t channels_[1] = { - {0x7022, 0x01, 16}, /* Output */ - }; - ec_pdo_info_t pdos_[1] = { - {0x1604, 1, channels_ + 0}, /* Channel 1 */ - }; - ec_sync_info_t syncs_[2] = { - {2, EC_DIR_OUTPUT, 1, pdos_ + 0, EC_WD_DISABLE}, - {0xff} - }; - DomainMap domains_ = { - {0, {0}} - }; -}; -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Omron_NX_ECC202_NX_OD5256, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_omron_modules/test/test_load_ec_modules.cpp b/ethercat_plugins/ethercat_omron_modules/test/test_load_ec_modules.cpp deleted file mode 100644 index 6ee73a76..00000000 --- a/ethercat_plugins/ethercat_omron_modules/test/test_load_ec_modules.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2023 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include "ethercat_interface/ec_slave.hpp" - -TEST(TestLoadOmron_NX_ECC201_NX_ID5442, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Omron_NX_ECC201_NX_ID5442")); -} - -TEST(TestLoadOmron_NX_ECC201_NX_OD5256, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Omron_NX_ECC201_NX_OD5256")); -} - -TEST(TestLoadOmron_NX_ECC202_NX_ID5142_1, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Omron_NX_ECC202_NX_ID5142_1")); -} - -TEST(TestLoadOmron_NX_ECC202_NX_OD5256, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Omron_NX_ECC202_NX_OD5256")); -} diff --git a/ethercat_plugins/ethercat_schneider_drives/CMakeLists.txt b/ethercat_plugins/ethercat_schneider_drives/CMakeLists.txt deleted file mode 100644 index 4526cea1..00000000 --- a/ethercat_plugins/ethercat_schneider_drives/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(ethercat_schneider_drives) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) -find_package(ethercat_interface REQUIRED) -find_package(pluginlib REQUIRED) - -file(GLOB_RECURSE PLUGINS_SRC src/*.cpp) -add_library(${PROJECT_NAME} ${PLUGINS_SRC}) -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) -ament_target_dependencies( - ${PROJECT_NAME} - "ethercat_interface" - "pluginlib" -) -pluginlib_export_plugin_description_file(ethercat_interface ethercat_plugins.xml) -install( - DIRECTORY include/ - DESTINATION include -) -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(pluginlib REQUIRED) - find_package(ethercat_interface REQUIRED) - - ament_lint_auto_find_test_dependencies() - - ament_add_gmock( - test_load_schneider_plugins - test/test_load_ec_modules.cpp - ) - target_include_directories(test_load_schneider_plugins PRIVATE include) - ament_target_dependencies(test_load_schneider_plugins - pluginlib - ethercat_interface - ) -endif() -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_targets( - export_${PROJECT_NAME} -) -ament_package() diff --git a/ethercat_plugins/ethercat_schneider_drives/README.md b/ethercat_plugins/ethercat_schneider_drives/README.md deleted file mode 100644 index 67a7dee4..00000000 --- a/ethercat_plugins/ethercat_schneider_drives/README.md +++ /dev/null @@ -1,89 +0,0 @@ -# URDF, example -```xml - - - - - - - ethercat_driver/EthercatDriver - 0 - 100 - - - - - - ethercat_plugins/Schneider_ATV320 - 0 - 0 - - - - -``` - -# Controller example -```yaml -controller_manager: - ros__parameters: - update_rate: 100 # Hz - - forward_velocity_controller: - type: velocity_controllers/JointGroupVelocityController - -forward_velocity_controller: - ros__parameters: - joints: - - atv_320_test -``` - -# Launch file example -```python -from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution, Command, FindExecutable -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - velocity_controller_path = 'controller.yaml' - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name='xacro')]), - ' ', - 'velocity_ros2.urdf' - ] - ) - robot_description = {'robot_description': robot_description_content} - - ros2_control_node = Node( - package='controller_manager', - executable='ros2_control_node', - parameters=[robot_description, velocity_controller_path], - output='screen', - ) - - velocity_controller_node = Node( - package='controller_manager', - executable='spawner', - arguments=['forward_velocity_controller'], - output='screen', - ) - - # Create the launch description and populate - ld = LaunchDescription() - - # declared_arguments - - # nodes_to_start - ld.add_action(ros2_control_node) - ld.add_action(velocity_controller_node) - - return ld -``` - -# Setting velocity from command line -```bash -ros2 topic pub /forward_velocity_controller/commands std_msgs/msg/Float64MultiArray "data: [ 1500.0 ]" -``` diff --git a/ethercat_plugins/ethercat_schneider_drives/ethercat_plugins.xml b/ethercat_plugins/ethercat_schneider_drives/ethercat_plugins.xml deleted file mode 100644 index 9ed18fc1..00000000 --- a/ethercat_plugins/ethercat_schneider_drives/ethercat_plugins.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - Schneider Electric ATV320 - - diff --git a/ethercat_plugins/ethercat_schneider_drives/include/ethercat_schneider_drives/schneider_defs.hpp b/ethercat_plugins/ethercat_schneider_drives/include/ethercat_schneider_drives/schneider_defs.hpp deleted file mode 100644 index 0a283320..00000000 --- a/ethercat_plugins/ethercat_schneider_drives/include/ethercat_schneider_drives/schneider_defs.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2023 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ETHERCAT_SCHNEIDER_DRIVES__SCHNEIDER_DEFS_HPP_ -#define ETHERCAT_SCHNEIDER_DRIVES__SCHNEIDER_DEFS_HPP_ - -#include -#include - -enum DeviceState -{ - STATE_UNDEFINED = 0, - STATE_START = 1, - STATE_NOT_READY_TO_SWITCH_ON, - STATE_SWITCH_ON_DISABLED, - STATE_READY_TO_SWITCH_ON, - STATE_SWITCH_ON, - STATE_OPERATION_ENABLED, - STATE_QUICK_STOP_ACTIVE, - STATE_FAULT_REACTION_ACTIVE, - STATE_FAULT -}; - -enum ModeOfOperation -{ - MODE_NO_MODE = 0, - MODE_PROFILED_POSITION = 1, - MODE_PROFILED_VELOCITY = 3, - MODE_PROFILED_TORQUE = 4, - MODE_HOMING = 6, - MODE_INTERPOLATED_POSITION = 7, - MODE_CYCLIC_SYNC_POSITION = 8, - MODE_CYCLIC_SYNC_VELOCITY = 9, - MODE_CYCLIC_SYNC_TORQUE = 10 -}; - -const std::map DEVICE_STATE_STR = { - {STATE_START, "Start"}, - {STATE_NOT_READY_TO_SWITCH_ON, "Not Ready to Switch On"}, - {STATE_SWITCH_ON_DISABLED, "Switch on Disabled"}, - {STATE_READY_TO_SWITCH_ON, "Ready to Switch On"}, - {STATE_SWITCH_ON, "Switch On"}, - {STATE_OPERATION_ENABLED, "Operation Enabled"}, - {STATE_QUICK_STOP_ACTIVE, "Quick Stop Active"}, - {STATE_FAULT_REACTION_ACTIVE, "Fault Reaction Active"}, - {STATE_FAULT, "Fault"} -}; - -#endif // ETHERCAT_SCHNEIDER_DRIVES__SCHNEIDER_DEFS_HPP_ diff --git a/ethercat_plugins/ethercat_schneider_drives/package.xml b/ethercat_plugins/ethercat_schneider_drives/package.xml deleted file mode 100644 index 5ef2d428..00000000 --- a/ethercat_plugins/ethercat_schneider_drives/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - ethercat_schneider_drives - 1.2.0 - Plugin implementations of EtherCAT modules - Maciej Bednarczyk - Apache-2.0 - - ament_cmake_ros - - ethercat_interface - pluginlib - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/ethercat_plugins/ethercat_schneider_drives/src/schneider_atv320.cpp b/ethercat_plugins/ethercat_schneider_drives/src/schneider_atv320.cpp deleted file mode 100644 index 2cd3aa10..00000000 --- a/ethercat_plugins/ethercat_schneider_drives/src/schneider_atv320.cpp +++ /dev/null @@ -1,206 +0,0 @@ -// Copyright 2023 ICube-Robotics -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ethercat_interface/ec_slave.hpp" -#include "ethercat_schneider_drives/schneider_defs.hpp" - -namespace ethercat_plugins -{ - -class Schneider_ATV320 : public ethercat_interface::EcSlave -{ -public: - Schneider_ATV320() - : EcSlave(0x0800005a, 0x00000389) - { - std::cerr << "The Schneider_ATV320 plugin is depreciated and will be removed in the future." - << "Use the EcCiA402Drive plugin instead." << std::endl; - } - virtual ~Schneider_ATV320() {} - /* Returns true if atv320 has reached "operation enabled" state. */ - bool initialized() const {return state_ == STATE_OPERATION_ENABLED;} - virtual bool use_dc_sync() {return true;} - - virtual void processData(size_t index, uint8_t * domain_address) - { - switch (index) { - case 0: // Control word - control_word_ = EC_READ_U16(domain_address); - control_word_ = transition(state_, control_word_); - EC_WRITE_U16(domain_address, control_word_); - break; - case 1: // Target velocity - target_velocity_ = command_interface_ptr_->at(cii_target_velocity); - EC_WRITE_S16(domain_address, target_velocity_); - break; - case 2: // Status word - status_word_ = EC_READ_U16(domain_address); - state_ = deviceState(status_word_); - break; - case 3: // Current velocity - // Read - velocity_ = EC_READ_S16(domain_address); - state_interface_ptr_->at(sii_velocity) = velocity_; - break; - default: - throw std::runtime_error("ATV320 pdo out of range"); - } - } - - virtual const ec_sync_info_t * syncs() {return &syncs_[0];} - virtual size_t syncSize() - { - return 5; - } - virtual const ec_pdo_entry_info_t * channels() - { - return channels_; - } - virtual void domains(DomainMap & domains) const - { - domains = domains_; - } - - virtual bool setupSlave( - std::unordered_map slave_paramters, - std::vector * state_interface, - std::vector * command_interface) - { - state_interface_ptr_ = state_interface; - command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; - - sii_velocity = std::stoi(paramters_["state_interface/velocity"]); - cii_target_velocity = std::stoi(paramters_["command_interface/velocity"]); - - return true; - } - - // CIA - 402 Velocity mode parameters. - uint16_t control_word_ = 0; // write - uint16_t status_word_ = 0; // read - int sii_velocity; - int cii_target_velocity; - - int32_t target_velocity_ = 0; // write - int32_t velocity_ = 0; // read - -private: - ec_pdo_entry_info_t channels_[4] = { - {0x6040, 0x00, 16}, // 0 CMD - Control word - {0x6042, 0x00, 16}, // 1 LFR - Reference speed - {0x6041, 0x00, 16}, // 2 ETA - Status word - {0x6044, 0x00, 16}, // 3 RFR - Current speed - }; - - ec_pdo_info_t pdos_[2] = { - {0x1600, 2, channels_ + 0}, - {0x1a00, 2, channels_ + 2}, - }; - - ec_sync_info_t syncs_[5] = { - {0, EC_DIR_OUTPUT, 0}, - {1, EC_DIR_INPUT, 0}, - {2, EC_DIR_OUTPUT, 1, pdos_ + 0, EC_WD_ENABLE}, - {3, EC_DIR_INPUT, 1, pdos_ + 1, EC_WD_DISABLE}, - {0xff} - }; - - DomainMap domains_ = { - {0, {0, 1, 2, 3}} - }; -//======================================================== -// CIA402 - Specific -//======================================================== - enum DeviceState - { - STATE_NOT_READY_TO_SWITCH_ON, - STATE_SWITCH_ON_DISABLED, - STATE_READY_TO_SWITCH_ON, - STATE_SWITCH_ON, - STATE_OPERATION_ENABLED, - STATE_QUICK_STOP_ACTIVE, - STATE_FAULT_REACTION_ACTIVE, - STATE_FAULT - }; - std::map device_state_str_ = { - {STATE_NOT_READY_TO_SWITCH_ON, "Not Ready to Switch On"}, - {STATE_SWITCH_ON_DISABLED, "Switch on Disabled"}, - {STATE_READY_TO_SWITCH_ON, "Ready to Switch On"}, - {STATE_SWITCH_ON, "Switch On"}, - {STATE_OPERATION_ENABLED, "Operation Enabled"}, - {STATE_QUICK_STOP_ACTIVE, "Quick Stop Active"}, - {STATE_FAULT_REACTION_ACTIVE, "Fault Reaction Active"}, - {STATE_FAULT, "Fault"} - }; - - /** returns device state based upon the status_word */ - DeviceState deviceState(uint16_t status_word) - { - switch (status_word & 0b01001111) { - case 0b00000000: - return STATE_NOT_READY_TO_SWITCH_ON; - case 0b01000000: - return STATE_SWITCH_ON_DISABLED; - case 0b00001111: - return STATE_FAULT_REACTION_ACTIVE; - case 0b00001000: - return STATE_FAULT; - default: - switch (status_word & 0b01101111) { - case 0b00100001: - return STATE_READY_TO_SWITCH_ON; - case 0b00100011: - return STATE_SWITCH_ON; - case 0b00100111: - return STATE_OPERATION_ENABLED; - case 0b00000111: - return STATE_QUICK_STOP_ACTIVE; - default: - throw std::runtime_error("Undefined state hit ATV320"); - } - } - } - /** returns the control word that will take device from state to next desired state */ - uint16_t transition(DeviceState state, uint16_t control_word) - { - switch (state) { - case STATE_NOT_READY_TO_SWITCH_ON: // -> STATE_SWITCH_ON_DISABLED (automatic) - return control_word; - case STATE_SWITCH_ON_DISABLED: // -> STATE_READY_TO_SWITCH_ON - return (control_word & 0b01111110) | 0b00000110; - case STATE_READY_TO_SWITCH_ON: // -> STATE_SWITCH_ON - return (control_word & 0b01110111) | 0b00000111; - case STATE_SWITCH_ON: // -> STATE_OPERATION_ENABLED - return (control_word & 0b01111111) | 0b00001111; - case STATE_OPERATION_ENABLED: // -> GOOD - return control_word; - case STATE_QUICK_STOP_ACTIVE: // -> STATE_OPERATION_ENABLED - return (control_word & 0b01111111) | 0b00001111; - case STATE_FAULT_REACTION_ACTIVE: // -> STATE_FAULT (automatic) - return control_word; - case STATE_FAULT: // -> STATE_SWITCH_ON_DISABLED - return (control_word & 0b11111111) | 0b10000000; - default: - break; - } - return control_word; - } - DeviceState state_ = STATE_NOT_READY_TO_SWITCH_ON; -}; -} // namespace ethercat_plugins - -#include - -PLUGINLIB_EXPORT_CLASS(ethercat_plugins::Schneider_ATV320, ethercat_interface::EcSlave) diff --git a/ethercat_plugins/ethercat_schneider_drives/test/test_load_ec_modules.cpp b/ethercat_plugins/ethercat_schneider_drives/test/test_load_ec_modules.cpp deleted file mode 100644 index 61d7ba90..00000000 --- a/ethercat_plugins/ethercat_schneider_drives/test/test_load_ec_modules.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023 ICUBE Laboratory, University of Strasbourg -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include "ethercat_interface/ec_slave.hpp" - -TEST(TestLoadSchneider_ATV320, load_ec_module) -{ - pluginlib::ClassLoader ec_loader_{ - "ethercat_interface", "ethercat_interface::EcSlave"}; - ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_plugins/Schneider_ATV320")); -}