├── .gitignore ├── .travis.yml ├── LICENSE ├── README.md ├── epos_hardware ├── 90-ftd2xx.rules ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── epos_hardware │ │ ├── epos.h │ │ ├── epos_hardware.h │ │ ├── epos_manager.h │ │ └── utils.h ├── install_udev_rules ├── launch │ ├── example.launch │ ├── example.urdf │ └── example.yaml ├── package.xml └── src │ ├── nodes │ └── epos_hardware_node.cpp │ ├── tools │ ├── get_state.cpp │ └── list_devices.cpp │ └── util │ ├── epos.cpp │ ├── epos_hardware.cpp │ ├── epos_manager.cpp │ └── utils.cpp └── epos_library ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── include └── epos_library │ └── Definitions.h ├── lib ├── arm │ ├── hf │ │ ├── libEposCmd.so.5.0.1.0 │ │ └── libftd2xx.so.1.2.8 │ ├── rpi │ │ ├── libEposCmd.so.5.0.1.0 │ │ └── libftd2xx.so.1.2.8 │ └── sf │ │ ├── libEposCmd.so.5.0.1.0 │ │ └── libftd2xx.so.1.1.12 ├── x86 │ ├── libEposCmd.so.5.0.1.0 │ └── libftd2xx.so.1.1.12 └── x86_64 │ ├── libEposCmd.so.5.0.1.0 │ └── libftd2xx.so.1.1.12 └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | *.dylib 9 | 10 | # Compiled Static libraries 11 | *.lai 12 | *.la 13 | *.a 14 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: 2 | - cpp 3 | compiler: 4 | - gcc 5 | 6 | install: 7 | - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' 8 | - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 9 | - sudo apt-get update -qq 10 | - sudo apt-get install python-catkin-pkg python-rosdep python-catkin-tools python-catkin-lint ros-hydro-catkin ros-hydro-ros-base -qq 11 | - sudo rosdep init 12 | - rosdep update 13 | - mkdir -p /tmp/ws/src 14 | - ln -s `pwd` /tmp/ws/src/package 15 | - cd /tmp/ws/src 16 | - git clone https://github.com/ros-controls/ros_control.git 17 | - touch ros_control/rqt_controller_manager/CATKIN_IGNORE 18 | - touch ros_control/joint_limits_interface/CATKIN_IGNORE 19 | - cd /tmp/ws 20 | - rosdep install --from-paths src --ignore-src --rosdistro hydro -y 21 | 22 | script: 23 | - source /opt/ros/hydro/setup.bash 24 | - catkin build --no-status 25 | - source devel/setup.bash 26 | - catkin lint src/package -W2 --ignore target_name_collision 27 | - catkin config --install 28 | - catkin clean -b 29 | - catkin build --no-status 30 | - source install/setup.bash 31 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2014, Worcester Polytechnic Institute 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of Worcester Polytechnic Institute 17 | nor the names of its contributors may be used to 18 | endorse or promote products derived from this software without 19 | specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | POSSIBILITY OF SUCH DAMAGE. 33 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | epos_hardware 2 | ============ 3 | -------------------------------------------------------------------------------- /epos_hardware/90-ftd2xx.rules: -------------------------------------------------------------------------------- 1 | SUBSYSTEM=="usb|usb_device", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="a8b0", GROUP="dialout", MODE="666", SYMLINK+="ftd2xx%n" 2 | -------------------------------------------------------------------------------- /epos_hardware/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package epos_hardware 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.3 (2015-06-18) 6 | ------------------ 7 | * Fixed quickstop diagnostics status 8 | * Added diagnostics information from statusword 9 | * Added ability to set fault recovery option 10 | * Added nominal current and max current to diagnostics 11 | * Added diagnostics warning if nominal current is exceeded 12 | * Added diagnostics for motor output 13 | * Added torque constant parameter 14 | * Added support for velocity halt command 15 | * Show error message if not all faults were cleared 16 | * Don't write value if command is nan 17 | * Limit profile velocity to specified limit 18 | * Cast parameters to integers because sometimes it causes the configuration to fail 19 | * Contributors: Mitchell Wills 20 | 21 | 0.0.2 (2015-03-06) 22 | ------------------ 23 | * Fixed so that udev rules file and example launch files are actually installed 24 | * Contributors: Mitchell Wills 25 | 26 | 0.0.1 (2015-01-30) 27 | ------------------ 28 | * Initial release of epos_hardware 29 | * Contributors: Mitchell Wills 30 | -------------------------------------------------------------------------------- /epos_hardware/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(epos_hardware) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | epos_library 6 | hardware_interface 7 | transmission_interface 8 | controller_manager 9 | roscpp 10 | diagnostic_updater 11 | ) 12 | 13 | catkin_package( 14 | INCLUDE_DIRS include 15 | LIBRARIES epos_library_utils epos_manager epos_hardware 16 | CATKIN_DEPENDS epos_library hardware_interface roscpp controller_manager diagnostic_updater 17 | ) 18 | 19 | ########### 20 | ## Build ## 21 | ########### 22 | 23 | include_directories(include) 24 | include_directories( 25 | ${catkin_INCLUDE_DIRS} 26 | ) 27 | 28 | # A collection of utilities for using the EPOS Command Libary 29 | add_library(epos_library_utils 30 | src/util/utils.cpp 31 | ) 32 | target_link_libraries(epos_library_utils 33 | ${catkin_LIBRARIES} 34 | ) 35 | 36 | 37 | # Build tool to list available devices 38 | add_executable(list_devices src/tools/list_devices.cpp) 39 | target_link_libraries(list_devices 40 | ${catkin_LIBRARIES} 41 | epos_library_utils 42 | ) 43 | 44 | # Build tool to list a devices state 45 | add_executable(get_state src/tools/get_state.cpp) 46 | target_link_libraries(get_state 47 | ${catkin_LIBRARIES} 48 | epos_library_utils 49 | ) 50 | 51 | add_library(epos_manager 52 | src/util/epos.cpp src/util/epos_manager.cpp 53 | ) 54 | target_link_libraries(epos_manager 55 | ${catkin_LIBRARIES} 56 | epos_library_utils 57 | ) 58 | 59 | add_library(epos_hardware 60 | src/util/epos_hardware.cpp 61 | ) 62 | target_link_libraries(epos_hardware 63 | ${catkin_LIBRARIES} 64 | epos_manager 65 | epos_library_utils 66 | ) 67 | 68 | 69 | add_executable(epos_hardware_node src/nodes/epos_hardware_node.cpp) 70 | target_link_libraries(epos_hardware_node 71 | ${catkin_LIBRARIES} 72 | epos_hardware 73 | ) 74 | 75 | ############# 76 | ## Install ## 77 | ############# 78 | 79 | 80 | # Mark udev install program for installation 81 | install(PROGRAMS 82 | install_udev_rules 83 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 84 | ) 85 | install(FILES 90-ftd2xx.rules 86 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 87 | ) 88 | 89 | install(DIRECTORY launch 90 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 91 | ) 92 | 93 | # Mark libraries and nodes for installation 94 | install(TARGETS epos_library_utils epos_manager epos_hardware list_devices get_state epos_hardware_node 95 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 96 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 97 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 98 | ) 99 | 100 | # Mark cpp header files for installation 101 | install(DIRECTORY include/${PROJECT_NAME}/ 102 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 103 | FILES_MATCHING PATTERN "*.h" 104 | PATTERN ".svn" EXCLUDE 105 | ) 106 | -------------------------------------------------------------------------------- /epos_hardware/include/epos_hardware/epos.h: -------------------------------------------------------------------------------- 1 | #ifndef EPOS_HARDWARE_EPOS_H_ 2 | #define EPOS_HARDWARE_EPOS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "epos_hardware/utils.h" 9 | 10 | namespace epos_hardware { 11 | 12 | #define STATUSWORD(b, v) ((v >> b) & 1) 13 | #define READY_TO_SWITCH_ON (0) 14 | #define SWITCHED_ON (1) 15 | #define ENABLE (2) 16 | #define FAULT (3) 17 | #define VOLTAGE_ENABLED (4) 18 | #define QUICKSTOP (5) 19 | #define WARNING (7) 20 | #define TARGET_REACHED (10) 21 | #define CURRENT_LIMIT_ACTIVE (11) 22 | 23 | 24 | class Epos { 25 | public: 26 | typedef enum { 27 | PROFILE_POSITION_MODE = 1, 28 | PROFILE_VELOCITY_MODE = 3 29 | } OperationMode; 30 | 31 | Epos(const std::string& name, 32 | ros::NodeHandle& nh, ros::NodeHandle& config_nh, 33 | EposFactory* epos_factory, 34 | hardware_interface::ActuatorStateInterface& asi, 35 | hardware_interface::VelocityActuatorInterface& avi, 36 | hardware_interface::PositionActuatorInterface& api); 37 | ~Epos(); 38 | bool init(); 39 | void read(); 40 | void write(); 41 | std::string name() { return name_; } 42 | std::string actuator_name() { return actuator_name_; } 43 | void update_diagnostics(); 44 | private: 45 | ros::NodeHandle config_nh_; 46 | diagnostic_updater::Updater diagnostic_updater_; 47 | EposFactory* epos_factory_; 48 | std::string name_; 49 | std::string actuator_name_; 50 | uint64_t serial_number_; 51 | OperationMode operation_mode_; 52 | NodeHandlePtr node_handle_; 53 | bool valid_; 54 | bool has_init_; 55 | 56 | double position_; 57 | double velocity_; 58 | double effort_; 59 | double current_; 60 | uint16_t statusword_; 61 | 62 | double position_cmd_; 63 | double velocity_cmd_; 64 | int max_profile_velocity_; 65 | bool halt_velocity_; 66 | double torque_constant_; 67 | double nominal_current_; 68 | double max_current_; 69 | 70 | void buildMotorStatus(diagnostic_updater::DiagnosticStatusWrapper &stat); 71 | void buildMotorOutputStatus(diagnostic_updater::DiagnosticStatusWrapper &stat); 72 | }; 73 | 74 | } 75 | 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /epos_hardware/include/epos_hardware/epos_hardware.h: -------------------------------------------------------------------------------- 1 | #ifndef EPOS_HARDWARE_EPOS_HARDWARE_H_ 2 | #define EPOS_HARDWARE_EPOS_HARDWARE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "epos_hardware/utils.h" 11 | #include "epos_hardware/epos.h" 12 | #include "epos_hardware/epos_manager.h" 13 | 14 | 15 | namespace epos_hardware { 16 | 17 | class EposHardware : public hardware_interface::RobotHW { 18 | public: 19 | EposHardware(ros::NodeHandle& nh, ros::NodeHandle& pnh, const std::vector& motor_names); 20 | bool init(); 21 | void read(); 22 | void write(); 23 | void update_diagnostics(); 24 | private: 25 | hardware_interface::ActuatorStateInterface asi; 26 | hardware_interface::VelocityActuatorInterface avi; 27 | hardware_interface::PositionActuatorInterface api; 28 | 29 | EposManager epos_manager_; 30 | 31 | transmission_interface::RobotTransmissions robot_transmissions; 32 | boost::scoped_ptr transmission_loader; 33 | }; 34 | 35 | } 36 | 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /epos_hardware/include/epos_hardware/epos_manager.h: -------------------------------------------------------------------------------- 1 | #ifndef EPOS_HARDWARE_EPOS_MANAGER_H_ 2 | #define EPOS_HARDWARE_EPOS_MANAGER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "epos_hardware/utils.h" 12 | #include "epos_hardware/epos.h" 13 | 14 | 15 | namespace epos_hardware { 16 | 17 | class EposManager { 18 | public: 19 | EposManager(hardware_interface::ActuatorStateInterface& asi, 20 | hardware_interface::VelocityActuatorInterface& avi, 21 | hardware_interface::PositionActuatorInterface& api, 22 | ros::NodeHandle& nh, ros::NodeHandle& pnh, 23 | const std::vector& motor_names); 24 | bool init(); 25 | void read(); 26 | void write(); 27 | void update_diagnostics(); 28 | std::vector > motors() { return motors_; }; 29 | private: 30 | std::vector > motors_; 31 | EposFactory epos_factory; 32 | 33 | hardware_interface::ActuatorStateInterface* asi_; 34 | hardware_interface::VelocityActuatorInterface* avi_; 35 | hardware_interface::PositionActuatorInterface* api_; 36 | }; 37 | 38 | } 39 | 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /epos_hardware/include/epos_hardware/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef EPOS_HARDWARE_UTILS_H_ 2 | #define EPOS_HARDWARE_UTILS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "epos_library/Definitions.h" 9 | #include 10 | #include 11 | 12 | bool SerialNumberFromHex(const std::string& str, uint64_t* serial_number); 13 | 14 | int GetErrorInfo(unsigned int error_code, std::string* error_string); 15 | 16 | int GetDeviceNameList(std::vector* device_names, unsigned int* error_code); 17 | 18 | int GetProtocolStackNameList(const std::string device_name, std::vector* protocol_stack_names, unsigned int* error_code); 19 | 20 | int GetInterfaceNameList(const std::string device_name, const std::string protocol_stack_name, 21 | std::vector* interface_names, unsigned int* error_code); 22 | 23 | int GetPortNameList(const std::string device_name, const std::string protocol_stack_name, const std::string interface_name, 24 | std::vector* port_names, unsigned int* error_code); 25 | 26 | int GetBaudrateList(const std::string device_name, const std::string protocol_stack_name, const std::string interface_name, 27 | const std::string port_name, std::vector* port_names, unsigned int* error_code); 28 | 29 | 30 | // An object that wraps a handle and closes it when destroyed 31 | class DeviceHandle { 32 | public: 33 | DeviceHandle(void* ptr) : ptr(ptr) {} 34 | ~DeviceHandle() { 35 | unsigned int error_code; 36 | VCS_CloseDevice(const_cast(ptr), &error_code); 37 | } 38 | void* const ptr; 39 | }; 40 | typedef boost::shared_ptr DeviceHandlePtr; 41 | 42 | class NodeHandle { 43 | public: 44 | NodeHandle(DeviceHandlePtr device_handle, unsigned short node_id) 45 | : device_handle(device_handle), node_id(node_id) {} 46 | const DeviceHandlePtr device_handle; 47 | const unsigned short node_id; 48 | }; 49 | typedef boost::shared_ptr NodeHandlePtr; 50 | 51 | typedef struct { 52 | std::string device_name; 53 | std::string protocol_stack_name; 54 | std::string interface_name; 55 | std::string port_name; 56 | unsigned short node_id; 57 | uint64_t serial_number; 58 | unsigned short hardware_version; 59 | unsigned short software_version; 60 | unsigned short application_number; 61 | unsigned short application_version; 62 | } EnumeratedNode; 63 | 64 | class EposFactory { 65 | public: 66 | 67 | EposFactory(); 68 | DeviceHandlePtr CreateDeviceHandle(const std::string device_name, 69 | const std::string protocol_stack_name, 70 | const std::string interface_name, 71 | const std::string port_name, 72 | unsigned int* error_code); 73 | 74 | NodeHandlePtr CreateNodeHandle(const std::string device_name, 75 | const std::string protocol_stack_name, 76 | const std::string interface_name, 77 | uint64_t serial_number, 78 | unsigned int* error_code); 79 | 80 | NodeHandlePtr CreateNodeHandle(const EnumeratedNode& node, 81 | unsigned int* error_code); 82 | 83 | 84 | int EnumerateNodes(const std::string device_name, const std::string protocol_stack_name, const std::string interface_name, 85 | const std::string port_name, std::vector* devices, unsigned int* error_code); 86 | 87 | int EnumerateNodes(const std::string device_name, const std::string protocol_stack_name, const std::string interface_name, 88 | std::vector* devices, unsigned int* error_code); 89 | 90 | 91 | private: 92 | std::map > existing_handles; 93 | }; 94 | 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /epos_hardware/install_udev_rules: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "" 4 | echo "This script copies a udev rule for ftd2xx devices" 5 | echo "" 6 | 7 | sudo cp `rospack find epos_hardware`/90-ftd2xx.rules /etc/udev/rules.d 8 | 9 | 10 | echo "" 11 | echo "Restarting udev" 12 | echo "" 13 | sudo service udev reload 14 | sudo service udev restart -------------------------------------------------------------------------------- /epos_hardware/launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /epos_hardware/launch/example.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | transmission_interface/SimpleTransmission 21 | 22 | hardware_interface/VelocityJointInterface 23 | 24 | 25 | 1000 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /epos_hardware/launch/example.yaml: -------------------------------------------------------------------------------- 1 | # Time in seconds 2 | # Current in amps 3 | # position, velocity, and acceleration in device units 4 | my_joint_actuator: 5 | actuator_name: 'test_joint_actuator' 6 | serial_number: '662080000026' 7 | operation_mode: 'profile_velocity' 8 | clear_faults: true 9 | 10 | motor: 11 | type: 10 12 | ec_motor: 13 | nominal_current: 9.650 14 | max_output_current: 10.500 15 | thermal_time_constant: 2.11 16 | number_of_pole_pairs: 2 17 | 18 | sensor: 19 | type: 1 20 | incremental_encoder: 21 | resolution: 500 22 | inverted_polarity: false 23 | 24 | safety: 25 | max_following_error: 20000 26 | max_profile_velocity: 12000 27 | max_acceleration: 15000 28 | 29 | position_profile: 30 | velocity: 10000 31 | acceleration: 8000 32 | deceleration: 9000 33 | 34 | velocity_profile: 35 | acceleration: 8000 36 | deceleration: 9000 37 | -------------------------------------------------------------------------------- /epos_hardware/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | epos_hardware 4 | 0.0.3 5 | A wrapper around the EPOS Command Library to provide easy integration with ROS control 6 | 7 | Mitchell Wills 8 | 9 | BSD 10 | 11 | https://github.com/RIVeR-Lab/epos_hardware 12 | 13 | Mitchell Wills 14 | 15 | catkin 16 | epos_library 17 | hardware_interface 18 | transmission_interface 19 | controller_manager 20 | roscpp 21 | diagnostic_updater 22 | epos_library 23 | hardware_interface 24 | transmission_interface 25 | controller_manager 26 | roscpp 27 | diagnostic_updater 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /epos_hardware/src/nodes/epos_hardware_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "epos_hardware/epos_hardware.h" 4 | #include 5 | #include 6 | 7 | int main(int argc, char** argv) { 8 | ros::init(argc, argv, "epos_velocity_hardware"); 9 | ros::NodeHandle nh; 10 | ros::NodeHandle pnh("~"); 11 | 12 | std::vector motor_names; 13 | for(int i = 0; i < argc-1; ++i) { 14 | motor_names.push_back(argv[i+1]); 15 | } 16 | epos_hardware::EposHardware robot(nh, pnh, motor_names); 17 | controller_manager::ControllerManager cm(&robot, nh); 18 | 19 | ros::AsyncSpinner spinner(1); 20 | spinner.start(); 21 | 22 | ROS_INFO("Initializing Motors"); 23 | if(!robot.init()) { 24 | ROS_FATAL("Failed to initialize motors"); 25 | return 1; 26 | } 27 | ROS_INFO("Motors Initialized"); 28 | 29 | ros::Rate controller_rate(50); 30 | ros::Time last = ros::Time::now(); 31 | while (ros::ok()) { 32 | robot.read(); 33 | ros::Time now = ros::Time::now(); 34 | cm.update(now, now-last); 35 | robot.write(); 36 | last = now; 37 | robot.update_diagnostics(); 38 | controller_rate.sleep(); 39 | } 40 | 41 | } 42 | -------------------------------------------------------------------------------- /epos_hardware/src/tools/get_state.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "epos_hardware/utils.h" 3 | #include "epos_library/Definitions.h" 4 | #include 5 | 6 | int main(int argc, char** argv){ 7 | uint64_t serial_number; 8 | if(argc == 2){ 9 | if(!SerialNumberFromHex(argv[1], &serial_number)) { 10 | std::cerr << "Expected a serial number" << std::endl; 11 | return 1; 12 | } 13 | } 14 | else { 15 | std::cerr << "Expected exactly one argument that is a serial number" << std::endl; 16 | return 1; 17 | } 18 | 19 | std::string error_string; 20 | unsigned int error_code = 0; 21 | 22 | std::cout << "Searching for USB EPOS2: 0x" << std::hex << serial_number << std::endl; 23 | 24 | std::string port_name; 25 | 26 | EposFactory epos_factory; 27 | 28 | NodeHandlePtr handle; 29 | if(handle = epos_factory.CreateNodeHandle("EPOS2", "MAXON SERIAL V2", "USB", serial_number, &error_code)) { 30 | int position; 31 | if(VCS_GetPositionIs(handle->device_handle->ptr, handle->node_id, &position, &error_code)){ 32 | std::cout << "Position: " << std::dec << position << std::endl; 33 | } 34 | else { 35 | if(GetErrorInfo(error_code, &error_string)){ 36 | std::cerr << "Could not get position: " << error_string << std::endl; 37 | } else { 38 | std::cerr << "Could not get position" << std::endl; 39 | } 40 | } 41 | 42 | int velocity; 43 | if(VCS_GetVelocityIs(handle->device_handle->ptr, handle->node_id, &velocity, &error_code)){ 44 | std::cout << "Velocity: " << std::dec << velocity << std::endl; 45 | } 46 | else { 47 | if(GetErrorInfo(error_code, &error_string)){ 48 | std::cerr << "Could not get velocity: " << error_string << std::endl; 49 | } else { 50 | std::cerr << "Could not get velocity" << std::endl; 51 | } 52 | } 53 | 54 | short current; 55 | if(VCS_GetCurrentIs(handle->device_handle->ptr, handle->node_id, ¤t, &error_code)){ 56 | std::cout << "Current: " << std::dec << current << std::endl; 57 | } 58 | else { 59 | if(GetErrorInfo(error_code, &error_string)){ 60 | std::cerr << "Could not get current: " << error_string << std::endl; 61 | } else { 62 | std::cerr << "Could not get current" << std::endl; 63 | } 64 | } 65 | 66 | } 67 | else { 68 | if(GetErrorInfo(error_code, &error_string)){ 69 | std::cerr << "Could not open device: " << error_string << std::endl; 70 | } else { 71 | std::cerr << "Could not open device" << std::endl; 72 | } 73 | return 1; 74 | } 75 | } 76 | -------------------------------------------------------------------------------- /epos_hardware/src/tools/list_devices.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "epos_hardware/utils.h" 3 | #include "epos_library/Definitions.h" 4 | #include 5 | 6 | int main(int argc, char** argv){ 7 | bool skip_rs232 = true; 8 | if(argc == 2){ 9 | if(!strcmp(argv[1], "--rs232")) 10 | skip_rs232 = false; 11 | else{ 12 | std::cerr << "unknown option: " << argv[1] << std::endl; 13 | return 1; 14 | } 15 | } 16 | else if(argc > 1){ 17 | std::cerr << "unknown options" << std::endl; 18 | return 1; 19 | } 20 | 21 | std::string error_string; 22 | unsigned int error_code = 0; 23 | 24 | EposFactory epos_factory; 25 | 26 | std::cout << "Listing Devices:" << std::endl; 27 | 28 | std::vector device_names; 29 | if(GetDeviceNameList(&device_names, &error_code)) { 30 | BOOST_FOREACH(const std::string& device_name, device_names) { 31 | std::cout << device_name < protocol_stack_names; 34 | if(GetProtocolStackNameList(device_name, &protocol_stack_names, &error_code)) { 35 | BOOST_FOREACH(const std::string& protocol_stack_name, protocol_stack_names) { 36 | std::cout << "\t" << protocol_stack_name < interface_names; 39 | if(GetInterfaceNameList(device_name, protocol_stack_name, &interface_names, &error_code)) { 40 | BOOST_FOREACH(const std::string& interface_name, interface_names) { 41 | std::cout << "\t\t" << interface_name < port_names; 48 | if(GetPortNameList(device_name, protocol_stack_name, interface_name, &port_names, &error_code)) { 49 | BOOST_FOREACH(const std::string& port_name, port_names) { 50 | std::cout << "\t\t\t" << port_name < baudrates; 53 | if(GetBaudrateList(device_name, protocol_stack_name, interface_name, port_name, &baudrates, &error_code)) { 54 | std::cout << "\t\t\t\tBaudrates:" << std::endl; 55 | BOOST_FOREACH(unsigned int baudrate, baudrates) { 56 | std::cout << "\t\t\t\t\t" << std::dec << baudrate < devices; 60 | if(epos_factory.EnumerateNodes(device_name, protocol_stack_name, interface_name, port_name, &devices, &error_code)) { 61 | std::cout << "\t\t\t\tDevices:" << std::endl; 62 | BOOST_FOREACH(const EnumeratedNode& node, devices) { 63 | std::cout << "\t\t\t\t\tNode Id: " << std::dec << node.node_id << std::endl; 64 | std::cout << "\t\t\t\t\t\tSerial Number: 0x" << std::hex << node.serial_number << std::endl; 65 | std::cout << "\t\t\t\t\t\tHardware Version: 0x" << std::hex << node.hardware_version << std::endl; 66 | std::cout << "\t\t\t\t\t\tSoftware Version: 0x" << std::hex << node.software_version << std::endl; 67 | std::cout << "\t\t\t\t\t\tApplication Number: 0x" << std::hex << node.application_number << std::endl; 68 | std::cout << "\t\t\t\t\t\tApplication Version: 0x" << std::hex << node.application_version << std::endl; 69 | } 70 | } 71 | else { 72 | if(GetErrorInfo(error_code, &error_string)){ 73 | std::cerr << "Could not enumerate devices: " << error_string << std::endl; 74 | } else { 75 | std::cerr << "Could not enumerate devices" << std::endl; 76 | } 77 | } 78 | 79 | } 80 | else { 81 | if(GetErrorInfo(error_code, &error_string)){ 82 | std::cerr << "Could not get baudrates: " << error_string << std::endl; 83 | } else { 84 | std::cerr << "Could not get baudrates" << std::endl; 85 | } 86 | } 87 | 88 | } 89 | } 90 | else { 91 | if(GetErrorInfo(error_code, &error_string)){ 92 | std::cerr << "Could not get port names: " << error_string << std::endl; 93 | } else { 94 | std::cerr << "Could not get port names" << std::endl; 95 | } 96 | } 97 | 98 | } 99 | } 100 | else { 101 | if(GetErrorInfo(error_code, &error_string)){ 102 | std::cerr << "Could not get interface names: " << error_string << std::endl; 103 | } else { 104 | std::cerr << "Could not get interface names" << std::endl; 105 | } 106 | } 107 | 108 | } 109 | } 110 | else { 111 | if(GetErrorInfo(error_code, &error_string)){ 112 | std::cerr << "Could not get protocol stack names: " << error_string << std::endl; 113 | } else { 114 | std::cerr << "Could not get protocol stack names" << std::endl; 115 | } 116 | } 117 | 118 | } 119 | } 120 | else { 121 | if(GetErrorInfo(error_code, &error_string)){ 122 | std::cerr << "Could not get device names: " << error_string << std::endl; 123 | } else { 124 | std::cerr << "Could not get device names" << std::endl; 125 | } 126 | } 127 | } 128 | -------------------------------------------------------------------------------- /epos_hardware/src/util/epos.cpp: -------------------------------------------------------------------------------- 1 | #include "epos_hardware/epos.h" 2 | #include 3 | 4 | namespace epos_hardware { 5 | 6 | Epos::Epos(const std::string& name, 7 | ros::NodeHandle& nh, ros::NodeHandle& config_nh, 8 | EposFactory* epos_factory, 9 | hardware_interface::ActuatorStateInterface& asi, 10 | hardware_interface::VelocityActuatorInterface& avi, 11 | hardware_interface::PositionActuatorInterface& api) 12 | : name_(name), config_nh_(config_nh), diagnostic_updater_(nh, config_nh), epos_factory_(epos_factory), 13 | has_init_(false), 14 | position_(0), velocity_(0), effort_(0), current_(0), statusword_(0), 15 | position_cmd_(0), velocity_cmd_(0) { 16 | 17 | valid_ = true; 18 | if(!config_nh_.getParam("actuator_name", actuator_name_)) { 19 | ROS_ERROR("You must specify an actuator name"); 20 | valid_ = false; 21 | } 22 | 23 | std::string serial_number_str; 24 | if(!config_nh_.getParam("serial_number", serial_number_str)) { 25 | ROS_ERROR("You must specify a serial number"); 26 | valid_ = false; 27 | } 28 | else { 29 | ROS_ASSERT(SerialNumberFromHex(serial_number_str, &serial_number_)); 30 | } 31 | 32 | 33 | std::string operation_mode_str; 34 | if(!config_nh_.getParam("operation_mode", operation_mode_str)) { 35 | ROS_ERROR("You must specify an operation mode"); 36 | valid_ = false; 37 | } 38 | else { 39 | if(operation_mode_str == "profile_position") { 40 | operation_mode_ = PROFILE_POSITION_MODE; 41 | } 42 | else if(operation_mode_str == "profile_velocity") { 43 | operation_mode_ = PROFILE_VELOCITY_MODE; 44 | } 45 | else { 46 | ROS_ERROR_STREAM(operation_mode_str << " is not a valid operation mode"); 47 | valid_ = false; 48 | } 49 | } 50 | 51 | ROS_INFO_STREAM(actuator_name_); 52 | hardware_interface::ActuatorStateHandle state_handle(actuator_name_, &position_, &velocity_, &effort_); 53 | asi.registerHandle(state_handle); 54 | 55 | hardware_interface::ActuatorHandle position_handle(state_handle, &position_cmd_); 56 | api.registerHandle(position_handle); 57 | hardware_interface::ActuatorHandle velocity_handle(state_handle, &velocity_cmd_); 58 | avi.registerHandle(velocity_handle); 59 | 60 | diagnostic_updater_.setHardwareID(serial_number_str); 61 | std::stringstream motor_diagnostic_name_ss; 62 | motor_diagnostic_name_ss << name << ": " << "Motor"; 63 | diagnostic_updater_.add(motor_diagnostic_name_ss.str(), boost::bind(&Epos::buildMotorStatus, this, _1)); 64 | std::stringstream motor_output_diagnostic_name_ss; 65 | motor_output_diagnostic_name_ss << name << ": " << "Motor Output"; 66 | diagnostic_updater_.add(motor_output_diagnostic_name_ss.str(), boost::bind(&Epos::buildMotorOutputStatus, this, _1)); 67 | } 68 | 69 | Epos::~Epos() { 70 | unsigned int error_code; 71 | if(node_handle_) 72 | VCS_SetDisableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code); 73 | } 74 | 75 | class ParameterSetLoader { 76 | public: 77 | ParameterSetLoader(ros::NodeHandle nh) : nh_(nh){} 78 | ParameterSetLoader(ros::NodeHandle parent_nh, const std::string& name) : nh_(parent_nh, name){} 79 | template ParameterSetLoader& param(const std::string& name, T& value) { 80 | if(nh_.getParam(name, value)) 81 | found_.push_back(name); 82 | else 83 | not_found_.push_back(name); 84 | return *this; 85 | } 86 | bool all_or_none(bool& found_all) { 87 | if(not_found_.size() == 0) { 88 | found_all = true; 89 | return true; 90 | } 91 | if(found_.size() == 0) { 92 | found_all = false; 93 | return true; 94 | } 95 | ROS_ERROR_STREAM("Expected all or none parameter set: (" << nh_.getNamespace() << ")"); 96 | BOOST_FOREACH(const std::string& name, found_) { 97 | ROS_ERROR_STREAM("\tFound: " << nh_.resolveName(name)); 98 | } 99 | BOOST_FOREACH(const std::string& name, not_found_) { 100 | ROS_ERROR_STREAM("\tExpected: " << nh_.resolveName(name)); 101 | } 102 | return false; 103 | } 104 | 105 | private: 106 | ros::NodeHandle nh_; 107 | std::vector found_; 108 | std::vector not_found_; 109 | }; 110 | 111 | #define VCS(func, ...) \ 112 | if(!VCS_##func(node_handle_->device_handle->ptr, node_handle_->node_id, __VA_ARGS__, &error_code)) { \ 113 | ROS_ERROR("Failed to "#func); \ 114 | return false; \ 115 | } 116 | 117 | #define VCS_FROM_SINGLE_PARAM_REQUIRED(nh, type, name, func) \ 118 | type name; \ 119 | if(!nh.getParam(#name, name)) { \ 120 | ROS_ERROR_STREAM(nh.resolveName(#name) << " not specified"); \ 121 | return false; \ 122 | } \ 123 | else { \ 124 | VCS(func, name); \ 125 | } 126 | #define VCS_FROM_SINGLE_PARAM_OPTIONAL(nh, type, name, func) \ 127 | bool name##_set; \ 128 | type name; \ 129 | if(name##_set = nh.getParam(#name, name)) { \ 130 | VCS(func, name); \ 131 | } \ 132 | 133 | 134 | bool Epos::init() { 135 | if(!valid_) { 136 | ROS_ERROR_STREAM("Not Initializing: 0x" << std::hex << serial_number_ << ", initial construction failed"); 137 | return false; 138 | } 139 | 140 | ROS_INFO_STREAM("Initializing: 0x" << std::hex << serial_number_); 141 | unsigned int error_code; 142 | node_handle_ = epos_factory_->CreateNodeHandle("EPOS2", "MAXON SERIAL V2", "USB", serial_number_, &error_code); 143 | if(!node_handle_) { 144 | ROS_ERROR("Could not find motor"); 145 | return false; 146 | } 147 | ROS_INFO_STREAM("Found Motor"); 148 | 149 | if(!VCS_SetProtocolStackSettings(node_handle_->device_handle->ptr, 1000000, 500, &error_code)) { 150 | ROS_ERROR("Failed to SetProtocolStackSettings"); 151 | return false; 152 | } 153 | 154 | if(!VCS_SetDisableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) { 155 | ROS_ERROR("Failed to SetDisableState"); 156 | return false; 157 | } 158 | 159 | VCS(SetOperationMode, operation_mode_); 160 | 161 | std::string fault_reaction_str; 162 | #define SET_FAULT_REACTION_OPTION(val) \ 163 | do { \ 164 | unsigned int length = 2; \ 165 | unsigned int bytes_written; \ 166 | int16_t data = val; \ 167 | VCS(SetObject, 0x605E, 0x00, &data, length, &bytes_written); \ 168 | } while(true) 169 | 170 | if(config_nh_.getParam("fault_reaction_option", fault_reaction_str)) { 171 | if(fault_reaction_str == "signal_only") { 172 | SET_FAULT_REACTION_OPTION(-1); 173 | } 174 | else if(fault_reaction_str == "disable_drive") { 175 | SET_FAULT_REACTION_OPTION(0); 176 | } 177 | else if(fault_reaction_str == "slow_down_ramp") { 178 | SET_FAULT_REACTION_OPTION(1); 179 | } 180 | else if(fault_reaction_str == "slow_down_quickstop") { 181 | SET_FAULT_REACTION_OPTION(2); 182 | } 183 | else { 184 | ROS_ERROR_STREAM(fault_reaction_str << " is not a valid fault reaction option"); 185 | return false; 186 | } 187 | } 188 | 189 | 190 | ROS_INFO("Configuring Motor"); 191 | { 192 | nominal_current_ = 0; 193 | max_current_ = 0; 194 | ros::NodeHandle motor_nh(config_nh_, "motor"); 195 | 196 | VCS_FROM_SINGLE_PARAM_REQUIRED(motor_nh, int, type, SetMotorType); 197 | 198 | { 199 | bool dc_motor; 200 | double nominal_current; 201 | double max_output_current; 202 | double thermal_time_constant; 203 | if(!ParameterSetLoader(motor_nh, "dc_motor") 204 | .param("nominal_current", nominal_current) 205 | .param("max_output_current", max_output_current) 206 | .param("thermal_time_constant", thermal_time_constant) 207 | .all_or_none(dc_motor)) 208 | return false; 209 | if(dc_motor){ 210 | nominal_current_ = nominal_current; 211 | max_current_ = max_output_current; 212 | VCS(SetDcMotorParameter, 213 | (int)(1000 * nominal_current), // A -> mA 214 | (int)(1000 * max_output_current), // A -> mA 215 | (int)(10 * thermal_time_constant) // s -> 100ms 216 | ); 217 | } 218 | } 219 | 220 | 221 | { 222 | bool ec_motor; 223 | double nominal_current; 224 | double max_output_current; 225 | double thermal_time_constant; 226 | int number_of_pole_pairs; 227 | if(!ParameterSetLoader(motor_nh, "ec_motor") 228 | .param("nominal_current", nominal_current) 229 | .param("max_output_current", max_output_current) 230 | .param("thermal_time_constant", thermal_time_constant) 231 | .param("number_of_pole_pairs", number_of_pole_pairs) 232 | .all_or_none(ec_motor)) 233 | return false; 234 | 235 | if(ec_motor) { 236 | nominal_current_ = nominal_current; 237 | max_current_ = max_output_current; 238 | VCS(SetEcMotorParameter, 239 | (int)(1000 * nominal_current), // A -> mA 240 | (int)(1000 * max_output_current), // A -> mA 241 | (int)(10 * thermal_time_constant), // s -> 100ms 242 | number_of_pole_pairs); 243 | } 244 | } 245 | } 246 | 247 | ROS_INFO("Configuring Sensor"); 248 | { 249 | ros::NodeHandle sensor_nh(config_nh_, "sensor"); 250 | 251 | VCS_FROM_SINGLE_PARAM_REQUIRED(sensor_nh, int, type, SetSensorType); 252 | 253 | { 254 | bool incremental_encoder; 255 | int resolution; 256 | bool inverted_polarity; 257 | 258 | if(!ParameterSetLoader(sensor_nh, "incremental_encoder") 259 | .param("resolution", resolution) 260 | .param("inverted_polarity", inverted_polarity) 261 | .all_or_none(incremental_encoder)) 262 | return false; 263 | if(incremental_encoder) { 264 | VCS(SetIncEncoderParameter, resolution, inverted_polarity); 265 | } 266 | } 267 | 268 | { 269 | bool hall_sensor; 270 | bool inverted_polarity; 271 | 272 | if(!ParameterSetLoader(sensor_nh, "hall_sensor") 273 | .param("inverted_polarity", inverted_polarity) 274 | .all_or_none(hall_sensor)) 275 | return false; 276 | if(hall_sensor) { 277 | VCS(SetHallSensorParameter, inverted_polarity); 278 | } 279 | } 280 | 281 | { 282 | bool ssi_absolute_encoder; 283 | int data_rate; 284 | int number_of_multiturn_bits; 285 | int number_of_singleturn_bits; 286 | bool inverted_polarity; 287 | 288 | if(!ParameterSetLoader(sensor_nh, "ssi_absolute_encoder") 289 | .param("data_rate", data_rate) 290 | .param("number_of_multiturn_bits", number_of_multiturn_bits) 291 | .param("number_of_singleturn_bits", number_of_singleturn_bits) 292 | .param("inverted_polarity", inverted_polarity) 293 | .all_or_none(ssi_absolute_encoder)) 294 | return false; 295 | if(ssi_absolute_encoder) { 296 | VCS(SetSsiAbsEncoderParameter, 297 | data_rate, 298 | number_of_multiturn_bits, 299 | number_of_singleturn_bits, 300 | inverted_polarity); 301 | } 302 | } 303 | 304 | } 305 | 306 | { 307 | ROS_INFO("Configuring Safety"); 308 | ros::NodeHandle safety_nh(config_nh_, "safety"); 309 | 310 | VCS_FROM_SINGLE_PARAM_OPTIONAL(safety_nh, int, max_following_error, SetMaxFollowingError); 311 | VCS_FROM_SINGLE_PARAM_OPTIONAL(safety_nh, int, max_profile_velocity, SetMaxProfileVelocity); 312 | VCS_FROM_SINGLE_PARAM_OPTIONAL(safety_nh, int, max_acceleration, SetMaxAcceleration); 313 | if(max_profile_velocity_set) 314 | max_profile_velocity_ = max_profile_velocity; 315 | else 316 | max_profile_velocity_ = -1; 317 | } 318 | 319 | { 320 | ROS_INFO("Configuring Position Regulator"); 321 | ros::NodeHandle position_regulator_nh(config_nh_, "position_regulator"); 322 | { 323 | bool position_regulator_gain; 324 | int p, i, d; 325 | if(!ParameterSetLoader(position_regulator_nh, "gain") 326 | .param("p", p) 327 | .param("i", i) 328 | .param("d", d) 329 | .all_or_none(position_regulator_gain)) 330 | return false; 331 | if(position_regulator_gain){ 332 | VCS(SetPositionRegulatorGain, p, i, d); 333 | } 334 | } 335 | 336 | { 337 | bool position_regulator_feed_forward; 338 | int velocity, acceleration; 339 | if(!ParameterSetLoader(position_regulator_nh, "feed_forward") 340 | .param("velocity", velocity) 341 | .param("acceleration", acceleration) 342 | .all_or_none(position_regulator_feed_forward)) 343 | return false; 344 | if(position_regulator_feed_forward){ 345 | VCS(SetPositionRegulatorFeedForward, velocity, acceleration); 346 | } 347 | } 348 | } 349 | 350 | { 351 | ROS_INFO("Configuring Velocity Regulator"); 352 | ros::NodeHandle velocity_regulator_nh(config_nh_, "velocity_regulator"); 353 | { 354 | bool velocity_regulator_gain; 355 | int p, i; 356 | if(!ParameterSetLoader(velocity_regulator_nh, "gain") 357 | .param("p", p) 358 | .param("i", i) 359 | .all_or_none(velocity_regulator_gain)) 360 | return false; 361 | if(velocity_regulator_gain){ 362 | VCS(SetVelocityRegulatorGain, p, i); 363 | } 364 | } 365 | 366 | { 367 | bool velocity_regulator_feed_forward; 368 | int velocity, acceleration; 369 | if(!ParameterSetLoader(velocity_regulator_nh, "feed_forward") 370 | .param("velocity", velocity) 371 | .param("acceleration", acceleration) 372 | .all_or_none(velocity_regulator_feed_forward)) 373 | return false; 374 | if(velocity_regulator_feed_forward){ 375 | VCS(SetVelocityRegulatorFeedForward, velocity, acceleration); 376 | } 377 | } 378 | } 379 | 380 | 381 | { 382 | ROS_INFO("Configuring Current Regulator"); 383 | ros::NodeHandle current_regulator_nh(config_nh_, "current_regulator"); 384 | { 385 | bool current_regulator_gain; 386 | int p, i; 387 | if(!ParameterSetLoader(current_regulator_nh, "gain") 388 | .param("p", p) 389 | .param("i", i) 390 | .all_or_none(current_regulator_gain)) 391 | return false; 392 | if(current_regulator_gain){ 393 | VCS(SetCurrentRegulatorGain, p, i); 394 | } 395 | } 396 | } 397 | 398 | 399 | { 400 | ROS_INFO("Configuring Position Profile"); 401 | ros::NodeHandle position_profile_nh(config_nh_, "position_profile"); 402 | { 403 | bool position_profile; 404 | int velocity, acceleration, deceleration; 405 | if(!ParameterSetLoader(position_profile_nh) 406 | .param("velocity", velocity) 407 | .param("acceleration", acceleration) 408 | .param("deceleration", deceleration) 409 | .all_or_none(position_profile)) 410 | return false; 411 | if(position_profile){ 412 | VCS(SetPositionProfile, velocity, acceleration, deceleration); 413 | } 414 | } 415 | 416 | { 417 | bool position_profile_window; 418 | int window; 419 | double time; 420 | if(!ParameterSetLoader(position_profile_nh, "window") 421 | .param("window", window) 422 | .param("time", time) 423 | .all_or_none(position_profile_window)) 424 | return false; 425 | if(position_profile_window){ 426 | VCS(EnablePositionWindow, 427 | window, 428 | (int)(1000 * time) // s -> ms 429 | ); 430 | } 431 | } 432 | } 433 | 434 | { 435 | ROS_INFO("Configuring Velocity Profile"); 436 | ros::NodeHandle velocity_profile_nh(config_nh_, "velocity_profile"); 437 | { 438 | bool velocity_profile; 439 | int acceleration, deceleration; 440 | if(!ParameterSetLoader(velocity_profile_nh) 441 | .param("acceleration", acceleration) 442 | .param("deceleration", deceleration) 443 | .all_or_none(velocity_profile)) 444 | return false; 445 | if(velocity_profile){ 446 | VCS(SetVelocityProfile, acceleration, deceleration); 447 | } 448 | } 449 | 450 | { 451 | bool velocity_profile_window; 452 | int window; 453 | double time; 454 | if(!ParameterSetLoader(velocity_profile_nh, "window") 455 | .param("window", window) 456 | .param("time", time) 457 | .all_or_none(velocity_profile_window)) 458 | return false; 459 | if(velocity_profile_window){ 460 | VCS(EnableVelocityWindow, 461 | window, 462 | (int)(1000 * time) // s -> ms 463 | ); 464 | } 465 | } 466 | } 467 | 468 | 469 | 470 | ROS_INFO("Querying Faults"); 471 | unsigned char num_errors; 472 | if(!VCS_GetNbOfDeviceError(node_handle_->device_handle->ptr, node_handle_->node_id, &num_errors, &error_code)) 473 | return false; 474 | for(int i = 1; i<= num_errors; ++i) { 475 | unsigned int error_number; 476 | if(!VCS_GetDeviceErrorCode(node_handle_->device_handle->ptr, node_handle_->node_id, i, &error_number, &error_code)) 477 | return false; 478 | ROS_WARN_STREAM("EPOS Device Error: 0x" << std::hex << error_number); 479 | } 480 | 481 | bool clear_faults; 482 | config_nh_.param("clear_faults", clear_faults, false); 483 | if(num_errors > 0) { 484 | if(clear_faults) { 485 | ROS_INFO("Clearing faults"); 486 | if(!VCS_ClearFault(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) { 487 | ROS_ERROR("Could not clear faults"); 488 | return false; 489 | } 490 | else 491 | ROS_INFO("Cleared faults"); 492 | } 493 | else { 494 | ROS_ERROR("Not clearing faults, but faults exist"); 495 | return false; 496 | } 497 | } 498 | 499 | if(!VCS_GetNbOfDeviceError(node_handle_->device_handle->ptr, node_handle_->node_id, &num_errors, &error_code)) 500 | return false; 501 | if(num_errors > 0) { 502 | ROS_ERROR("Not all faults were cleared"); 503 | return false; 504 | } 505 | 506 | config_nh_.param("halt_velocity", halt_velocity_, false); 507 | 508 | if(!config_nh_.getParam("torque_constant", torque_constant_)) { 509 | ROS_WARN("No torque constant specified, you can supply one using the 'torque_constant' parameter"); 510 | torque_constant_ = 1.0; 511 | } 512 | 513 | ROS_INFO_STREAM("Enabling Motor"); 514 | if(!VCS_SetEnableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) 515 | return false; 516 | 517 | has_init_ = true; 518 | return true; 519 | } 520 | 521 | void Epos::read() { 522 | if(!has_init_) 523 | return; 524 | 525 | unsigned int error_code; 526 | 527 | // Read statusword 528 | unsigned int bytes_read; 529 | VCS_GetObject(node_handle_->device_handle->ptr, node_handle_->node_id, 0x6041, 0x00, &statusword_, 2, &bytes_read, &error_code); 530 | 531 | int position_raw; 532 | int velocity_raw; 533 | short current_raw; 534 | VCS_GetPositionIs(node_handle_->device_handle->ptr, node_handle_->node_id, &position_raw, &error_code); 535 | VCS_GetVelocityIs(node_handle_->device_handle->ptr, node_handle_->node_id, &velocity_raw, &error_code); 536 | VCS_GetCurrentIs(node_handle_->device_handle->ptr, node_handle_->node_id, ¤t_raw, &error_code); 537 | position_ = position_raw; 538 | velocity_ = velocity_raw; 539 | current_ = current_raw / 1000.0; // mA -> A 540 | effort_ = current_ * torque_constant_; 541 | 542 | } 543 | 544 | void Epos::write() { 545 | if(!has_init_) 546 | return; 547 | 548 | unsigned int error_code; 549 | if(operation_mode_ == PROFILE_VELOCITY_MODE) { 550 | if(isnan(velocity_cmd_)) 551 | return; 552 | int cmd = (int)velocity_cmd_; 553 | if(max_profile_velocity_ >= 0) { 554 | if(cmd < -max_profile_velocity_) 555 | cmd = -max_profile_velocity_; 556 | if(cmd > max_profile_velocity_) 557 | cmd = max_profile_velocity_; 558 | } 559 | 560 | if(cmd == 0 && halt_velocity_) { 561 | VCS_HaltVelocityMovement(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code); 562 | } 563 | else { 564 | VCS_MoveWithVelocity(node_handle_->device_handle->ptr, node_handle_->node_id, cmd, &error_code); 565 | } 566 | } 567 | else if(operation_mode_ == PROFILE_POSITION_MODE) { 568 | if(isnan(position_cmd_)) 569 | return; 570 | VCS_MoveToPosition(node_handle_->device_handle->ptr, node_handle_->node_id, (int)position_cmd_, true, true, &error_code); 571 | } 572 | } 573 | 574 | void Epos::update_diagnostics() { 575 | diagnostic_updater_.update(); 576 | } 577 | void Epos::buildMotorStatus(diagnostic_updater::DiagnosticStatusWrapper &stat) { 578 | stat.add("Actuator Name", actuator_name_); 579 | unsigned int error_code; 580 | if(has_init_) { 581 | bool enabled = STATUSWORD(READY_TO_SWITCH_ON, statusword_) && STATUSWORD(SWITCHED_ON, statusword_) && STATUSWORD(ENABLE, statusword_); 582 | if(enabled) { 583 | stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Enabled"); 584 | } 585 | else { 586 | stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Disabled"); 587 | } 588 | 589 | // Quickstop is enabled when bit is unset (only read quickstop when enabled) 590 | if(!STATUSWORD(QUICKSTOP, statusword_) && enabled) { 591 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Quickstop"); 592 | } 593 | 594 | if(STATUSWORD(WARNING, statusword_)) { 595 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Warning"); 596 | } 597 | 598 | if(STATUSWORD(FAULT, statusword_)) { 599 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Fault"); 600 | } 601 | 602 | stat.add("Enabled", STATUSWORD(ENABLE, statusword_)); 603 | stat.add("Fault", STATUSWORD(FAULT, statusword_)); 604 | stat.add("Voltage Enabled", STATUSWORD(VOLTAGE_ENABLED, statusword_)); 605 | stat.add("Quickstop", STATUSWORD(QUICKSTOP, statusword_)); 606 | stat.add("Warning", STATUSWORD(WARNING, statusword_)); 607 | 608 | unsigned char num_errors; 609 | if(VCS_GetNbOfDeviceError(node_handle_->device_handle->ptr, node_handle_->node_id, &num_errors, &error_code)) { 610 | for(int i = 1; i<= num_errors; ++i) { 611 | unsigned int error_number; 612 | if(VCS_GetDeviceErrorCode(node_handle_->device_handle->ptr, node_handle_->node_id, i, &error_number, &error_code)) { 613 | std::stringstream error_msg; 614 | error_msg << "EPOS Device Error: 0x" << std::hex << error_number; 615 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, error_msg.str()); 616 | } 617 | else { 618 | std::string error_str; 619 | if(GetErrorInfo(error_code, &error_str)) { 620 | std::stringstream error_msg; 621 | error_msg << "Could not read device error: " << error_str; 622 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, error_msg.str()); 623 | } 624 | else { 625 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Could not read device error"); 626 | } 627 | } 628 | } 629 | } 630 | else { 631 | std::string error_str; 632 | if(GetErrorInfo(error_code, &error_str)) { 633 | std::stringstream error_msg; 634 | error_msg << "Could not read device errors: " << error_str; 635 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, error_msg.str()); 636 | } 637 | else { 638 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Could not read device errors"); 639 | } 640 | } 641 | 642 | 643 | } 644 | else { 645 | stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "EPOS not initialized"); 646 | } 647 | } 648 | 649 | void Epos::buildMotorOutputStatus(diagnostic_updater::DiagnosticStatusWrapper &stat) { 650 | std::string operation_mode_str; 651 | if(operation_mode_ == PROFILE_POSITION_MODE) { 652 | operation_mode_str = "Profile Position Mode"; 653 | stat.add("Commanded Position", boost::lexical_cast(position_cmd_) + " rotations"); 654 | } 655 | else if(operation_mode_ == PROFILE_VELOCITY_MODE) { 656 | operation_mode_str = "Profile Velocity Mode"; 657 | stat.add("Commanded Velocity", boost::lexical_cast(velocity_cmd_) + " rpm"); 658 | } 659 | else { 660 | operation_mode_str = "Unknown Mode"; 661 | } 662 | stat.add("Operation Mode", operation_mode_str); 663 | stat.add("Nominal Current", boost::lexical_cast(nominal_current_) + " A"); 664 | stat.add("Max Current", boost::lexical_cast(max_current_) + " A"); 665 | 666 | unsigned int error_code; 667 | if(has_init_) { 668 | stat.add("Position", boost::lexical_cast(position_) + " rotations"); 669 | stat.add("Velocity", boost::lexical_cast(velocity_) + " rpm"); 670 | stat.add("Torque", boost::lexical_cast(effort_) + " Nm"); 671 | stat.add("Current", boost::lexical_cast(current_) + " A"); 672 | 673 | 674 | stat.add("Target Reached", STATUSWORD(TARGET_REACHED, statusword_)); 675 | stat.add("Current Limit Active", STATUSWORD(CURRENT_LIMIT_ACTIVE, statusword_)); 676 | 677 | 678 | stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "EPOS operating in " + operation_mode_str); 679 | if(STATUSWORD(CURRENT_LIMIT_ACTIVE, statusword_)) 680 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Current Limit Active"); 681 | if(nominal_current_ > 0 && std::abs(current_) > nominal_current_) { 682 | stat.mergeSummaryf(diagnostic_msgs::DiagnosticStatus::WARN, "Nominal Current Exceeded (Current: %f A)", current_); 683 | } 684 | 685 | 686 | } 687 | else { 688 | stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "EPOS not initialized"); 689 | } 690 | } 691 | 692 | 693 | } 694 | -------------------------------------------------------------------------------- /epos_hardware/src/util/epos_hardware.cpp: -------------------------------------------------------------------------------- 1 | #include "epos_hardware/epos_hardware.h" 2 | #include 3 | 4 | namespace epos_hardware { 5 | 6 | EposHardware::EposHardware(ros::NodeHandle& nh, ros::NodeHandle& pnh, const std::vector& motor_names) 7 | : epos_manager_(asi, avi, api, nh, pnh, motor_names) { 8 | 9 | // TODO throw exception or something 10 | try { 11 | transmission_loader.reset(new transmission_interface::TransmissionInterfaceLoader(this, &robot_transmissions)); 12 | } 13 | catch(const std::invalid_argument& ex){ 14 | ROS_ERROR_STREAM("Failed to create transmission interface loader. " << ex.what()); 15 | return; 16 | } 17 | catch(const pluginlib::LibraryLoadException& ex){ 18 | ROS_ERROR_STREAM("Failed to create transmission interface loader. " << ex.what()); 19 | return; 20 | } 21 | catch(...){ 22 | ROS_ERROR_STREAM("Failed to create transmission interface loader. "); 23 | return; 24 | } 25 | 26 | registerInterface(&asi); 27 | registerInterface(&avi); 28 | registerInterface(&api); 29 | 30 | std::string urdf_string; 31 | nh.getParam("robot_description", urdf_string); 32 | while (urdf_string.empty() && ros::ok()){ 33 | ROS_INFO_STREAM_ONCE("Waiting for robot_description"); 34 | nh.getParam("robot_description", urdf_string); 35 | ros::Duration(0.1).sleep(); 36 | } 37 | 38 | transmission_interface::TransmissionParser parser; 39 | std::vector infos; 40 | // TODO: throw exception 41 | if (!parser.parse(urdf_string, infos)) { 42 | ROS_ERROR("Error parsing URDF"); 43 | return; 44 | } 45 | 46 | // build a list of all loaded actuator names 47 | std::vector actuator_names; 48 | std::vector > motors = epos_manager_.motors(); 49 | BOOST_FOREACH(const boost::shared_ptr& motor, motors) { 50 | actuator_names.push_back(motor->actuator_name()); 51 | } 52 | 53 | // Load all transmissions that are for the loaded motors 54 | BOOST_FOREACH(const transmission_interface::TransmissionInfo& info, infos) { 55 | bool found_some = false; 56 | bool found_all = true; 57 | BOOST_FOREACH(const transmission_interface::ActuatorInfo& actuator, info.actuators_) { 58 | if(std::find(actuator_names.begin(), actuator_names.end(), actuator.name_) != actuator_names.end()) 59 | found_some = true; 60 | else 61 | found_all = false; 62 | } 63 | if(found_all) { 64 | if (!transmission_loader->load(info)) { 65 | ROS_ERROR_STREAM("Error loading transmission: " << info.name_); 66 | return; 67 | } 68 | else 69 | ROS_INFO_STREAM("Loaded transmission: " << info.name_); 70 | } 71 | else if(found_some){ 72 | ROS_ERROR_STREAM("Do not support transmissions that contain only some EPOS actuators: " << info.name_); 73 | } 74 | } 75 | 76 | } 77 | 78 | bool EposHardware::init() { 79 | return epos_manager_.init(); 80 | } 81 | 82 | void EposHardware::update_diagnostics() { 83 | epos_manager_.update_diagnostics(); 84 | } 85 | 86 | void EposHardware::read() { 87 | epos_manager_.read(); 88 | if(robot_transmissions.get()) 89 | robot_transmissions.get()->propagate(); 90 | } 91 | 92 | void EposHardware::write() { 93 | if(robot_transmissions.get()) 94 | robot_transmissions.get()->propagate(); 95 | if(robot_transmissions.get()) 96 | robot_transmissions.get()->propagate(); 97 | epos_manager_.write(); 98 | } 99 | 100 | } 101 | -------------------------------------------------------------------------------- /epos_hardware/src/util/epos_manager.cpp: -------------------------------------------------------------------------------- 1 | #include "epos_hardware/epos_manager.h" 2 | #include 3 | 4 | namespace epos_hardware { 5 | 6 | EposManager::EposManager(hardware_interface::ActuatorStateInterface& asi, 7 | hardware_interface::VelocityActuatorInterface& avi, 8 | hardware_interface::PositionActuatorInterface& api, 9 | ros::NodeHandle& nh, ros::NodeHandle& pnh, 10 | const std::vector& motor_names) 11 | : asi_(&asi), avi_(&avi), api_(&api) { 12 | BOOST_FOREACH(const std::string& motor_name, motor_names) { 13 | ROS_INFO_STREAM("Loading EPOS: " << motor_name); 14 | ros::NodeHandle motor_config_nh(pnh, motor_name); 15 | boost::shared_ptr motor(new Epos(motor_name, nh, motor_config_nh, &epos_factory, *asi_, *avi_, *api_)); 16 | motors_.push_back(motor); 17 | } 18 | } 19 | 20 | 21 | bool EposManager::init() { 22 | bool success = true; 23 | BOOST_FOREACH(const boost::shared_ptr& motor, motors_) { 24 | if(!motor->init()) { 25 | ROS_ERROR_STREAM("Could not configure motor: " << motor->name()); 26 | success = false; 27 | } 28 | } 29 | return success; 30 | } 31 | 32 | void EposManager::update_diagnostics() { 33 | BOOST_FOREACH(const boost::shared_ptr& motor, motors_) { 34 | motor->update_diagnostics(); 35 | } 36 | } 37 | 38 | void EposManager::read() { 39 | BOOST_FOREACH(const boost::shared_ptr& motor, motors_) { 40 | motor->read(); 41 | } 42 | } 43 | 44 | void EposManager::write() { 45 | BOOST_FOREACH(const boost::shared_ptr& motor, motors_) { 46 | motor->write(); 47 | } 48 | } 49 | 50 | 51 | } 52 | -------------------------------------------------------------------------------- /epos_hardware/src/util/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "epos_hardware/utils.h" 2 | #include 3 | #include 4 | 5 | #define MAX_STRING_SIZE 1000 6 | 7 | bool SerialNumberFromHex(const std::string& str, uint64_t* serial_number) { 8 | std::stringstream ss; 9 | ss << std::hex << str; 10 | ss >> *serial_number; 11 | return true; 12 | } 13 | 14 | int GetErrorInfo(unsigned int error_code, std::string* error_string) { 15 | char buffer[MAX_STRING_SIZE]; 16 | int result; 17 | if(result = VCS_GetErrorInfo(error_code, buffer, MAX_STRING_SIZE)) { 18 | *error_string = buffer; 19 | } 20 | return result; 21 | } 22 | 23 | 24 | int GetDeviceNameList(std::vector* device_names, unsigned int* error_code) { 25 | char buffer[MAX_STRING_SIZE]; 26 | int end_of_selection; //BOOL 27 | int result; 28 | 29 | result = VCS_GetDeviceNameSelection(true, buffer, MAX_STRING_SIZE, &end_of_selection, error_code); 30 | if(!result) 31 | return result; 32 | device_names->push_back(buffer); 33 | 34 | while(!end_of_selection) { 35 | result = VCS_GetDeviceNameSelection(false, buffer, MAX_STRING_SIZE, &end_of_selection, error_code); 36 | if(!result) 37 | return result; 38 | device_names->push_back(buffer); 39 | } 40 | 41 | return 1; 42 | } 43 | 44 | int GetProtocolStackNameList(const std::string device_name, std::vector* protocol_stack_names, unsigned int* error_code) { 45 | char buffer[MAX_STRING_SIZE]; 46 | int end_of_selection; //BOOL 47 | int result; 48 | 49 | result = VCS_GetProtocolStackNameSelection((char*)device_name.c_str(), true, buffer, MAX_STRING_SIZE, &end_of_selection, error_code); 50 | if(!result) 51 | return result; 52 | protocol_stack_names->push_back(buffer); 53 | 54 | while(!end_of_selection) { 55 | result = VCS_GetProtocolStackNameSelection((char*)device_name.c_str(), false, buffer, MAX_STRING_SIZE, &end_of_selection, error_code); 56 | if(!result) 57 | return result; 58 | protocol_stack_names->push_back(buffer); 59 | } 60 | 61 | return 1; 62 | } 63 | 64 | 65 | int GetInterfaceNameList(const std::string device_name, const std::string protocol_stack_name, std::vector* interface_names, unsigned int* error_code) { 66 | char buffer[MAX_STRING_SIZE]; 67 | int end_of_selection; //BOOL 68 | int result; 69 | 70 | result = VCS_GetInterfaceNameSelection((char*)device_name.c_str(), (char*)protocol_stack_name.c_str(), true, buffer, MAX_STRING_SIZE, &end_of_selection, error_code); 71 | if(!result) 72 | return result; 73 | interface_names->push_back(buffer); 74 | 75 | while(!end_of_selection) { 76 | result = VCS_GetInterfaceNameSelection((char*)device_name.c_str(), (char*)protocol_stack_name.c_str(), false, buffer, MAX_STRING_SIZE, &end_of_selection, error_code); 77 | if(!result) 78 | return result; 79 | interface_names->push_back(buffer); 80 | } 81 | 82 | return 1; 83 | } 84 | 85 | int GetPortNameList(const std::string device_name, const std::string protocol_stack_name, const std::string interface_name, std::vector* port_names, unsigned int* error_code) { 86 | char buffer[MAX_STRING_SIZE]; 87 | int end_of_selection; //BOOL 88 | int result; 89 | 90 | result = VCS_GetPortNameSelection((char*)device_name.c_str(), (char*)protocol_stack_name.c_str(), (char*)interface_name.c_str(), true, buffer, MAX_STRING_SIZE, &end_of_selection, error_code); 91 | if(!result) 92 | return result; 93 | port_names->push_back(buffer); 94 | 95 | while(!end_of_selection) { 96 | result = VCS_GetPortNameSelection((char*)device_name.c_str(), (char*)protocol_stack_name.c_str(), (char*)interface_name.c_str(), false, buffer, MAX_STRING_SIZE, &end_of_selection, error_code); 97 | if(!result) 98 | return result; 99 | port_names->push_back(buffer); 100 | } 101 | 102 | return 1; 103 | } 104 | 105 | int GetBaudrateList(const std::string device_name, const std::string protocol_stack_name, const std::string interface_name, 106 | const std::string port_name, std::vector* baudrates, unsigned int* error_code) { 107 | unsigned int baudrate; 108 | int end_of_selection; //BOOL 109 | int result; 110 | 111 | result = VCS_GetBaudrateSelection((char*)device_name.c_str(), (char*)protocol_stack_name.c_str(), (char*)interface_name.c_str(), (char*)port_name.c_str(), true, &baudrate, &end_of_selection, error_code); 112 | if(!result) 113 | return result; 114 | baudrates->push_back(baudrate); 115 | 116 | while(!end_of_selection) { 117 | result = VCS_GetBaudrateSelection((char*)device_name.c_str(), (char*)protocol_stack_name.c_str(), (char*)interface_name.c_str(), (char*)port_name.c_str(), false, &baudrate, &end_of_selection, error_code); 118 | if(!result) 119 | return result; 120 | baudrates->push_back(baudrate); 121 | } 122 | 123 | return 1; 124 | } 125 | 126 | 127 | 128 | 129 | 130 | EposFactory::EposFactory() { 131 | } 132 | 133 | DeviceHandlePtr EposFactory::CreateDeviceHandle(const std::string device_name, 134 | const std::string protocol_stack_name, 135 | const std::string interface_name, 136 | const std::string port_name, 137 | unsigned int* error_code) { 138 | const std::string key = device_name + '/' + protocol_stack_name + '/' + interface_name + '/' + port_name; 139 | DeviceHandlePtr handle; 140 | if(!(handle = existing_handles[key].lock())) { // Handle exists 141 | void* raw_handle = VCS_OpenDevice((char*)device_name.c_str(), (char*)protocol_stack_name.c_str(), (char*)interface_name.c_str(), (char*)port_name.c_str(), error_code); 142 | if(!raw_handle) // failed to open device 143 | return DeviceHandlePtr(); 144 | 145 | handle = DeviceHandlePtr(new DeviceHandle(raw_handle)); 146 | existing_handles[key] = handle; 147 | } 148 | 149 | return handle; 150 | } 151 | 152 | NodeHandlePtr EposFactory::CreateNodeHandle(const std::string device_name, 153 | const std::string protocol_stack_name, 154 | const std::string interface_name, 155 | const uint64_t serial_number, 156 | unsigned int* error_code) { 157 | std::vector nodes; 158 | EnumerateNodes(device_name, protocol_stack_name, interface_name, &nodes, error_code); 159 | BOOST_FOREACH(const EnumeratedNode& node, nodes) { 160 | if(node.serial_number == serial_number) { 161 | return CreateNodeHandle(node, error_code); 162 | } 163 | } 164 | return NodeHandlePtr(); 165 | } 166 | 167 | NodeHandlePtr EposFactory::CreateNodeHandle(const EnumeratedNode& node, 168 | unsigned int* error_code) { 169 | DeviceHandlePtr device_handle = CreateDeviceHandle(node.device_name, node.protocol_stack_name, node.interface_name, node.port_name, error_code); 170 | if(!device_handle) 171 | return NodeHandlePtr(); 172 | return NodeHandlePtr(new NodeHandle(device_handle, node.node_id)); 173 | } 174 | 175 | 176 | 177 | int EposFactory::EnumerateNodes(const std::string device_name, const std::string protocol_stack_name, const std::string interface_name, 178 | const std::string port_name, std::vector* nodes, unsigned int* error_code) { 179 | DeviceHandlePtr handle; 180 | if(!(handle = CreateDeviceHandle(device_name, protocol_stack_name, interface_name, port_name, error_code))){ 181 | return 0; 182 | } 183 | for(unsigned short i = 1; i < 127; ++i) { 184 | EnumeratedNode node; 185 | node.device_name = device_name; 186 | node.protocol_stack_name = protocol_stack_name; 187 | node.interface_name = interface_name; 188 | node.port_name = port_name; 189 | node.node_id = i; 190 | if(!VCS_GetVersion(handle->ptr, i, &node.hardware_version, &node.software_version, &node.application_number, &node.application_version, error_code)){ 191 | return 1; 192 | } 193 | unsigned int bytes_read; 194 | if(!VCS_GetObject(handle->ptr, i, 0x2004, 0x00, &node.serial_number, 8, &bytes_read, error_code)){ 195 | node.serial_number = 0; 196 | } 197 | nodes->push_back(node); 198 | } 199 | return 1; 200 | } 201 | 202 | 203 | int EposFactory::EnumerateNodes(const std::string device_name, const std::string protocol_stack_name, const std::string interface_name, 204 | std::vector* nodes, unsigned int* error_code) { 205 | std::vector port_names; 206 | if(GetPortNameList(device_name, protocol_stack_name, interface_name, &port_names, error_code)) { 207 | BOOST_FOREACH(const std::string& port_name, port_names) { 208 | if(!EnumerateNodes(device_name, protocol_stack_name, interface_name, port_name, nodes, error_code)){ 209 | return 0; 210 | } 211 | } 212 | return 1; 213 | } 214 | else 215 | return 0; 216 | } 217 | -------------------------------------------------------------------------------- /epos_library/.gitignore: -------------------------------------------------------------------------------- 1 | EPOS-Linux-Library-En.zip 2 | EPOS_Linux_Library/ 3 | -------------------------------------------------------------------------------- /epos_library/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package epos_library 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.3 (2015-06-18) 6 | ------------------ 7 | 8 | 0.0.2 (2015-03-06) 9 | ------------------ 10 | 11 | 0.0.1 (2015-01-30) 12 | ------------------ 13 | * initial release of 64-bit EPOS Linux libraries 14 | * Contributors: Mitchell Wills 15 | -------------------------------------------------------------------------------- /epos_library/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(epos_library) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | # find the 32 or 64 bit libraries 7 | if(CMAKE_SIZEOF_VOID_P EQUAL 8) 8 | set(ARCH "x86_64") 9 | else() 10 | set(ARCH "x86") 11 | endif() 12 | message(STATUS "Detected architecture: ${ARCH}") 13 | 14 | catkin_package( 15 | INCLUDE_DIRS include 16 | LIBRARIES ftd2xx EposCmd 17 | ) 18 | 19 | ########### 20 | ## Build ## 21 | ########### 22 | 23 | include_directories(include) 24 | 25 | set(ftd2xx_LIBRARY ${PROJECT_SOURCE_DIR}/lib/${ARCH}/libftd2xx.so.1.1.12) 26 | set(EposCmd_LIBRARY ${PROJECT_SOURCE_DIR}/lib/${ARCH}/libEposCmd.so.5.0.1.0) 27 | 28 | add_library(ftd2xx SHARED ${ftd2xx_LIBRARY}) 29 | add_custom_command(TARGET ftd2xx POST_BUILD COMMAND cp ${ftd2xx_LIBRARY} ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/libftd2xx.so) 30 | set_target_properties(ftd2xx PROPERTIES LINKER_LANGUAGE CXX) 31 | 32 | add_library(EposCmd SHARED ${EposCmd_LIBRARY}) 33 | add_custom_command(TARGET EposCmd POST_BUILD COMMAND cp ${EposCmd_LIBRARY} ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/libEposCmd.so) 34 | set_target_properties(EposCmd PROPERTIES LINKER_LANGUAGE CXX) 35 | 36 | ############# 37 | ## Install ## 38 | ############# 39 | 40 | install(TARGETS ftd2xx EposCmd 41 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 42 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 43 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 44 | ) 45 | 46 | install(DIRECTORY include/${PROJECT_NAME}/ 47 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 48 | FILES_MATCHING PATTERN "*.h" 49 | PATTERN ".svn" EXCLUDE 50 | ) 51 | -------------------------------------------------------------------------------- /epos_library/include/epos_library/Definitions.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************************************************************* 2 | ** maxon motor ag, CH-6072 Sachseln 3 | ************************************************************************************************************************************** 4 | ** 5 | ** FILE: Definitions.h 6 | ** 7 | ** Summary: Functions for Linux shared library 8 | ** 9 | ** Date: 10.10.2014 10 | ** Target: x86, x86_64, arm sf/hf 11 | ** Written by: maxon motor ag, CH-6072 Sachseln 12 | ** 13 | ** Changes: 4.8.1.0 (15.12.10): initial version 14 | ** 4.8.2.0 (14.03.11): usb interface related bugfix's 15 | ** 4.9.1.0 (27.07.12): ipm mode bugfix, kernel 2.6 support, ftdi driver update 16 | ** 4.9.2.0 (26.04.13): rs232 baudrate bugfix, new functions: VCS_GetHomingState, VCS_WaitForHomingAttained, ** VCS_GetVelocityIsAveraged, VCS_GetCurrentIsAveraged 17 | ** 5.0.1.0 (10.10.14): x86_64, arm sf/hf support, new functions: VCS_GetDriverInfo, bugfix: VCS_GetErrorInfo 18 | *************************************************************************************************************************************/ 19 | 20 | #ifndef _H_LINUX_EPOSCMD_ 21 | #define _H_LINUX_EPOSCMD_ 22 | 23 | //Communication 24 | int CreateCommunication(); 25 | int DeleteCommunication(); 26 | 27 | // INITIALISATION FUNCTIONS 28 | #define Initialisation_DllExport extern "C" 29 | #define HelpFunctions_DllExport extern "C" 30 | // CONFIGURATION FUNCTIONS 31 | #define Configuration_DllExport extern "C" 32 | // OPERATION FUNCTIONS 33 | #define Status_DllExport extern "C" 34 | #define StateMachine_DllExport extern "C" 35 | #define ErrorHandling_DllExport extern "C" 36 | #define MotionInfo_DllExport extern "C" 37 | #define ProfilePositionMode_DllExport extern "C" 38 | #define ProfileVelocityMode_DllExport extern "C" 39 | #define HomingMode_DllExport extern "C" 40 | #define InterpolatedPositionMode_DllExport extern "C" 41 | #define PositionMode_DllExport extern "C" 42 | #define VelocityMode_DllExport extern "C" 43 | #define CurrentMode_DllExport extern "C" 44 | #define MasterEncoderMode_DllExport extern "C" 45 | #define StepDirectionMode_DllExport extern "C" 46 | #define InputsOutputs_DllExport extern "C" 47 | // LOW LAYER FUNCTIONS 48 | #define CanLayer_DllExport extern "C" 49 | 50 | /************************************************************************************************************************************* 51 | * INITIALISATION FUNCTIONS 52 | *************************************************************************************************************************************/ 53 | 54 | //Communication 55 | Initialisation_DllExport void* VCS_OpenDevice(char* DeviceName, char* ProtocolStackName, char* InterfaceName, char* PortName, unsigned int* pErrorCode); 56 | Initialisation_DllExport int VCS_SetProtocolStackSettings(void* KeyHandle, unsigned int Baudrate, unsigned int Timeout, unsigned int* pErrorCode); 57 | Initialisation_DllExport int VCS_GetProtocolStackSettings(void* KeyHandle, unsigned int* pBaudrate, unsigned int* pTimeout, unsigned int* pErrorCode); 58 | Initialisation_DllExport int VCS_CloseDevice(void* KeyHandle, unsigned int* pErrorCode); 59 | Initialisation_DllExport int VCS_CloseAllDevices(unsigned int* pErrorCode); 60 | 61 | //Info 62 | HelpFunctions_DllExport int VCS_GetVersion(void* KeyHandle, unsigned short NodeId, unsigned short* pHardwareVersion, unsigned short* pSoftwareVersion, unsigned short* pApplicationNumber, unsigned short* pApplicationVersion, unsigned int* pErrorCode); 63 | HelpFunctions_DllExport int VCS_GetErrorInfo(unsigned int ErrorCodeValue, char* pErrorInfo, unsigned short MaxStrSize); 64 | HelpFunctions_DllExport int VCS_GetDriverInfo(char* p_pszLibraryName, unsigned short p_usMaxLibraryNameStrSize,char* p_pszLibraryVersion, unsigned short p_usMaxLibraryVersionStrSize, unsigned int* p_pErrorCode); 65 | 66 | //Advanced Functions 67 | HelpFunctions_DllExport int VCS_GetDeviceNameSelection(int StartOfSelection, char* pDeviceNameSel, unsigned short MaxStrSize, int* pEndOfSelection, unsigned int* pErrorCode); 68 | HelpFunctions_DllExport int VCS_GetProtocolStackNameSelection(char* DeviceName, int StartOfSelection, char* pProtocolStackNameSel, unsigned short MaxStrSize, int* pEndOfSelection, unsigned int* pErrorCode); 69 | HelpFunctions_DllExport int VCS_GetInterfaceNameSelection(char* DeviceName, char* ProtocolStackName, int StartOfSelection, char* pInterfaceNameSel, unsigned short MaxStrSize, int* pEndOfSelection, unsigned int* pErrorCode); 70 | HelpFunctions_DllExport int VCS_GetPortNameSelection(char* DeviceName, char* ProtocolStackName, char* InterfaceName, int StartOfSelection, char* pPortSel, unsigned short MaxStrSize, int* pEndOfSelection, unsigned int* pErrorCode); 71 | HelpFunctions_DllExport int VCS_GetBaudrateSelection(char* DeviceName, char* ProtocolStackName, char* InterfaceName, char* PortName, int StartOfSelection, unsigned int* pBaudrateSel, int* pEndOfSelection, unsigned int* pErrorCode); 72 | HelpFunctions_DllExport int VCS_GetKeyHandle(char* DeviceName, char* ProtocolStackName, char* InterfaceName, char* PortName, void** pKeyHandle, unsigned int* pErrorCode); 73 | HelpFunctions_DllExport int VCS_GetDeviceName(void* KeyHandle, char* pDeviceName, unsigned short MaxStrSize, unsigned int* pErrorCode); 74 | HelpFunctions_DllExport int VCS_GetProtocolStackName(void* KeyHandle, char* pProtocolStackName, unsigned short MaxStrSize, unsigned int* pErrorCode); 75 | HelpFunctions_DllExport int VCS_GetInterfaceName(void* KeyHandle, char* pInterfaceName, unsigned short MaxStrSize, unsigned int* pErrorCode); 76 | HelpFunctions_DllExport int VCS_GetPortName(void* KeyHandle, char* pPortName, unsigned short MaxStrSize, unsigned int* pErrorCode); 77 | 78 | /************************************************************************************************************************************* 79 | * CONFIGURATION FUNCTIONS 80 | *************************************************************************************************************************************/ 81 | 82 | //General 83 | Configuration_DllExport int VCS_SetObject(void* KeyHandle, unsigned short NodeId, unsigned short ObjectIndex, unsigned char ObjectSubIndex, void* pData, unsigned int NbOfBytesToWrite, unsigned int* pNbOfBytesWritten, unsigned int* pErrorCode); 84 | Configuration_DllExport int VCS_GetObject(void* KeyHandle, unsigned short NodeId, unsigned short ObjectIndex, unsigned char ObjectSubIndex, void* pData, unsigned int NbOfBytesToRead, unsigned int* pNbOfBytesRead, unsigned int* pErrorCode); 85 | Configuration_DllExport int VCS_Restore(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 86 | Configuration_DllExport int VCS_Store(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 87 | 88 | //Advanced Functions 89 | //Motor 90 | Configuration_DllExport int VCS_SetMotorType(void* KeyHandle, unsigned short NodeId, unsigned short MotorType, unsigned int* pErrorCode); 91 | Configuration_DllExport int VCS_SetDcMotorParameter(void* KeyHandle, unsigned short NodeId, unsigned short NominalCurrent, unsigned short MaxOutputCurrent, unsigned short ThermalTimeConstant, unsigned int* pErrorCode); 92 | Configuration_DllExport int VCS_SetEcMotorParameter(void* KeyHandle, unsigned short NodeId, unsigned short NominalCurrent, unsigned short MaxOutputCurrent, unsigned short ThermalTimeConstant, unsigned char NbOfPolePairs, unsigned int* pErrorCode); 93 | Configuration_DllExport int VCS_GetMotorType(void* KeyHandle, unsigned short NodeId, unsigned short* pMotorType, unsigned int* pErrorCode); 94 | Configuration_DllExport int VCS_GetDcMotorParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pNominalCurrent, unsigned short* pMaxOutputCurrent, unsigned short* pThermalTimeConstant, unsigned int* pErrorCode); 95 | Configuration_DllExport int VCS_GetEcMotorParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pNominalCurrent, unsigned short* pMaxOutputCurrent, unsigned short* pThermalTimeConstant, unsigned char* pNbOfPolePairs, unsigned int* pErrorCode); 96 | 97 | //Sensor 98 | Configuration_DllExport int VCS_SetSensorType(void* KeyHandle, unsigned short NodeId, unsigned short SensorType, unsigned int* pErrorCode); 99 | Configuration_DllExport int VCS_SetIncEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned int EncoderResolution, int InvertedPolarity, unsigned int* pErrorCode); 100 | Configuration_DllExport int VCS_SetHallSensorParameter(void* KeyHandle, unsigned short NodeId, int InvertedPolarity, unsigned int* pErrorCode); 101 | Configuration_DllExport int VCS_SetSsiAbsEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned short DataRate, unsigned short NbOfMultiTurnDataBits, unsigned short NbOfSingleTurnDataBits, int InvertedPolarity, unsigned int* pErrorCode); 102 | Configuration_DllExport int VCS_GetSensorType(void* KeyHandle, unsigned short NodeId, unsigned short* pSensorType, unsigned int* pErrorCode); 103 | Configuration_DllExport int VCS_GetIncEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned int* pEncoderResolution, int* pInvertedPolarity, unsigned int* pErrorCode); 104 | Configuration_DllExport int VCS_GetHallSensorParameter(void* KeyHandle, unsigned short NodeId, int* pInvertedPolarity, unsigned int* pErrorCode); 105 | Configuration_DllExport int VCS_GetSsiAbsEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pDataRate, unsigned short* pNbOfMultiTurnDataBits, unsigned short* pNbOfSingleTurnDataBits, int* pInvertedPolarity, unsigned int* pErrorCode); 106 | 107 | //Safety 108 | Configuration_DllExport int VCS_SetMaxFollowingError(void* KeyHandle, unsigned short NodeId, unsigned int MaxFollowingError, unsigned int* pErrorCode); 109 | Configuration_DllExport int VCS_GetMaxFollowingError(void* KeyHandle, unsigned short NodeId, unsigned int* pMaxFollowingError, unsigned int* pErrorCode); 110 | Configuration_DllExport int VCS_SetMaxProfileVelocity(void* KeyHandle, unsigned short NodeId, unsigned int MaxProfileVelocity, unsigned int* pErrorCode); 111 | Configuration_DllExport int VCS_GetMaxProfileVelocity(void* KeyHandle, unsigned short NodeId, unsigned int* pMaxProfileVelocity, unsigned int* pErrorCode); 112 | Configuration_DllExport int VCS_SetMaxAcceleration(void* KeyHandle, unsigned short NodeId, unsigned int MaxAcceleration, unsigned int* pErrorCode); 113 | Configuration_DllExport int VCS_GetMaxAcceleration(void* KeyHandle, unsigned short NodeId, unsigned int* pMaxAcceleration, unsigned int* pErrorCode); 114 | 115 | //Position Regulator 116 | Configuration_DllExport int VCS_SetPositionRegulatorGain(void* KeyHandle, unsigned short NodeId, unsigned short P, unsigned short I, unsigned short D, unsigned int* pErrorCode); 117 | Configuration_DllExport int VCS_SetPositionRegulatorFeedForward(void* KeyHandle, unsigned short NodeId, unsigned short VelocityFeedForward, unsigned short AccelerationFeedForward, unsigned int* pErrorCode); 118 | Configuration_DllExport int VCS_GetPositionRegulatorGain(void* KeyHandle, unsigned short NodeId, unsigned short* pP, unsigned short* pI, unsigned short* pD, unsigned int* pErrorCode); 119 | Configuration_DllExport int VCS_GetPositionRegulatorFeedForward(void* KeyHandle, unsigned short NodeId, unsigned short* pVelocityFeedForward, unsigned short* pAccelerationFeedForward, unsigned int* pErrorCode); 120 | 121 | //Velocity Regulator 122 | Configuration_DllExport int VCS_SetVelocityRegulatorGain(void* KeyHandle, unsigned short NodeId, unsigned short P, unsigned short I, unsigned int* pErrorCode); 123 | Configuration_DllExport int VCS_SetVelocityRegulatorFeedForward(void* KeyHandle, unsigned short NodeId, unsigned short VelocityFeedForward, unsigned short AccelerationFeedForward, unsigned int* pErrorCode); 124 | Configuration_DllExport int VCS_GetVelocityRegulatorGain(void* KeyHandle, unsigned short NodeId, unsigned short* pP, unsigned short* pI, unsigned int* pErrorCode); 125 | Configuration_DllExport int VCS_GetVelocityRegulatorFeedForward(void* KeyHandle, unsigned short NodeId, unsigned short* pVelocityFeedForward, unsigned short* pAccelerationFeedForward, unsigned int* pErrorCode); 126 | 127 | //Current Regulator 128 | Configuration_DllExport int VCS_SetCurrentRegulatorGain(void* KeyHandle, unsigned short NodeId, unsigned short P, unsigned short I, unsigned int* pErrorCode); 129 | Configuration_DllExport int VCS_GetCurrentRegulatorGain(void* KeyHandle, unsigned short NodeId, unsigned short* pP, unsigned short* pI, unsigned int* pErrorCode); 130 | 131 | //Inputs/Outputs 132 | Configuration_DllExport int VCS_DigitalInputConfiguration(void* KeyHandle, unsigned short NodeId, unsigned short DigitalInputNb, unsigned short Configuration, int Mask, int Polarity, int ExecutionMask, unsigned int* pErrorCode); 133 | Configuration_DllExport int VCS_DigitalOutputConfiguration(void* KeyHandle, unsigned short NodeId, unsigned short DigitalOutputNb, unsigned short Configuration, int State, int Mask, int Polarity, unsigned int* pErrorCode); 134 | Configuration_DllExport int VCS_AnalogInputConfiguration(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNb, unsigned short Configuration, int ExecutionMask, unsigned int* pErrorCode); 135 | 136 | //Units 137 | Configuration_DllExport int VCS_SetVelocityUnits(void* KeyHandle, unsigned short NodeId, unsigned char VelDimension, signed char VelNotation, unsigned int* pErrorCode); 138 | Configuration_DllExport int VCS_GetVelocityUnits(void* KeyHandle, unsigned short NodeId, unsigned char* pVelDimension, char* pVelNotation, unsigned int* pErrorCode); 139 | 140 | /************************************************************************************************************************************* 141 | * OPERATION FUNCTIONS 142 | *************************************************************************************************************************************/ 143 | 144 | //OperationMode 145 | Status_DllExport int VCS_SetOperationMode(void* KeyHandle, unsigned short NodeId, char OperationMode, unsigned int* pErrorCode); 146 | Status_DllExport int VCS_GetOperationMode(void* KeyHandle, unsigned short NodeId, char* pOperationMode, unsigned int* pErrorCode); 147 | 148 | //StateMachine 149 | StateMachine_DllExport int VCS_ResetDevice(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 150 | StateMachine_DllExport int VCS_SetState(void* KeyHandle, unsigned short NodeId, unsigned short State, unsigned int* pErrorCode); 151 | StateMachine_DllExport int VCS_SetEnableState(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 152 | StateMachine_DllExport int VCS_SetDisableState(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 153 | StateMachine_DllExport int VCS_SetQuickStopState(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 154 | StateMachine_DllExport int VCS_ClearFault(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 155 | StateMachine_DllExport int VCS_GetState(void* KeyHandle, unsigned short NodeId, unsigned short* pState, unsigned int* pErrorCode); 156 | StateMachine_DllExport int VCS_GetEnableState(void* KeyHandle, unsigned short NodeId, int* pIsEnabled, unsigned int* pErrorCode); 157 | StateMachine_DllExport int VCS_GetDisableState(void* KeyHandle, unsigned short NodeId, int* pIsDisabled, unsigned int* pErrorCode); 158 | StateMachine_DllExport int VCS_GetQuickStopState(void* KeyHandle, unsigned short NodeId, int* pIsQuickStopped, unsigned int* pErrorCode); 159 | StateMachine_DllExport int VCS_GetFaultState(void* KeyHandle, unsigned short NodeId, int* pIsInFault, unsigned int* pErrorCode); 160 | 161 | //ErrorHandling 162 | ErrorHandling_DllExport int VCS_GetNbOfDeviceError(void* KeyHandle, unsigned short NodeId, unsigned char *pNbDeviceError, unsigned int *pErrorCode); 163 | ErrorHandling_DllExport int VCS_GetDeviceErrorCode(void* KeyHandle, unsigned short NodeId, unsigned char DeviceErrorNumber, unsigned int *pDeviceErrorCode, unsigned int *pErrorCode); 164 | 165 | //Motion Info 166 | MotionInfo_DllExport int VCS_GetMovementState(void* KeyHandle, unsigned short NodeId, int* pTargetReached, unsigned int* pErrorCode); 167 | MotionInfo_DllExport int VCS_GetPositionIs(void* KeyHandle, unsigned short NodeId, int* pPositionIs, unsigned int* pErrorCode); 168 | MotionInfo_DllExport int VCS_GetVelocityIs(void* KeyHandle, unsigned short NodeId, int* pVelocityIs, unsigned int* pErrorCode); 169 | MotionInfo_DllExport int VCS_GetVelocityIsAveraged(void* KeyHandle, unsigned short NodeId, int* pVelocityIsAveraged, unsigned int* pErrorCode); 170 | MotionInfo_DllExport int VCS_GetCurrentIs(void* KeyHandle, unsigned short NodeId, short* pCurrentIs, unsigned int* pErrorCode); 171 | MotionInfo_DllExport int VCS_GetCurrentIsAveraged(void* KeyHandle, unsigned short NodeId, short* pCurrentIsAveraged, unsigned int* pErrorCode); 172 | MotionInfo_DllExport int VCS_WaitForTargetReached(void* KeyHandle, unsigned short NodeId, unsigned int Timeout, unsigned int* pErrorCode); 173 | 174 | //Profile Position Mode 175 | ProfilePositionMode_DllExport int VCS_ActivateProfilePositionMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 176 | ProfilePositionMode_DllExport int VCS_SetPositionProfile(void* KeyHandle, unsigned short NodeId, unsigned int ProfileVelocity, unsigned int ProfileAcceleration, unsigned int ProfileDeceleration, unsigned int* pErrorCode); 177 | ProfilePositionMode_DllExport int VCS_GetPositionProfile(void* KeyHandle, unsigned short NodeId, unsigned int* pProfileVelocity, unsigned int* pProfileAcceleration, unsigned int* pProfileDeceleration, unsigned int* pErrorCode); 178 | ProfilePositionMode_DllExport int VCS_MoveToPosition(void* KeyHandle, unsigned short NodeId, long TargetPosition, int Absolute, int Immediately, unsigned int* pErrorCode); 179 | ProfilePositionMode_DllExport int VCS_GetTargetPosition(void* KeyHandle, unsigned short NodeId, long* pTargetPosition, unsigned int* pErrorCode); 180 | ProfilePositionMode_DllExport int VCS_HaltPositionMovement(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 181 | 182 | //Advanced Functions 183 | ProfilePositionMode_DllExport int VCS_EnablePositionWindow(void* KeyHandle, unsigned short NodeId, unsigned int PositionWindow, unsigned short PositionWindowTime, unsigned int* pErrorCode); 184 | ProfilePositionMode_DllExport int VCS_DisablePositionWindow(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 185 | 186 | //Profile Velocity Mode 187 | ProfileVelocityMode_DllExport int VCS_ActivateProfileVelocityMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 188 | ProfileVelocityMode_DllExport int VCS_SetVelocityProfile(void* KeyHandle, unsigned short NodeId, unsigned int ProfileAcceleration, unsigned int ProfileDeceleration, unsigned int* pErrorCode); 189 | ProfileVelocityMode_DllExport int VCS_GetVelocityProfile(void* KeyHandle, unsigned short NodeId, unsigned int* pProfileAcceleration, unsigned int* pProfileDeceleration, unsigned int* pErrorCode); 190 | ProfileVelocityMode_DllExport int VCS_MoveWithVelocity(void* KeyHandle, unsigned short NodeId, long TargetVelocity, unsigned int* pErrorCode); 191 | ProfileVelocityMode_DllExport int VCS_GetTargetVelocity(void* KeyHandle, unsigned short NodeId, long* pTargetVelocity, unsigned int* pErrorCode); 192 | ProfileVelocityMode_DllExport int VCS_HaltVelocityMovement(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 193 | 194 | //Advanced Functions 195 | ProfileVelocityMode_DllExport int VCS_EnableVelocityWindow(void* KeyHandle, unsigned short NodeId, unsigned int VelocityWindow, unsigned short VelocityWindowTime, unsigned int* pErrorCode); 196 | ProfileVelocityMode_DllExport int VCS_DisableVelocityWindow(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 197 | 198 | //Homing Mode 199 | HomingMode_DllExport int VCS_ActivateHomingMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 200 | HomingMode_DllExport int VCS_SetHomingParameter(void* KeyHandle, unsigned short NodeId, unsigned int HomingAcceleration, unsigned int SpeedSwitch, unsigned int SpeedIndex, int HomeOffset, unsigned short CurrentTreshold, int HomePosition, unsigned int* pErrorCode); 201 | HomingMode_DllExport int VCS_GetHomingParameter(void* KeyHandle, unsigned short NodeId, unsigned int* pHomingAcceleration, unsigned int* pSpeedSwitch, unsigned int* pSpeedIndex, int* pHomeOffset, unsigned short* pCurrentTreshold, int* pHomePosition, unsigned int* pErrorCode); 202 | HomingMode_DllExport int VCS_FindHome(void* KeyHandle, unsigned short NodeId, signed char HomingMethod, unsigned int* pErrorCode); 203 | HomingMode_DllExport int VCS_StopHoming(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 204 | HomingMode_DllExport int VCS_DefinePosition(void* KeyHandle, unsigned short NodeId, int HomePosition, unsigned int* pErrorCode); 205 | HomingMode_DllExport int VCS_WaitForHomingAttained(void* KeyHandle, unsigned short NodeId, int Timeout, unsigned int* pErrorCode); 206 | HomingMode_DllExport int VCS_GetHomingState(void* KeyHandle, unsigned short NodeId, int* pHomingAttained, int* pHomingError, unsigned int* pErrorCode); 207 | 208 | //Interpolated Position Mode 209 | InterpolatedPositionMode_DllExport int VCS_ActivateInterpolatedPositionMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 210 | InterpolatedPositionMode_DllExport int VCS_SetIpmBufferParameter(void* KeyHandle, unsigned short NodeId, unsigned short UnderflowWarningLimit, unsigned short OverflowWarningLimit, unsigned int* pErrorCode); 211 | InterpolatedPositionMode_DllExport int VCS_GetIpmBufferParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pUnderflowWarningLimit, unsigned short* pOverflowWarningLimit, unsigned int* pMaxBufferSize, unsigned int* pErrorCode); 212 | InterpolatedPositionMode_DllExport int VCS_ClearIpmBuffer(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 213 | InterpolatedPositionMode_DllExport int VCS_GetFreeIpmBufferSize(void* KeyHandle, unsigned short NodeId, unsigned int* pBufferSize, unsigned int* pErrorCode); 214 | InterpolatedPositionMode_DllExport int VCS_AddPvtValueToIpmBuffer(void* KeyHandle, unsigned short NodeId, long Position, long Velocity, unsigned char Time, unsigned int* pErrorCode); 215 | InterpolatedPositionMode_DllExport int VCS_StartIpmTrajectory(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 216 | InterpolatedPositionMode_DllExport int VCS_StopIpmTrajectory(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 217 | InterpolatedPositionMode_DllExport int VCS_GetIpmStatus(void* KeyHandle, unsigned short NodeId, int* pTrajectoryRunning, int* pIsUnderflowWarning, int* pIsOverflowWarning, int* pIsVelocityWarning, int* pIsAccelerationWarning, int* pIsUnderflowError, int* pIsOverflowError, int* pIsVelocityError, int* pIsAccelerationError, unsigned int* pErrorCode); 218 | 219 | //Position Mode 220 | PositionMode_DllExport int VCS_ActivatePositionMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 221 | PositionMode_DllExport int VCS_SetPositionMust(void* KeyHandle, unsigned short NodeId, long PositionMust, unsigned int* pErrorCode); 222 | PositionMode_DllExport int VCS_GetPositionMust(void* KeyHandle, unsigned short NodeId, long* pPositionMust, unsigned int* pErrorCode); 223 | 224 | //Advanced Functions 225 | PositionMode_DllExport int VCS_ActivateAnalogPositionSetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, float Scaling, long Offset, unsigned int* pErrorCode); 226 | PositionMode_DllExport int VCS_DeactivateAnalogPositionSetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, unsigned int* pErrorCode); 227 | PositionMode_DllExport int VCS_EnableAnalogPositionSetpoint(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 228 | PositionMode_DllExport int VCS_DisableAnalogPositionSetpoint(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 229 | 230 | //Velocity Mode 231 | VelocityMode_DllExport int VCS_ActivateVelocityMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 232 | VelocityMode_DllExport int VCS_SetVelocityMust(void* KeyHandle, unsigned short NodeId, long VelocityMust, unsigned int* pErrorCode); 233 | VelocityMode_DllExport int VCS_GetVelocityMust(void* KeyHandle, unsigned short NodeId, long* pVelocityMust, unsigned int* pErrorCode); 234 | 235 | //Advanced Functions 236 | VelocityMode_DllExport int VCS_ActivateAnalogVelocitySetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, float Scaling, long Offset, unsigned int* pErrorCode); 237 | VelocityMode_DllExport int VCS_DeactivateAnalogVelocitySetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, unsigned int* pErrorCode); 238 | VelocityMode_DllExport int VCS_EnableAnalogVelocitySetpoint(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 239 | VelocityMode_DllExport int VCS_DisableAnalogVelocitySetpoint(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 240 | 241 | //Current Mode 242 | CurrentMode_DllExport int VCS_ActivateCurrentMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 243 | CurrentMode_DllExport int VCS_SetCurrentMust(void* KeyHandle, unsigned short NodeId, short CurrentMust, unsigned int* pErrorCode); 244 | CurrentMode_DllExport int VCS_GetCurrentMust(void* KeyHandle, unsigned short NodeId, short* pCurrentMust, unsigned int* pErrorCode); 245 | 246 | //Advanced Functions 247 | CurrentMode_DllExport int VCS_ActivateAnalogCurrentSetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, float Scaling, short Offset, unsigned int* pErrorCode); 248 | CurrentMode_DllExport int VCS_DeactivateAnalogCurrentSetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, unsigned int* pErrorCode); 249 | CurrentMode_DllExport int VCS_EnableAnalogCurrentSetpoint(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 250 | CurrentMode_DllExport int VCS_DisableAnalogCurrentSetpoint(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 251 | 252 | //MasterEncoder Mode 253 | MasterEncoderMode_DllExport int VCS_ActivateMasterEncoderMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 254 | MasterEncoderMode_DllExport int VCS_SetMasterEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned short ScalingNumerator, unsigned short ScalingDenominator, unsigned char Polarity, unsigned int MaxVelocity, unsigned int MaxAcceleration, unsigned int* pErrorCode); 255 | MasterEncoderMode_DllExport int VCS_GetMasterEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pScalingNumerator, unsigned short* pScalingDenominator, unsigned char* pPolarity, unsigned int* pMaxVelocity, unsigned int* pMaxAcceleration, unsigned int* pErrorCode); 256 | 257 | //StepDirection Mode 258 | StepDirectionMode_DllExport int VCS_ActivateStepDirectionMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 259 | StepDirectionMode_DllExport int VCS_SetStepDirectionParameter(void* KeyHandle, unsigned short NodeId, unsigned short ScalingNumerator, unsigned short ScalingDenominator, unsigned char Polarity, unsigned int MaxVelocity, unsigned int MaxAcceleration, unsigned int* pErrorCode); 260 | StepDirectionMode_DllExport int VCS_GetStepDirectionParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pScalingNumerator, unsigned short* pScalingDenominator, unsigned char* pPolarity, unsigned int* pMaxVelocity, unsigned int* pMaxAcceleration, unsigned int* pErrorCode); 261 | 262 | //Inputs Outputs 263 | //General 264 | InputsOutputs_DllExport int VCS_GetAllDigitalInputs(void* KeyHandle, unsigned short NodeId, unsigned short* pInputs, unsigned int* pErrorCode); 265 | InputsOutputs_DllExport int VCS_GetAllDigitalOutputs(void* KeyHandle, unsigned short NodeId, unsigned short* pOutputs, unsigned int* pErrorCode); 266 | InputsOutputs_DllExport int VCS_SetAllDigitalOutputs(void* KeyHandle, unsigned short NodeId, unsigned short Outputs, unsigned int* pErrorCode); 267 | InputsOutputs_DllExport int VCS_GetAnalogInput(void* KeyHandle, unsigned short NodeId, unsigned short InputNumber, unsigned short* pAnalogValue, unsigned int* pErrorCode); 268 | InputsOutputs_DllExport int VCS_SetAnalogOutput(void* KeyHandle, unsigned short NodeId, unsigned short OutputNumber, unsigned short AnalogValue, unsigned int* pErrorCode); 269 | 270 | //Position Compare 271 | InputsOutputs_DllExport int VCS_SetPositionCompareParameter(void* KeyHandle, unsigned short NodeId, unsigned char OperationalMode, unsigned char IntervalMode, unsigned char DirectionDependency, unsigned short IntervalWidth, unsigned short IntervalRepetitions, unsigned short PulseWidth, unsigned int* pErrorCode); 272 | InputsOutputs_DllExport int VCS_GetPositionCompareParameter(void* KeyHandle, unsigned short NodeId, unsigned char* pOperationalMode, unsigned char* pIntervalMode, unsigned char* pDirectionDependency, unsigned short* pIntervalWidth, unsigned short* pIntervalRepetitions, unsigned short* pPulseWidth, unsigned int* pErrorCode); 273 | InputsOutputs_DllExport int VCS_ActivatePositionCompare(void* KeyHandle, unsigned short NodeId, unsigned short DigitalOutputNumber, int Polarity, unsigned int* pErrorCode); 274 | InputsOutputs_DllExport int VCS_DeactivatePositionCompare(void* KeyHandle, unsigned short NodeId, unsigned short DigitalOutputNumber, unsigned int* pErrorCode); 275 | InputsOutputs_DllExport int VCS_EnablePositionCompare(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 276 | InputsOutputs_DllExport int VCS_DisablePositionCompare(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 277 | InputsOutputs_DllExport int VCS_SetPositionCompareReferencePosition(void* KeyHandle, unsigned short NodeId, long ReferencePosition, unsigned int* pErrorCode); 278 | 279 | //Position Marker 280 | InputsOutputs_DllExport int VCS_SetPositionMarkerParameter(void* KeyHandle, unsigned short NodeId, unsigned char PositionMarkerEdgeType, unsigned char PositionMarkerMode, unsigned int* pErrorCode); 281 | InputsOutputs_DllExport int VCS_GetPositionMarkerParameter(void* KeyHandle, unsigned short NodeId, unsigned char* pPositionMarkerEdgeType, unsigned char* pPositionMarkerMode, unsigned int* pErrorCode); 282 | InputsOutputs_DllExport int VCS_ActivatePositionMarker(void* KeyHandle, unsigned short NodeId, unsigned short DigitalInputNumber, int Polarity, unsigned int* pErrorCode); 283 | InputsOutputs_DllExport int VCS_DeactivatePositionMarker(void* KeyHandle, unsigned short NodeId, unsigned short DigitalInputNumber, unsigned int* pErrorCode); 284 | InputsOutputs_DllExport int VCS_ReadPositionMarkerCounter(void* KeyHandle, unsigned short NodeId, unsigned short* pCount, unsigned int* pErrorCode); 285 | InputsOutputs_DllExport int VCS_ReadPositionMarkerCapturedPosition(void* KeyHandle, unsigned short NodeId, unsigned short CounterIndex, long* pCapturedPosition, unsigned int* pErrorCode); 286 | InputsOutputs_DllExport int VCS_ResetPositionMarkerCounter(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); 287 | 288 | /************************************************************************************************************************************* 289 | * LOW LAYER FUNCTIONS 290 | *************************************************************************************************************************************/ 291 | 292 | //CanLayer Functions 293 | CanLayer_DllExport int VCS_SendCANFrame(void* KeyHandle, unsigned short CobID, unsigned short Length, void* pData, unsigned int* pErrorCode); 294 | CanLayer_DllExport int VCS_ReadCANFrame(void* KeyHandle, unsigned short CobID, unsigned short Length, void* pData, unsigned int Timeout, unsigned int* pErrorCode); 295 | CanLayer_DllExport int VCS_RequestCANFrame(void* KeyHandle, unsigned short CobID, unsigned short Length, void* pData, unsigned int* pErrorCode); 296 | CanLayer_DllExport int VCS_SendNMTService(void* KeyHandle, unsigned short NodeId, unsigned short CommandSpecifier, unsigned int* pErrorCode); 297 | 298 | /************************************************************************************************************************************* 299 | * TYPE DEFINITIONS 300 | *************************************************************************************************************************************/ 301 | //Communication 302 | //Dialog Mode 303 | const int DM_PROGRESS_DLG = 0; 304 | const int DM_PROGRESS_AND_CONFIRM_DLG = 1; 305 | const int DM_CONFIRM_DLG = 2; 306 | const int DM_NO_DLG = 3; 307 | 308 | //Configuration 309 | //MotorType 310 | const unsigned short MT_DC_MOTOR = 1; 311 | const unsigned short MT_EC_SINUS_COMMUTATED_MOTOR = 10; 312 | const unsigned short MT_EC_BLOCK_COMMUTATED_MOTOR = 11; 313 | 314 | //SensorType 315 | const unsigned short ST_UNKNOWN = 0; 316 | const unsigned short ST_INC_ENCODER_3CHANNEL = 1; 317 | const unsigned short ST_INC_ENCODER_2CHANNEL = 2; 318 | const unsigned short ST_HALL_SENSORS = 3; 319 | const unsigned short ST_SSI_ABS_ENCODER_BINARY = 4; 320 | const unsigned short ST_SSI_ABS_ENCODER_GREY = 5; 321 | 322 | //In- and outputs 323 | //Digital input configuration 324 | const unsigned short DIC_NEGATIVE_LIMIT_SWITCH = 0; 325 | const unsigned short DIC_POSITIVE_LIMIT_SWITCH = 1; 326 | const unsigned short DIC_HOME_SWITCH = 2; 327 | const unsigned short DIC_POSITION_MARKER = 3; 328 | const unsigned short DIC_DRIVE_ENABLE = 4; 329 | const unsigned short DIC_QUICK_STOP = 5; 330 | const unsigned short DIC_GENERAL_PURPOSE_J = 6; 331 | const unsigned short DIC_GENERAL_PURPOSE_I = 7; 332 | const unsigned short DIC_GENERAL_PURPOSE_H = 8; 333 | const unsigned short DIC_GENERAL_PURPOSE_G = 9; 334 | const unsigned short DIC_GENERAL_PURPOSE_F = 10; 335 | const unsigned short DIC_GENERAL_PURPOSE_E = 11; 336 | const unsigned short DIC_GENERAL_PURPOSE_D = 12; 337 | const unsigned short DIC_GENERAL_PURPOSE_C = 13; 338 | const unsigned short DIC_GENERAL_PURPOSE_B = 14; 339 | const unsigned short DIC_GENERAL_PURPOSE_A = 15; 340 | 341 | //Digital output configuration 342 | const unsigned short DOC_READY_FAULT = 0; 343 | const unsigned short DOC_POSITION_COMPARE = 1; 344 | const unsigned short DOC_GENERAL_PURPOSE_H = 8; 345 | const unsigned short DOC_GENERAL_PURPOSE_G = 9; 346 | const unsigned short DOC_GENERAL_PURPOSE_F = 10; 347 | const unsigned short DOC_GENERAL_PURPOSE_E = 11; 348 | const unsigned short DOC_GENERAL_PURPOSE_D = 12; 349 | const unsigned short DOC_GENERAL_PURPOSE_C = 13; 350 | const unsigned short DOC_GENERAL_PURPOSE_B = 14; 351 | const unsigned short DOC_GENERAL_PURPOSE_A = 15; 352 | 353 | //Analog input configuration 354 | const unsigned short AIC_ANALOG_CURRENT_SETPOINT = 0; 355 | const unsigned short AIC_ANALOG_VELOCITY_SETPOINT = 1; 356 | const unsigned short AIC_ANALOG_POSITION_SETPOINT = 2; 357 | 358 | //Units 359 | //VelocityDimension 360 | const unsigned char VD_RPM = 0xA4; 361 | 362 | //VelocityNotation 363 | const signed char VN_STANDARD = 0; 364 | const signed char VN_DECI = -1; 365 | const signed char VN_CENTI = -2; 366 | const signed char VN_MILLI = -3; 367 | 368 | //Operational mode 369 | const signed char OMD_PROFILE_POSITION_MODE = 1; 370 | const signed char OMD_PROFILE_VELOCITY_MODE = 3; 371 | const signed char OMD_HOMING_MODE = 6; 372 | const signed char OMD_INTERPOLATED_POSITION_MODE = 7; 373 | const signed char OMD_POSITION_MODE = -1; 374 | const signed char OMD_VELOCITY_MODE = -2; 375 | const signed char OMD_CURRENT_MODE = -3; 376 | const signed char OMD_MASTER_ENCODER_MODE = -5; 377 | const signed char OMD_STEP_DIRECTION_MODE = -6; 378 | 379 | //States 380 | const unsigned short ST_DISABLED = 0; 381 | const unsigned short ST_ENABLED = 1; 382 | const unsigned short ST_QUICKSTOP = 2; 383 | const unsigned short ST_FAULT = 3; 384 | 385 | //Homing mode 386 | //Homing method 387 | const char HM_ACTUAL_POSITION = 35; 388 | const char HM_NEGATIVE_LIMIT_SWITCH = 17; 389 | const char HM_NEGATIVE_LIMIT_SWITCH_AND_INDEX = 1; 390 | const char HM_POSITIVE_LIMIT_SWITCH = 18; 391 | const char HM_POSITIVE_LIMIT_SWITCH_AND_INDEX = 2; 392 | const char HM_HOME_SWITCH_POSITIVE_SPEED = 23; 393 | const char HM_HOME_SWITCH_POSITIVE_SPEED_AND_INDEX = 7; 394 | const char HM_HOME_SWITCH_NEGATIVE_SPEED = 27; 395 | const char HM_HOME_SWITCH_NEGATIVE_SPEED_AND_INDEX = 11; 396 | const char HM_CURRENT_THRESHOLD_POSITIVE_SPEED = -3; 397 | const char HM_CURRENT_THRESHOLD_POSITIVE_SPEED_AND_INDEX = -1; 398 | const char HM_CURRENT_THRESHOLD_NEGATIVE_SPEED = -4; 399 | const char HM_CURRENT_THRESHOLD_NEGATIVE_SPEED_AND_INDEX = -2; 400 | const char HM_INDEX_POSITIVE_SPEED = 34; 401 | const char HM_INDEX_NEGATIVE_SPEED = 33; 402 | 403 | //Input Output PositionMarker 404 | //PositionMarkerEdgeType 405 | const unsigned char PET_BOTH_EDGES = 0; 406 | const unsigned char PET_RISING_EDGE = 1; 407 | const unsigned char PET_FALLING_EDGE = 2; 408 | 409 | //PositionMarkerMode 410 | const unsigned char PM_CONTINUOUS = 0; 411 | const unsigned char PM_SINGLE = 1; 412 | const unsigned char PM_MULTIPLE = 2; 413 | 414 | //Input Output PositionCompare 415 | //PositionCompareOperationalMode 416 | const unsigned char PCO_SINGLE_POSITION_MODE = 0; 417 | const unsigned char PCO_POSITION_SEQUENCE_MODE = 1; 418 | 419 | //PositionCompareIntervalMode 420 | const unsigned char PCI_NEGATIVE_DIR_TO_REFPOS = 0; 421 | const unsigned char PCI_POSITIVE_DIR_TO_REFPOS = 1; 422 | const unsigned char PCI_BOTH_DIR_TO_REFPOS = 2; 423 | 424 | //PositionCompareDirectionDependency 425 | const unsigned char PCD_MOTOR_DIRECTION_NEGATIVE = 0; 426 | const unsigned char PCD_MOTOR_DIRECTION_POSITIVE = 1; 427 | const unsigned char PCD_MOTOR_DIRECTION_BOTH = 2; 428 | 429 | //Data recorder 430 | //Trigger type 431 | const unsigned short DR_MOVEMENT_START_TRIGGER = 1; //bit 1 432 | const unsigned short DR_ERROR_TRIGGER = 2; //bit 2 433 | const unsigned short DR_DIGITAL_INPUT_TRIGGER = 4; //bit 3 434 | const unsigned short DR_MOVEMENT_END_TRIGGER = 8; //bit 4 435 | 436 | //CanLayer Functions 437 | const unsigned short NCS_START_REMOTE_NODE = 1; 438 | const unsigned short NCS_STOP_REMOTE_NODE = 2; 439 | const unsigned short NCS_ENTER_PRE_OPERATIONAL = 128; 440 | const unsigned short NCS_RESET_NODE = 129; 441 | const unsigned short NCS_RESET_COMMUNICATION = 130; 442 | 443 | #endif //_H_LINUX_EPOSCMD_ 444 | 445 | 446 | -------------------------------------------------------------------------------- /epos_library/lib/arm/hf/libEposCmd.so.5.0.1.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RIVeR-Lab/epos_hardware/10fb2be121a7aa33085151845e5d1cf5a47f16bb/epos_library/lib/arm/hf/libEposCmd.so.5.0.1.0 -------------------------------------------------------------------------------- /epos_library/lib/arm/hf/libftd2xx.so.1.2.8: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RIVeR-Lab/epos_hardware/10fb2be121a7aa33085151845e5d1cf5a47f16bb/epos_library/lib/arm/hf/libftd2xx.so.1.2.8 -------------------------------------------------------------------------------- /epos_library/lib/arm/rpi/libEposCmd.so.5.0.1.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RIVeR-Lab/epos_hardware/10fb2be121a7aa33085151845e5d1cf5a47f16bb/epos_library/lib/arm/rpi/libEposCmd.so.5.0.1.0 -------------------------------------------------------------------------------- /epos_library/lib/arm/rpi/libftd2xx.so.1.2.8: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RIVeR-Lab/epos_hardware/10fb2be121a7aa33085151845e5d1cf5a47f16bb/epos_library/lib/arm/rpi/libftd2xx.so.1.2.8 -------------------------------------------------------------------------------- /epos_library/lib/arm/sf/libEposCmd.so.5.0.1.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RIVeR-Lab/epos_hardware/10fb2be121a7aa33085151845e5d1cf5a47f16bb/epos_library/lib/arm/sf/libEposCmd.so.5.0.1.0 -------------------------------------------------------------------------------- /epos_library/lib/arm/sf/libftd2xx.so.1.1.12: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RIVeR-Lab/epos_hardware/10fb2be121a7aa33085151845e5d1cf5a47f16bb/epos_library/lib/arm/sf/libftd2xx.so.1.1.12 -------------------------------------------------------------------------------- /epos_library/lib/x86/libEposCmd.so.5.0.1.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RIVeR-Lab/epos_hardware/10fb2be121a7aa33085151845e5d1cf5a47f16bb/epos_library/lib/x86/libEposCmd.so.5.0.1.0 -------------------------------------------------------------------------------- /epos_library/lib/x86/libftd2xx.so.1.1.12: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RIVeR-Lab/epos_hardware/10fb2be121a7aa33085151845e5d1cf5a47f16bb/epos_library/lib/x86/libftd2xx.so.1.1.12 -------------------------------------------------------------------------------- /epos_library/lib/x86_64/libEposCmd.so.5.0.1.0: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RIVeR-Lab/epos_hardware/10fb2be121a7aa33085151845e5d1cf5a47f16bb/epos_library/lib/x86_64/libEposCmd.so.5.0.1.0 -------------------------------------------------------------------------------- /epos_library/lib/x86_64/libftd2xx.so.1.1.12: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RIVeR-Lab/epos_hardware/10fb2be121a7aa33085151845e5d1cf5a47f16bb/epos_library/lib/x86_64/libftd2xx.so.1.1.12 -------------------------------------------------------------------------------- /epos_library/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | epos_library 4 | 0.0.3 5 | A catkin package that provides the EPOS Command Library 6 | 7 | Mitchell Wills 8 | 9 | TODO 10 | 11 | http://www.maxonmotorusa.com/medias/sys_master/root/8815100788766/EPOS-Command-Library-En.pdf 12 | https://github.com/RIVeR-Lab/epos_hardware 13 | 14 | catkin 15 | 16 | 17 | 18 | 19 | --------------------------------------------------------------------------------