├── vesc ├── CMakeLists.txt └── package.xml ├── vesc_msgs ├── msg │ ├── VescStateStamped.msg │ └── VescState.msg ├── package.xml └── CMakeLists.txt ├── .github ├── ISSUE_TEMPLATE │ ├── -------improvement-suggestions.md │ └── -------bug-report.md └── PULL_REQUEST_TEMPLATE.md ├── vesc_hw_interface ├── vesc_hw_interface_plugin.xml ├── config │ ├── position_sample.yaml │ └── velocity_duty_sample.yaml ├── package.xml ├── launch │ ├── velocity_test.ros2_control.xacro │ ├── position_test.ros2_control.xacro │ ├── velocity_duty_test.ros2_control.xacro │ ├── position_duty_test.ros2_control.xacro │ ├── position_control_sample.launch.py │ └── velocity_control_sample.launch.py ├── CMakeLists.txt ├── include │ └── vesc_hw_interface │ │ ├── vesc_step_difference.hpp │ │ ├── visibility_control.h │ │ ├── vesc_wheel_controller.hpp │ │ ├── vesc_hw_interface.hpp │ │ └── vesc_servo_controller.hpp ├── src │ ├── vesc_step_difference.cpp │ ├── vesc_wheel_controller.cpp │ ├── vesc_hw_interface.cpp │ └── vesc_servo_controller.cpp └── README.md ├── vesc_driver ├── CMakeLists.txt ├── package.xml ├── launch │ ├── vesc_driver_node.launch │ └── vesc_driver_nodelet.launch ├── README.md ├── src │ ├── vesc_driver_node.cpp │ ├── vesc_driver_nodelet.cpp │ ├── vesc_packet_factory.cpp │ ├── vesc_interface.cpp │ └── vesc_driver.cpp └── include │ └── vesc_driver │ ├── vesc_packet_factory.hpp │ ├── vesc_driver.h │ ├── vesc_interface.hpp │ ├── vesc_packet.hpp │ └── data_map.hpp ├── .gitignore ├── README.md └── LICENSE /vesc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(vesc) 3 | find_package(ament_cmake REQUIRED) 4 | ament_package() 5 | -------------------------------------------------------------------------------- /vesc_msgs/msg/VescStateStamped.msg: -------------------------------------------------------------------------------- 1 | # Timestamped VESC open source motor controller state (telemetry) 2 | 3 | std_msgs/Header header 4 | VescState state 5 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/-------improvement-suggestions.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Improvement Suggestion 3 | about: New features, optimization, etc. 4 | title: '' 5 | labels: 'enhancement' 6 | assignees: '' 7 | 8 | --- 9 | 10 | 11 | ## Abstract 12 | 13 | ## Purpose 14 | 15 | ## Implementation Details 16 | -------------------------------------------------------------------------------- /vesc_hw_interface/vesc_hw_interface_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | plugin of VESC hardware interface 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /.github/PULL_REQUEST_TEMPLATE.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ## Change Summary 5 | 6 | ## Details 7 | 8 | ## Impacts 9 | 10 | 11 | ## References 12 | 13 | 14 | ## Additional Information 15 | 16 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/-------bug-report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug Report 3 | about: A problem due to a bug of algorithms or codes 4 | title: '' 5 | labels: bug 6 | assignees: '' 7 | 8 | --- 9 | 10 | 11 | ## Abstract 12 | 13 | ## How to Reproduce the Bug 14 | 15 | ## Problem due to the Bug 16 | 17 | ## Factors 18 | 19 | 20 | ## Suggestion 21 | 22 | -------------------------------------------------------------------------------- /vesc_hw_interface/config/position_sample.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | joint_state_broadcaster: 5 | type: joint_state_broadcaster/JointStateBroadcaster 6 | joint_position_controller: 7 | type: position_controllers/JointGroupPositionController 8 | 9 | # Publish all joint states ----------------------------------- 10 | joint_state_broadcaster: 11 | ros__parameters: 12 | publish_rate: 100 13 | 14 | joint_position_controller: 15 | ros__parameters: 16 | joints: 17 | - vesc_joint 18 | publish_rate: 100 19 | -------------------------------------------------------------------------------- /vesc_hw_interface/config/velocity_duty_sample.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | joint_state_broadcaster: 5 | type: joint_state_broadcaster/JointStateBroadcaster 6 | joint_velocity_controller: 7 | type: velocity_controllers/JointGroupVelocityController 8 | 9 | # Publish all joint states ----------------------------------- 10 | joint_state_broadcaster: 11 | ros__parameters: 12 | publish_rate: 100 13 | 14 | joint_velocity_controller: 15 | ros__parameters: 16 | joints: 17 | - vesc_joint 18 | publish_rate: 100 19 | -------------------------------------------------------------------------------- /vesc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vesc 5 | 1.0.0 6 | Metapackage of VESC packages. 7 | SoftBank Corp. 8 | Apache 2.0 9 | 10 | ament_cmake 11 | 12 | vesc_driver 13 | vesc_msgs 14 | vesc_hw_interface 15 | 16 | 17 | ament_cmake 18 | 19 | 20 | -------------------------------------------------------------------------------- /vesc_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(vesc_driver) 3 | 4 | add_compile_options(-std=c++17) 5 | 6 | if(CMAKE_COMPILER_IS_GNUCXX 7 | OR CMAKE_CXX_COMPILER_ID 8 | MATCHES 9 | "Clang" 10 | ) 11 | add_compile_options( 12 | -Wall 13 | -Wextra 14 | -Wpedantic 15 | ) 16 | endif() 17 | 18 | find_package(ament_cmake_auto REQUIRED) 19 | find_package(Threads) 20 | ament_auto_find_build_dependencies() 21 | 22 | ########### 23 | ## Build ## 24 | ########### 25 | 26 | # driver libraries 27 | ament_auto_add_library( 28 | ${PROJECT_NAME} 29 | SHARED 30 | src/vesc_interface.cpp 31 | src/vesc_packet.cpp 32 | src/vesc_packet_factory.cpp 33 | ) 34 | 35 | ament_auto_package() 36 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # VS Code 51 | .vscode/ 52 | */.vscode/ 53 | 54 | # Catkin custom files 55 | CATKIN_IGNORE -------------------------------------------------------------------------------- /vesc_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vesc_driver 5 | 1.0.0 6 | ROS device driver for VESC open source motor driver. 7 | SoftBank Corp. 8 | Apache 2.0 9 | 10 | ament_cmake_auto 11 | asio_cmake_module 12 | 13 | rclcpp 14 | rclcpp_components 15 | std_msgs 16 | vesc_msgs 17 | serial_driver 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # VESC 2 | ## ROS Interfaces for VESC Motor Drivers 3 | 4 | 5 | 6 | ## Composition 7 | This repository is composed as follows: 8 | 9 | - `vesc` is a meta package to manage the other packages. 10 | - `vesc_driver` is the main library to drive VESCs. 11 | - `vesc_hw_interface` wraps `vesc_driver` as a hardware interface to use `ros_control` systems. 12 | - `vesc_msgs` defines messages to publish VESC status. 13 | 14 | ## License 15 | This repositry is licensed under the [Apache 2.0 License](https://www.apache.org/licenses/LICENSE-2.0.html). 16 | 17 | ## Note 18 | A part of these packages had been developed by Michael T. Boulet at MIT under the BSD 3-clause License until Dec. 2016 in the repository named [mit-racecar/vesc](https://github.com/mit-racecar/vesc). Since Nov. 2019, Softbank Corp. takes over development as new packages. 19 | -------------------------------------------------------------------------------- /vesc_driver/launch/vesc_driver_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /vesc_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vesc_msgs 5 | 1.0.0 6 | ROS message definitions for VESC open source motor driver. 7 | SoftBank Corp. 8 | Apache 2.0 9 | 10 | ament_cmake 11 | rosidl_default_generators 12 | 13 | builtin_interfaces 14 | std_msgs 15 | 16 | rosidl_default_runtime 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | 21 | rosidl_interface_packages 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /vesc_msgs/msg/VescState.msg: -------------------------------------------------------------------------------- 1 | # Vedder VESC open source motor controller state (telemetry) 2 | 3 | # fault codes 4 | int32 FAULT_CODE_NONE=0 5 | int32 FAULT_CODE_OVER_VOLTAGE=1 6 | int32 FAULT_CODE_UNDER_VOLTAGE=2 7 | int32 FAULT_CODE_DRV8302=3 8 | int32 FAULT_CODE_ABS_OVER_CURRENT=4 9 | int32 FAULT_CODE_OVER_TEMP_FET=5 10 | int32 FAULT_CODE_OVER_TEMP_MOTOR=6 11 | 12 | float64 voltage_input # input voltage (volt) 13 | float64 temperature_pcb # temperature of printed circuit board (degrees Celsius) 14 | float64 current_motor # motor current (ampere) 15 | float64 current_input # input current (ampere) 16 | float64 speed # motor velocity (rad/s) 17 | float64 duty_cycle # duty cycle (0 to 1) 18 | float64 charge_drawn # electric charge drawn from input (ampere-hour) 19 | float64 charge_regen # electric charge regenerated to input (ampere-hour) 20 | float64 energy_drawn # energy drawn from input (watt-hour) 21 | float64 energy_regen # energy regenerated to input (watt-hour) 22 | float64 displacement # net tachometer (counts) 23 | float64 distance_traveled # total tachnometer (counts) 24 | int32 fault_code 25 | -------------------------------------------------------------------------------- /vesc_hw_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | vesc_hw_interface 5 | 1.0.0 6 | VESC hardware interfaces 7 | SoftBank Corp. 8 | Apache 2.0 9 | 10 | Michael T. Boulet 11 | Yuki Onishi 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | rclcpp_lifecycle 17 | angles 18 | hardware_interface 19 | controller_manager 20 | velocity_controllers 21 | position_controllers 22 | joint_state_broadcaster 23 | joint_limits 24 | pluginlib 25 | std_msgs 26 | tinyxml2_vendor 27 | urdf 28 | vesc_driver 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /vesc_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(vesc_msgs) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX 5 | OR CMAKE_CXX_COMPILER_ID 6 | MATCHES 7 | "Clang" 8 | ) 9 | add_compile_options( 10 | -Wall 11 | -Wextra 12 | -Wpedantic 13 | ) 14 | endif() 15 | 16 | find_package(ament_cmake REQUIRED) 17 | find_package(rosidl_default_generators REQUIRED) 18 | find_package(std_msgs REQUIRED) 19 | 20 | set(msg_files "msg/VescState.msg" "msg/VescStateStamped.msg") 21 | 22 | rosidl_generate_interfaces( 23 | ${PROJECT_NAME} 24 | ${msg_files} 25 | DEPENDENCIES 26 | std_msgs 27 | ) 28 | 29 | ament_export_dependencies(rosidl_default_runtime) 30 | 31 | if(BUILD_TESTING) 32 | find_package(ament_lint_auto REQUIRED) 33 | # the following line skips the linter which checks for copyrights 34 | # comment the line when a copyright and license is added to all source files 35 | set(ament_cmake_copyright_FOUND TRUE) 36 | # the following line skips cpplint (only works in a git repo) 37 | # comment the line when this package is in a git repo and when 38 | # a copyright and license is added to all source files 39 | set(ament_cmake_cpplint_FOUND TRUE) 40 | ament_lint_auto_find_test_dependencies() 41 | endif() 42 | 43 | ament_package() 44 | -------------------------------------------------------------------------------- /vesc_hw_interface/launch/velocity_test.ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | vesc_hw_interface/VescHwInterface 15 | /dev/ttyACM0 16 | velocity 17 | 18 | 19 | 20 | -1 21 | 1 22 | 23 | 24 | -1 25 | 1 26 | 27 | 28 | -1 29 | 1 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /vesc_driver/README.md: -------------------------------------------------------------------------------- 1 | # VESC Driving Library 2 | `vesc_driver` is a basic library to drive motors with a VESC. 3 | 4 | ## Features 5 | - communicates with a VESC. 6 | - Position (PID), velocity, current, and duty-cycle control are supported. 7 | - Calibration and joint limitation are implemented for position control. 8 | - You can give torque constant and gear ratio of your geared motor 9 | 10 | ## Package Structures 11 | #### Dependency Diagram 12 | ``` 13 | vesc_driver 14 | -> vesc_interface 15 | |-> vesc_packet_factory 16 | |-> vesc_packet 17 | ``` 18 | 19 | #### Description 20 | - `vesc_packet` defines communication packets and the lowest interfaces via USB serial. 21 | - `vesc_packet_factory` creates packets defines in `vesc_packet`. 22 | - `vesc_interface` provides us with basic APIs to drive VESC moto drivers: API functions send commands including duty, reference current, brake, and reference velocity. 23 | - `vesc_driver` publishes all states using `vesc_msgs` and sends commands with callback functions. 24 | 25 | 26 | ## Installation 27 | This package depends on the following ROS packages. 28 | 29 | - nodelet 30 | - pluginlib 31 | - roscpp 32 | - std_msgs 33 | - serial 34 | 35 | Before building this, you have to install them (e.g. with `apt` command). 36 | 37 | ## Usage 38 | will be prepared ... 39 | 40 | ## License 41 | `vesc_driver` is licensed under the [Apache 2.0 license](https://www.apache.org/licenses/LICENSE-2.0.html). 42 | -------------------------------------------------------------------------------- /vesc_driver/launch/vesc_driver_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 26 | 27 | 28 | 29 | 30 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /vesc_hw_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(vesc_hw_interface) 3 | 4 | add_compile_options(-std=c++17) 5 | 6 | if(CMAKE_COMPILER_IS_GNUCXX 7 | OR CMAKE_CXX_COMPILER_ID 8 | MATCHES 9 | "Clang" 10 | ) 11 | add_compile_options( 12 | -Wall 13 | -Wextra 14 | -Wpedantic 15 | ) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake_auto REQUIRED) 20 | find_package(Threads) 21 | ament_auto_find_build_dependencies() 22 | 23 | ament_auto_add_library( 24 | ${PROJECT_NAME} 25 | SHARED 26 | src/vesc_hw_interface.cpp 27 | src/vesc_servo_controller.cpp 28 | src/vesc_wheel_controller.cpp 29 | src/vesc_step_difference.cpp 30 | ) 31 | 32 | target_include_directories(${PROJECT_NAME} PUBLIC include) 33 | 34 | ament_target_dependencies(${PROJECT_NAME} ${DEPENDENCIES}) 35 | target_compile_definitions(${PROJECT_NAME} PRIVATE "HARDWARE_INTERFACE_BUILDING_DLL") 36 | 37 | pluginlib_export_plugin_description_file(hardware_interface vesc_hw_interface_plugin.xml) 38 | 39 | install( 40 | DIRECTORY launch config 41 | DESTINATION share/${PROJECT_NAME} 42 | ) 43 | 44 | if(BUILD_TESTING) 45 | find_package(ament_lint_auto REQUIRED) 46 | # the following line skips the linter which checks for copyrights 47 | # comment the line when a copyright and license is added to all source files 48 | set(ament_cmake_copyright_FOUND TRUE) 49 | # the following line skips cpplint (only works in a git repo) 50 | # comment the line when this package is in a git repo and when 51 | # a copyright and license is added to all source files 52 | set(ament_cmake_cpplint_FOUND TRUE) 53 | ament_lint_auto_find_test_dependencies() 54 | endif() 55 | 56 | ament_auto_package() 57 | -------------------------------------------------------------------------------- /vesc_hw_interface/include/vesc_hw_interface/vesc_step_difference.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2023 SoftBank Corp. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | ********************************************************************/ 17 | 18 | #ifndef VESC_STEP_DIFFERENCE_HPP_ 19 | #define VESC_STEP_DIFFERENCE_HPP_ 20 | 21 | #include 22 | #include 23 | 24 | namespace vesc_step_difference 25 | { 26 | class VescStepDifference 27 | { 28 | public: 29 | VescStepDifference(); 30 | ~VescStepDifference(); 31 | double getStepDifference(const double step_in); 32 | void resetStepDifference(const double step_in); 33 | void enableSmooth(double control_rate, double max_sampling_time, int max_step_diff); 34 | 35 | private: 36 | double stepDifferenceRaw(const double step_in, bool reset); 37 | double stepDifferenceVariableWindow(const double step_in, bool reset); 38 | 39 | // Enable smooth difference 40 | bool enable_smooth_; 41 | // Params for counterTDRaw 42 | int32_t step_in_previous_; 43 | // Params for counterTDVariableWindow 44 | int step_diff_vw_max_step_; 45 | std::deque step_input_queue_; 46 | }; 47 | } // namespace vesc_step_difference 48 | 49 | #endif // VESC_STEP_DIFFERENCE_HPP_ 50 | -------------------------------------------------------------------------------- /vesc_hw_interface/launch/position_test.ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | vesc_hw_interface/VescHwInterface 15 | /dev/ttyACM0 16 | false 17 | 1.0 18 | -0.08 19 | 0.01 20 | -0.08 21 | duty 22 | 0.0 23 | false 24 | 1 25 | 0.8 26 | 0.02 27 | position 28 | 29 | 30 | 31 | -1 32 | 1 33 | 34 | 35 | -1 36 | 1 37 | 38 | 39 | -1 40 | 1 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /vesc_hw_interface/launch/velocity_duty_test.ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | vesc_hw_interface/VescHwInterface 15 | /dev/ttyACM0 16 | 0.230769 17 | 1.0 18 | 3 19 | 20 20 | 0.1 21 | 0.01 22 | 0.02 23 | 1.0 24 | 1.0 25 | true 26 | 100.0 27 | true 28 | 0.2 29 | 10 30 | velocity_duty 31 | 32 | 33 | 34 | -1 35 | 1 36 | 37 | 38 | -1 39 | 1 40 | 41 | 42 | -1 43 | 1 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /vesc_hw_interface/include/vesc_hw_interface/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef VESC_HW_INTERFACE__VISIBILITY_H_ 16 | #define VESC_HW_INTERFACE__VISIBILITY_H_ 17 | 18 | #ifdef __cplusplus 19 | extern "C" { 20 | #endif 21 | 22 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 23 | // https://gcc.gnu.org/wiki/Visibility 24 | 25 | #if defined _WIN32 || defined __CYGWIN__ 26 | 27 | #ifdef __GNUC__ 28 | #define VESC_HW_INTERFACE_EXPORT __attribute__((dllexport)) 29 | #define VESC_HW_INTERFACE_IMPORT __attribute__((dllimport)) 30 | #else 31 | #define VESC_HW_INTERFACE_EXPORT __declspec(dllexport) 32 | #define VESC_HW_INTERFACE_IMPORT __declspec(dllimport) 33 | #endif 34 | 35 | #ifdef VESC_HW_INTERFACE_DLL 36 | #define VESC_HW_INTERFACE_PUBLIC VESC_HW_INTERFACE_EXPORT 37 | #else 38 | #define VESC_HW_INTERFACE_PUBLIC VESC_HW_INTERFACE_IMPORT 39 | #endif 40 | 41 | #define VESC_HW_INTERFACE_PUBLIC_TYPE VESC_HW_INTERFACE_PUBLIC 42 | 43 | #define VESC_HW_INTERFACE_LOCAL 44 | 45 | #else 46 | 47 | #define VESC_HW_INTERFACE_EXPORT __attribute__((visibility("default"))) 48 | #define VESC_HW_INTERFACE_IMPORT 49 | 50 | #if __GNUC__ >= 4 51 | #define VESC_HW_INTERFACE_PUBLIC __attribute__((visibility("default"))) 52 | #define VESC_HW_INTERFACE_LOCAL __attribute__((visibility("hidden"))) 53 | #else 54 | #define VESC_HW_INTERFACE_PUBLIC 55 | #define VESC_HW_INTERFACE_LOCAL 56 | #endif 57 | 58 | #define VESC_HW_INTERFACE_PUBLIC_TYPE 59 | #endif 60 | 61 | #ifdef __cplusplus 62 | } 63 | #endif 64 | 65 | #endif // VESC_HW_INTERFACE__VISIBILITY_H_ 66 | -------------------------------------------------------------------------------- /vesc_driver/src/vesc_driver_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of Softbank Corp. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | ********************************************************************/ 29 | 30 | /** NOTE ************************************************************* 31 | * This program had been developed by Michael T. Boulet at MIT under 32 | * the BSD 3-clause License until Dec. 2016. Since Nov. 2019, Softbank 33 | * Corp. takes over development as new packages. 34 | ********************************************************************/ 35 | 36 | // TODO: Migrate to ROS2 37 | #include 38 | 39 | #include "vesc_driver/vesc_driver.h" 40 | 41 | int main(int argc, char** argv) 42 | { 43 | ros::init(argc, argv, "vesc_driver_node"); 44 | ros::NodeHandle nh; 45 | ros::NodeHandle private_nh("~"); 46 | 47 | vesc_driver::VescDriver vesc_driver(nh, private_nh); 48 | 49 | ros::spin(); 50 | 51 | return 0; 52 | } 53 | -------------------------------------------------------------------------------- /vesc_hw_interface/launch/position_duty_test.ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | vesc_hw_interface/VescHwInterface 15 | /dev/ttyACM0 16 | 0.230769 17 | 1.0 18 | 3 19 | 1.0 20 | 20 21 | 0.005 22 | 0.005 23 | 0.0025 24 | 1.0 25 | 1.0 26 | true 27 | 100.0 28 | false 29 | 1.0 30 | -0.08 31 | 0.01 32 | -0.08 33 | duty 34 | 0.0 35 | true 36 | 1.0 37 | 10 38 | false 39 | 1 40 | 0.8 41 | 0.02 42 | position_duty 43 | 44 | 45 | 46 | -1 47 | 1 48 | 49 | 50 | -1 51 | 1 52 | 53 | 54 | -1 55 | 1 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /vesc_hw_interface/include/vesc_hw_interface/vesc_wheel_controller.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2022 SoftBank Corp. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | ********************************************************************/ 17 | 18 | #ifndef VESC_HW_INTERFACE_VESC_WHEEL_CONTROLLER_HPP_ 19 | #define VESC_HW_INTERFACE_VESC_WHEEL_CONTROLLER_HPP_ 20 | 21 | #include 22 | #include 23 | #include 24 | #include "vesc_hw_interface/vesc_step_difference.hpp" 25 | 26 | namespace vesc_hw_interface 27 | { 28 | using vesc_driver::VescInterface; 29 | using vesc_driver::VescPacket; 30 | using vesc_driver::VescPacketValues; 31 | using vesc_step_difference::VescStepDifference; 32 | 33 | class VescWheelController 34 | { 35 | public: 36 | void init(hardware_interface::HardwareInfo& info, const std::shared_ptr& interface); 37 | void control(const double control_rate); 38 | void setTargetVelocity(const double velocity); 39 | void setGearRatio(const double gear_ratio); 40 | void setTorqueConst(const double torque_const); 41 | void setRotorPoles(const int rotor_poles); 42 | void setHallSensors(const int hall_sensors); 43 | double getPositionSens(); 44 | double getVelocitySens(); 45 | double getEffortSens(); 46 | void updateSensor(const std::shared_ptr& packet); 47 | 48 | private: 49 | std::shared_ptr interface_ptr_; 50 | VescStepDifference vesc_step_difference_; 51 | 52 | double kp_, ki_, kd_; 53 | double i_clamp_; 54 | bool antiwindup_; 55 | double duty_limiter_; 56 | double num_rotor_pole_pairs_, num_rotor_poles_; 57 | double num_hall_sensors_; 58 | double gear_ratio_, torque_const_; 59 | 60 | double target_velocity_; 61 | double position_steps_; 62 | int prev_steps_; 63 | double position_sens_; 64 | double velocity_sens_; 65 | double effort_sens_; 66 | 67 | double error_, error_dt_, error_integ_; 68 | double target_steps_; 69 | bool pid_initialize_; 70 | bool sensor_initialize_; 71 | 72 | double control_rate_; 73 | // rclcpp::Timer control_timer_; 74 | // void controlTimerCallback(const ros::TimerEvent& e); 75 | }; 76 | } // namespace vesc_hw_interface 77 | 78 | #endif // VESC_HW_INTERFACE_VESC_WHEEL_CONTROLLER_HPP_ 79 | -------------------------------------------------------------------------------- /vesc_driver/src/vesc_driver_nodelet.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of Softbank Corp. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | ********************************************************************/ 29 | 30 | /** NOTE ************************************************************* 31 | * This program had been developed by Michael T. Boulet at MIT under 32 | * the BSD 3-clause License until Dec. 2016. Since Nov. 2019, Softbank 33 | * Corp. takes over development as new packages. 34 | ********************************************************************/ 35 | 36 | // TODO: Migrate to ROS2 37 | #include 38 | #include 39 | #include 40 | 41 | #include "vesc_driver/vesc_driver.h" 42 | 43 | namespace vesc_driver 44 | { 45 | class VescDriverNodelet : public nodelet::Nodelet 46 | { 47 | public: 48 | VescDriverNodelet() 49 | { 50 | } 51 | 52 | private: 53 | virtual void onInit(void); 54 | 55 | std::shared_ptr vesc_driver_; 56 | }; // class VescDriverNodelet 57 | 58 | void VescDriverNodelet::onInit() 59 | { 60 | NODELET_DEBUG("Initializing VESC driver nodelet"); 61 | vesc_driver_.reset(new VescDriver(getNodeHandle(), getPrivateNodeHandle())); 62 | } 63 | 64 | } // namespace vesc_driver 65 | 66 | PLUGINLIB_EXPORT_CLASS(vesc_driver::VescDriverNodelet, nodelet::Nodelet); 67 | -------------------------------------------------------------------------------- /vesc_hw_interface/launch/position_control_sample.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding:utf-8 -*- 3 | 4 | # Copyright (c) 2023 SoftBank Corp. 5 | # 6 | # Licensed under the Apache License, Version 2.0 (the "License"); 7 | # you may not use this file except in compliance with the License. 8 | # You may obtain a copy of the License at 9 | # 10 | # http://www.apache.org/licenses/LICENSE-2.0 11 | # 12 | # Unless required by applicable law or agreed to in writing, software 13 | # distributed under the License is distributed on an "AS IS" BASIS, 14 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | # See the License for the specific language governing permissions and 16 | # limitations under the License. 17 | # 18 | 19 | import pathlib 20 | 21 | import xacro 22 | from launch import LaunchDescription 23 | from launch.actions import DeclareLaunchArgument 24 | from launch.actions import GroupAction 25 | from launch.actions import OpaqueFunction 26 | from launch.launch_context import LaunchContext 27 | from launch.substitutions import LaunchConfiguration 28 | from launch_ros.actions import Node 29 | from launch_ros.substitutions import FindPackageShare 30 | 31 | 32 | def launch_setup(context: LaunchContext, *args, **kwargs) -> list: 33 | 34 | vesc_pkg = FindPackageShare('vesc_hw_interface').find('vesc_hw_interface') 35 | doc = xacro.process_file(LaunchConfiguration('model').perform(context)) 36 | robot_description = {'robot_description': doc.toprettyxml(indent=' ')} 37 | 38 | robot_controllers = [vesc_pkg, '/config/position_sample.yaml'] 39 | 40 | control_node = Node(package='controller_manager', 41 | executable='ros2_control_node', 42 | parameters=[robot_description, robot_controllers], 43 | output='both') 44 | 45 | controllers = GroupAction(actions=[ 46 | Node(package='controller_manager', 47 | executable='spawner', 48 | output='both', 49 | arguments=['--controller-manager', 'controller_manager', 'joint_state_broadcaster']), 50 | Node(package='controller_manager', 51 | executable='spawner', 52 | output='both', 53 | arguments=['--controller-manager', 'controller_manager', 'joint_position_controller']) 54 | ]) 55 | 56 | robot_state = Node(package='robot_state_publisher', 57 | executable='robot_state_publisher', 58 | parameters=[robot_description], 59 | output='both') 60 | 61 | return [control_node, controllers, robot_state] 62 | 63 | 64 | def generate_launch_description() -> LaunchDescription: 65 | """Generate launch descriptions. 66 | 67 | Returns: 68 | Launch descriptions 69 | """ 70 | vesc_pkg = pathlib.Path(FindPackageShare('vesc_hw_interface').find('vesc_hw_interface')) 71 | model_arg = DeclareLaunchArgument('model', default_value=str(vesc_pkg / 'launch/position_test.ros2_control.xacro')) 72 | 73 | return LaunchDescription([model_arg, OpaqueFunction(function=launch_setup)]) 74 | -------------------------------------------------------------------------------- /vesc_hw_interface/launch/velocity_control_sample.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding:utf-8 -*- 3 | 4 | # Copyright (c) 2023 SoftBank Corp. 5 | # 6 | # Licensed under the Apache License, Version 2.0 (the "License"); 7 | # you may not use this file except in compliance with the License. 8 | # You may obtain a copy of the License at 9 | # 10 | # http://www.apache.org/licenses/LICENSE-2.0 11 | # 12 | # Unless required by applicable law or agreed to in writing, software 13 | # distributed under the License is distributed on an "AS IS" BASIS, 14 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | # See the License for the specific language governing permissions and 16 | # limitations under the License. 17 | # 18 | 19 | import pathlib 20 | 21 | import xacro 22 | from launch import LaunchDescription 23 | from launch.actions import DeclareLaunchArgument 24 | from launch.actions import GroupAction 25 | from launch.actions import OpaqueFunction 26 | from launch.launch_context import LaunchContext 27 | from launch.substitutions import LaunchConfiguration 28 | from launch_ros.actions import Node 29 | from launch_ros.substitutions import FindPackageShare 30 | 31 | 32 | def launch_setup(context: LaunchContext, *args, **kwargs) -> list: 33 | 34 | vesc_pkg = FindPackageShare('vesc_hw_interface').find('vesc_hw_interface') 35 | doc = xacro.process_file(LaunchConfiguration('model').perform(context)) 36 | robot_description = {"robot_description": doc.toprettyxml(indent=' ')} 37 | 38 | robot_controllers = [vesc_pkg, '/config/velocity_duty_sample.yaml'] 39 | 40 | control_node = Node(package="controller_manager", 41 | executable="ros2_control_node", 42 | parameters=[robot_description, robot_controllers], 43 | output="both") 44 | 45 | controllers = GroupAction(actions=[Node(package='controller_manager', 46 | executable='spawner', 47 | output='both', 48 | arguments=["--controller-manager", "controller_manager", 49 | 'joint_state_broadcaster']), 50 | Node(package='controller_manager', 51 | executable='spawner', 52 | output='both', 53 | arguments=["--controller-manager", "controller_manager", 54 | 'joint_velocity_controller'])]) 55 | 56 | robot_state = Node(package='robot_state_publisher', 57 | executable='robot_state_publisher', 58 | parameters=[robot_description], 59 | output='both') 60 | 61 | return [control_node, controllers, robot_state] 62 | 63 | 64 | def generate_launch_description() -> LaunchDescription: 65 | """Generate launch descriptions. 66 | 67 | Returns: 68 | Launch descriptions 69 | """ 70 | vesc_pkg = pathlib.Path(FindPackageShare('vesc_hw_interface').find('vesc_hw_interface')) 71 | model_arg = DeclareLaunchArgument( 72 | 'model', 73 | default_value=str(vesc_pkg / 'launch/velocity_duty_test.ros2_control.xacro')) 74 | 75 | return LaunchDescription([ 76 | model_arg, 77 | OpaqueFunction(function=launch_setup) 78 | ]) 79 | -------------------------------------------------------------------------------- /vesc_hw_interface/include/vesc_hw_interface/vesc_hw_interface.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | ********************************************************************/ 16 | 17 | #ifndef VESC_HW_INTERFACE_VESC_HW_INTERFACE_HPP_ 18 | #define VESC_HW_INTERFACE_VESC_HW_INTERFACE_HPP_ 19 | 20 | #include "visibility_control.h" 21 | #include 22 | // #include 23 | // #include 24 | // #include 25 | // #include 26 | #include 27 | #include 28 | #include 29 | #include "vesc_driver/vesc_interface.hpp" 30 | #include "vesc_hw_interface/vesc_servo_controller.hpp" 31 | #include "vesc_hw_interface/vesc_wheel_controller.hpp" 32 | 33 | 34 | namespace vesc_hw_interface 35 | { 36 | using vesc_driver::VescInterface; 37 | using vesc_driver::VescPacket; 38 | using vesc_driver::VescPacketValues; 39 | using vesc_driver::VescPacketMCConf; 40 | using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; 41 | 42 | class VescHwInterface : public hardware_interface::ActuatorInterface 43 | { 44 | public: 45 | VescHwInterface(); 46 | 47 | CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override; 48 | CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; 49 | CallbackReturn on_cleanup(const rclcpp_lifecycle::State& previous_state) override; 50 | CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; 51 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; 52 | CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state) override; 53 | CallbackReturn on_error(const rclcpp_lifecycle::State& previous_state) override; 54 | std::vector export_state_interfaces() override; 55 | std::vector export_command_interfaces() override; 56 | hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; 57 | hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override; 58 | rclcpp::Time getTime() const; 59 | 60 | private: 61 | std::shared_ptr vesc_interface_; 62 | VescServoController servo_controller_; 63 | VescWheelController wheel_controller_; 64 | 65 | std::string joint_name_, command_mode_, port_; 66 | std::string joint_type_; 67 | double upper_limit_, lower_limit_; 68 | bool homing_enabled_; 69 | bool homing_done_; 70 | double homing_offset_; 71 | double homing_position_; 72 | int32_t prev_steps_; 73 | double position_steps_; 74 | bool sensor_initialize_; 75 | 76 | double command_; 77 | double position_, velocity_, effort_; // joint states 78 | 79 | int num_rotor_poles_; // the number of rotor poles 80 | int num_hall_sensors_; // the number of hall sensors 81 | double gear_ratio_, torque_const_; // physical params 82 | double screw_lead_; // linear distance (m) of 1 revolution 83 | 84 | // joint_limits_interface::PositionJointSaturationInterface limit_position_interface_; 85 | // joint_limits_interface::VelocityJointSaturationInterface limit_velocity_interface_; 86 | // joint_limits_interface::EffortJointSaturationInterface limit_effort_interface_; 87 | 88 | void packetCallback(const std::shared_ptr&); 89 | void errorCallback(const std::string&); 90 | 91 | static constexpr double VESC_POS_RANGE = 360.0; // Full angular range of the VESC PID position control 92 | static constexpr double VESC_POS_MAPPING_RANGE = 90.0; // Range for mapping the position to the VESC 93 | static constexpr double VESC_POS_WRAP_THRESHOLD = (VESC_POS_RANGE - VESC_POS_MAPPING_RANGE) / 2.0 + VESC_POS_MAPPING_RANGE; // Angle at which to wrap the position (overflow/underflow) 94 | }; 95 | 96 | } // namespace vesc_hw_interface 97 | 98 | #endif // VESC_HW_INTERFACE_VESC_HW_INTERFACE_HPP_ 99 | -------------------------------------------------------------------------------- /vesc_hw_interface/include/vesc_hw_interface/vesc_servo_controller.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | ********************************************************************/ 16 | 17 | #ifndef VESC_HW_INTERFACE_VESC_SERVO_CONTROLLER_HPP_ 18 | #define VESC_HW_INTERFACE_VESC_SERVO_CONTROLLER_HPP_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "vesc_hw_interface/vesc_step_difference.hpp" 25 | #include 26 | 27 | namespace vesc_hw_interface 28 | { 29 | using vesc_driver::VescInterface; 30 | using vesc_driver::VescPacket; 31 | using vesc_driver::VescPacketValues; 32 | using vesc_step_difference::VescStepDifference; 33 | 34 | class VescServoController 35 | { 36 | public: 37 | VescServoController(); 38 | ~VescServoController(); 39 | 40 | void init(hardware_interface::HardwareInfo& info, const std::shared_ptr& interface, 41 | const double gear_ratio = 0.0, const double torque_const = 0.0, const int rotor_poles = 0, 42 | const int hall_sensors = 0, const int joint_type = 0, const double screw_lead = 0.0, 43 | const double upper_endstop_position = 0.0, const double lower_endstop_position = 0.0); 44 | void control(const double control_rate); 45 | void setTargetPosition(const double position); 46 | void setGearRatio(const double gear_ratio); 47 | void setTorqueConst(const double torque_const); 48 | void setRotorPoles(const int rotor_poles); 49 | void setHallSensors(const int hall_sensors); 50 | void setJointType(const int joint_type); 51 | void setScrewLead(const double screw_lead); 52 | double getZeroPosition() const; 53 | void spinSensorData(); 54 | double getPositionSens(); 55 | double getVelocitySens(); 56 | double getEffortSens(); 57 | void executeCalibration(); 58 | void updateSensor(const std::shared_ptr& packet); 59 | bool calibrate(); 60 | struct CalibrationParameters 61 | { 62 | double calibration_position; 63 | bool enable_calibration; 64 | }; 65 | CalibrationParameters getCalibrationParameters() const; 66 | 67 | private: 68 | rclcpp::Node::SharedPtr node_; 69 | std::shared_ptr interface_ptr_; 70 | VescStepDifference vesc_step_difference_; 71 | 72 | const std::string DUTY_ = "duty"; 73 | const std::string CURRENT_ = "current"; 74 | 75 | bool calibration_flag_; 76 | bool calibration_rewind_; 77 | double calibration_current_; // unit: A 78 | double calibration_strict_current_; // unit: A 79 | double calibration_duty_; // 0.0 ~ 1.0 80 | double calibration_strict_duty_; // 0.0 ~ 1.0 81 | std::string calibration_mode_; // "duty" or "current" (default: "current") 82 | double calibration_position_; // unit: rad or m 83 | double zero_position_; // unit: rad or m 84 | double kp_, ki_, kd_; 85 | double i_clamp_, duty_limiter_; 86 | bool antiwindup_; 87 | double control_rate_; 88 | int num_rotor_poles_; // the number of rotor poles 89 | int num_hall_sensors_; // the number of hall sensors 90 | double gear_ratio_, torque_const_; // physical params 91 | double screw_lead_; // linear distance (m) of 1 revolution 92 | int joint_type_; 93 | // ros::Timer control_timer_; 94 | // Internal variables for PID control 95 | double target_position_; 96 | double target_position_previous_; 97 | double sens_position_, sens_velocity_, sens_effort_; 98 | double position_steps_; 99 | double position_resolution_; 100 | int32_t steps_previous_; 101 | double error_integ_; 102 | // Internal variables for initialization 103 | bool sensor_initialize_; 104 | int calibration_steps_; 105 | double calibration_previous_position_; 106 | std::string calibration_result_path_; 107 | double upper_endstop_position_, lower_endstop_position_; 108 | rclcpp::Subscription::SharedPtr endstop_sub_; 109 | std::deque endstop_deque_; 110 | int endstop_window_; 111 | double endstop_threshold_; 112 | double endstop_margin_; 113 | 114 | // void controlTimerCallback(const ros::TimerEvent& e); 115 | void endstopCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg); 116 | }; 117 | 118 | } // namespace vesc_hw_interface 119 | 120 | #endif // VESC_HW_INTERFACE_VESC_SERVO_CONTROLLER_HPP_ 121 | -------------------------------------------------------------------------------- /vesc_driver/include/vesc_driver/vesc_packet_factory.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of Softbank Corp. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | ********************************************************************/ 29 | 30 | /** NOTE ************************************************************* 31 | * This program had been developed by Michael T. Boulet at MIT under 32 | * the BSD 3-clause License until Dec. 2016. Since Nov. 2019, Softbank 33 | * Corp. takes over development as new packages. 34 | ********************************************************************/ 35 | 36 | #ifndef VESC_DRIVER_VESC_PACKET_FACTORY_HPP_ 37 | #define VESC_DRIVER_VESC_PACKET_FACTORY_HPP_ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | #include "vesc_driver/data_map.hpp" 54 | #include "vesc_driver/vesc_packet.hpp" 55 | 56 | namespace vesc_driver 57 | { 58 | /** 59 | * @brief Creates VESC packets from raw data. 60 | **/ 61 | class VescPacketFactory : private boost::noncopyable 62 | { 63 | public: 64 | static VescPacketPtr createPacket(const Buffer::const_iterator&, const Buffer::const_iterator&, int*, std::string*); 65 | 66 | typedef std::function)> CreateFn; 67 | 68 | /** Register a packet type with the factory. */ 69 | static void registerPacketType(int, CreateFn); 70 | 71 | private: 72 | typedef std::map FactoryMap; 73 | static FactoryMap* getMap(); 74 | }; 75 | 76 | /** 77 | * @def REGISTER_PACKET_TYPE 78 | * @brief Registers a type of the packet 79 | **/ 80 | #define REGISTER_PACKET_TYPE(id, klass) \ 81 | class klass##Factory \ 82 | { \ 83 | public: \ 84 | klass##Factory() \ 85 | { \ 86 | VescPacketFactory::registerPacketType((id), &klass##Factory::create); \ 87 | } \ 88 | static VescPacketPtr create(std::shared_ptr frame) \ 89 | { \ 90 | return VescPacketPtr(new klass(frame)); \ 91 | } \ 92 | }; \ 93 | static klass##Factory global_##klass##Factory; 94 | 95 | } // namespace vesc_driver 96 | 97 | #endif // VESC_DRIVER_VESC_PACKET_FACTORY_HPP_ 98 | -------------------------------------------------------------------------------- /vesc_driver/include/vesc_driver/vesc_driver.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of Softbank Corp. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | ********************************************************************/ 29 | 30 | /** NOTE ************************************************************* 31 | * This program had been developed by Michael T. Boulet at MIT under 32 | * the BSD 3-clause License until Dec. 2016. Since Nov. 2019, Softbank 33 | * Corp. takes over development as new packages. 34 | ********************************************************************/ 35 | 36 | // TODO: Migrate to ROS2 37 | 38 | #ifndef VESC_DRIVER_VESC_DRIVER_H_ 39 | #define VESC_DRIVER_VESC_DRIVER_H_ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | 50 | #include 51 | #include 52 | #include 53 | 54 | #include "vesc_driver/vesc_interface.h" 55 | #include "vesc_driver/vesc_packet.h" 56 | 57 | namespace vesc_driver 58 | { 59 | class VescDriver 60 | { 61 | public: 62 | VescDriver(ros::NodeHandle nh, ros::NodeHandle private_nh); 63 | 64 | private: 65 | // interface to the VESC 66 | VescInterface vesc_; 67 | void vescPacketCallback(const std::shared_ptr& packet); 68 | void vescErrorCallback(const std::string& error); 69 | 70 | // limits on VESC commands 71 | struct CommandLimit 72 | { 73 | CommandLimit(const ros::NodeHandle& nh, const std::string& str, 74 | const boost::optional& min_lower = boost::optional(), 75 | const boost::optional& max_upper = boost::optional()); 76 | double clip(double value); 77 | std::string name; 78 | boost::optional lower; 79 | boost::optional upper; 80 | }; 81 | CommandLimit duty_cycle_limit_; 82 | CommandLimit current_limit_; 83 | CommandLimit brake_limit_; 84 | CommandLimit speed_limit_; 85 | CommandLimit position_limit_; 86 | CommandLimit servo_limit_; 87 | 88 | // ROS services 89 | ros::Publisher state_pub_; 90 | ros::Publisher servo_sensor_pub_; 91 | ros::Subscriber duty_cycle_sub_; 92 | ros::Subscriber current_sub_; 93 | ros::Subscriber brake_sub_; 94 | ros::Subscriber speed_sub_; 95 | ros::Subscriber position_sub_; 96 | ros::Subscriber servo_sub_; 97 | ros::Timer timer_; 98 | 99 | // driver modes (possible states) 100 | typedef enum 101 | { 102 | MODE_INITIALIZING, 103 | MODE_OPERATING 104 | } driver_mode_t; 105 | 106 | // other variables 107 | driver_mode_t driver_mode_; ///< driver state machine mode (state) 108 | int fw_version_major_; ///< firmware major version reported by vesc 109 | int fw_version_minor_; ///< firmware minor version reported by vesc 110 | int num_motor_pole_pairs_; // the number of motor pole pairs 111 | 112 | // ROS callbacks 113 | void timerCallback(const ros::TimerEvent& event); 114 | void dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle); 115 | void currentCallback(const std_msgs::Float64::ConstPtr& current); 116 | void brakeCallback(const std_msgs::Float64::ConstPtr& brake); 117 | void speedCallback(const std_msgs::Float64::ConstPtr& speed); 118 | void positionCallback(const std_msgs::Float64::ConstPtr& position); 119 | void servoCallback(const std_msgs::Float64::ConstPtr& servo); 120 | }; 121 | 122 | } // namespace vesc_driver 123 | 124 | #endif // VESC_DRIVER_VESC_DRIVER_H_ 125 | -------------------------------------------------------------------------------- /vesc_hw_interface/src/vesc_step_difference.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2023 SoftBank Corp. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | ********************************************************************/ 17 | 18 | #include "vesc_hw_interface/vesc_step_difference.hpp" 19 | namespace vesc_step_difference 20 | { 21 | /** 22 | * Hall sensors attached to brushless motors have low angular resolution. 23 | * When looking at the time variation of the counter value obtained from the Hall sensor, 24 | * the value is slow and oscillatory. (The value will be either 0 or 1) 25 | * To prevent this, when the speed is slow, the sampling time is increased and the time variation of the counter value 26 | * is smoothed. This means that the smoothing filter is applied only at low speeds. 27 | */ 28 | VescStepDifference::VescStepDifference() 29 | { 30 | enable_smooth_ = false; 31 | } 32 | 33 | VescStepDifference::~VescStepDifference() 34 | { 35 | } 36 | 37 | /** 38 | * Enables smoothing and initializes internal parameters. 39 | * If this function is not called, smoothing is disabled and the getStepDifference function outputs the difference from 40 | * the previous step as is. 41 | */ 42 | void VescStepDifference::enableSmooth(double control_rate, double max_sampling_time, int max_step_diff) 43 | { 44 | step_diff_vw_max_step_ = max_step_diff; 45 | int step_diff_vw_max_window_size_ = static_cast(std::round(control_rate * max_sampling_time)); 46 | if (step_diff_vw_max_window_size_ > 0) 47 | { 48 | enable_smooth_ = true; 49 | step_input_queue_.resize(step_diff_vw_max_window_size_ + 1); 50 | } 51 | else 52 | { 53 | // Disable smoothing if step_diff_vw_max_window_size_ is too small 54 | RCLCPP_WARN(rclcpp::get_logger("VescHwInterface"), "[VescStepDifference::enableSmooth] max_sampling_time is too " 55 | "small, disable smoothing"); 56 | enable_smooth_ = false; 57 | } 58 | return; 59 | } 60 | 61 | /** 62 | * Initialize the buffer (past values) for calculating the step difference. 63 | * You must enter the current step for input. 64 | */ 65 | void VescStepDifference::resetStepDifference(const double step_in) 66 | { 67 | if (enable_smooth_) 68 | { 69 | this->stepDifferenceVariableWindow(step_in, true); 70 | } 71 | else 72 | { 73 | this->stepDifferenceRaw(step_in, true); 74 | } 75 | } 76 | 77 | /** 78 | * Outputs the difference between the previous input and the current input. 79 | * Smoothing is performed when "enable_smooth_" is true 80 | */ 81 | double VescStepDifference::getStepDifference(const double step_in) 82 | { 83 | double output; 84 | if (enable_smooth_) 85 | { 86 | output = this->stepDifferenceVariableWindow(step_in, false); 87 | } 88 | else 89 | { 90 | output = this->stepDifferenceRaw(step_in, false); 91 | } 92 | return output; 93 | } 94 | 95 | /** 96 | * Function called when the filter is disabled 97 | * 98 | * Outputs the difference of the counter value from the previous value. 99 | * This function is used when no filter is used. 100 | * 101 | * @step_in Enter the current counter value 102 | * @reset Used for initialization. 103 | * @return Returns the difference from the previous value. 104 | */ 105 | double VescStepDifference::stepDifferenceRaw(const double step_in, bool reset) 106 | { 107 | if (reset) 108 | { 109 | step_in_previous_ = static_cast(step_in); 110 | return 0.0; 111 | } 112 | int32_t step_diff = static_cast(step_in) - step_in_previous_; 113 | step_in_previous_ = static_cast(step_in); 114 | return static_cast(step_diff); 115 | } 116 | 117 | /** 118 | * Function called when the filter is enabled 119 | * 120 | * Output smoothed differences. 121 | * The smaller the difference, the longer the sampling time is stretched and smoothed. 122 | * If the latest difference is greater than max_step_diff, no smoothing is performed. 123 | * Smoothing is performed for max_sampling_time seconds when the latest difference is 0. 124 | * 125 | * @step_in Enter the current counter value 126 | * @reset Used for initialization. 127 | * @return Returns the difference from the previous value. 128 | */ 129 | double VescStepDifference::stepDifferenceVariableWindow(const double step_in, bool reset) 130 | { 131 | if (reset) 132 | { 133 | // Fill the buffer with the current value 134 | std::fill(step_input_queue_.begin(), step_input_queue_.end(), static_cast(step_in)); 135 | return 0.0; 136 | } 137 | // Increment buffer 138 | step_input_queue_.pop_back(); 139 | step_input_queue_.push_front(static_cast(step_in)); 140 | // Calculate window size 141 | int latest_step_diff = std::abs(step_input_queue_[0] - step_input_queue_[1]); 142 | int window_size = step_input_queue_.size() - 1; 143 | if (latest_step_diff >= step_diff_vw_max_step_) 144 | { 145 | window_size = 1; 146 | } 147 | else if (latest_step_diff > 0) 148 | { 149 | // The larger the latest_step_diff, the smaller the window_size. 150 | // window_size: minimum 1, maximum "step_input_queue_.size() - 1" ( == step_diff_vw_max_window_size) 151 | window_size = 1 + static_cast(std::round( 152 | static_cast(step_input_queue_.size() - 2) * 153 | (1.0 - static_cast(latest_step_diff) / static_cast(step_diff_vw_max_step_)))); 154 | } 155 | // Get output 156 | int32_t step_diff = step_input_queue_[0] - step_input_queue_[window_size]; 157 | return static_cast(step_diff) / static_cast(window_size); 158 | } 159 | 160 | } // namespace vesc_step_difference 161 | -------------------------------------------------------------------------------- /vesc_driver/include/vesc_driver/vesc_interface.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of Softbank Corp. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | ********************************************************************/ 29 | 30 | /** NOTE ************************************************************* 31 | * This program had been developed by Michael T. Boulet at MIT under 32 | * the BSD 3-clause License until Dec. 2016. Since Nov. 2019, Softbank 33 | * Corp. takes over development as new packages. 34 | ********************************************************************/ 35 | 36 | #ifndef VESC_DRIVER_VESC_INTERFACE_HPP_ 37 | #define VESC_DRIVER_VESC_INTERFACE_HPP_ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | 50 | #include "vesc_driver/vesc_packet.hpp" 51 | #include "vesc_driver/vesc_packet_factory.hpp" 52 | 53 | namespace vesc_driver 54 | { 55 | /** 56 | * Class providing an interface to the Vedder VESC motor controller via a serial port interface. 57 | */ 58 | class VescInterface 59 | { 60 | public: 61 | typedef std::function PacketHandlerFunction; 62 | typedef std::function ErrorHandlerFunction; 63 | 64 | /** 65 | * Creates a VescInterface object. Opens the serial port interface to the VESC if @p port is not 66 | * empty, otherwise the serial port remains closed until connect() is called. 67 | * 68 | * @param port Address of the serial port, e.g. '/dev/ttyUSB0'. 69 | * @param packet_handler Function this class calls when a VESC packet is received. 70 | * @param error_handler Function this class calls when an error is detected, such as a bad 71 | * checksum. 72 | * 73 | * @throw SerialException 74 | */ 75 | VescInterface(const std::string& port = std::string(), 76 | const PacketHandlerFunction& packet_handler = PacketHandlerFunction(), 77 | const ErrorHandlerFunction& error_handler = ErrorHandlerFunction()); 78 | 79 | /** 80 | * VescInterface destructor. 81 | */ 82 | ~VescInterface(); 83 | 84 | /** 85 | * Sets / updates the function that this class calls when a VESC packet is received. 86 | */ 87 | void setPacketHandler(const PacketHandlerFunction& handler); 88 | 89 | /** 90 | * Sets / updates the function that this class calls when an error is detected, such as a bad 91 | * checksum. 92 | */ 93 | void setErrorHandler(const ErrorHandlerFunction& handler); 94 | 95 | /** 96 | * Opens the serial port interface to the VESC. 97 | * 98 | * @throw SerialException 99 | */ 100 | void connect(const std::string& port); 101 | 102 | /** 103 | * Closes the serial port interface to the VESC. 104 | */ 105 | void disconnect(); 106 | 107 | /** 108 | * Gets the status of the serial interface to the VESC. 109 | * 110 | * @return Returns true if the serial port is open, false otherwise. 111 | */ 112 | bool isConnected() const; 113 | /** 114 | * Returns whether the data has been updated or not. 115 | * 116 | * @return Returns true if the data has been updated, false otherwise. 117 | */ 118 | bool isRxDataUpdated() const; 119 | 120 | /** 121 | * Send a VESC packet. 122 | */ 123 | void send(const VescPacket& packet); 124 | 125 | void requestFWVersion(); 126 | void requestState(); 127 | void requestMCConfiguration(); 128 | void setDutyCycle(double duty_cycle); 129 | void setCurrent(double current); 130 | void setBrake(double brake); 131 | void setSpeed(double speed); 132 | void setPosition(double position); 133 | void setServo(double servo); 134 | 135 | private: 136 | // Pimpl - hide serial port members from class users 137 | class Impl; 138 | std::unique_ptr impl_; 139 | }; 140 | 141 | // todo: review 142 | class SerialException : public std::exception 143 | { 144 | // Disable copy constructors 145 | SerialException& operator=(const SerialException&); 146 | std::string e_what_; 147 | 148 | public: 149 | explicit SerialException(const char* description) 150 | { 151 | std::stringstream ss; 152 | ss << "SerialException " << description << " failed."; 153 | e_what_ = ss.str(); 154 | } 155 | SerialException(const SerialException& other) : e_what_(other.e_what_) 156 | { 157 | } 158 | virtual ~SerialException() throw() 159 | { 160 | } 161 | virtual const char* what() const throw() 162 | { 163 | return e_what_.c_str(); 164 | } 165 | }; 166 | 167 | } // namespace vesc_driver 168 | 169 | #endif // VESC_DRIVER_VESC_INTERFACE_HPP_ 170 | -------------------------------------------------------------------------------- /vesc_driver/src/vesc_packet_factory.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of Softbank Corp. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | ********************************************************************/ 29 | 30 | /** NOTE ************************************************************* 31 | * This program had been developed by Michael T. Boulet at MIT under 32 | * the BSD 3-clause License until Dec. 2016. Since Nov. 2019, Softbank 33 | * Corp. takes over development as new packages. 34 | ********************************************************************/ 35 | 36 | #include "vesc_driver/vesc_packet_factory.hpp" 37 | 38 | namespace vesc_driver 39 | { 40 | /** 41 | * @brief Creates failure message when createPacket can not create a packet 42 | **/ 43 | VescPacketPtr createFailed(int* p_num_bytes_needed, std::string* p_what, const std::string& what, 44 | const int num_bytes_needed = 0) 45 | { 46 | if (p_num_bytes_needed != NULL) 47 | *p_num_bytes_needed = num_bytes_needed; 48 | if (p_what != NULL) 49 | *p_what = what; 50 | return VescPacketPtr(); 51 | } 52 | 53 | /** 54 | * @brief Creates a VESC packet 55 | * @details Create a VescPacket from a buffer (factory function). Packet must 56 | * start @p begin and complete (end of frame character) before what @p begin 57 | * points to end. The buffer element @p end is not examined, i.e. it can be the 58 | * past-the-end element. This function only returns a packet if the packet is 59 | * valid, i.e. valid size, matching checksum, complete etc. An empty pointer 60 | * is returned if a packet cannot be found or if it is invalid. If a valid 61 | * packet is not found, an optional output parameter @p what is set to a string 62 | * providing a reason why a packet was not found. If a packet was not found 63 | * because additional bytes are needed on the buffer, optional output parameter 64 | * @p num_bytes_needed will contain the number of bytes needed to either 65 | * determine the size of the packet or complete the packet. Output parameters 66 | * @p num_bytes_needed and @p what will be set to 0 and empty if a valid packet 67 | * is found. 68 | * 69 | * @param begin[in] Iterator to a buffer at the start-of-frame character. 70 | * @param end[in] Iterator to the buffer past-the-end element. 71 | * @param num_bytes_needed[out] Number of bytes needed to determine the packet 72 | * size or complete the frame. 73 | * @param what[out] Message string giving a reason why the packet was not found. 74 | * @return Pointer to a valid VescPacket if successful; otherwise, an empty 75 | * pointer. 76 | **/ 77 | VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begin, const Buffer::const_iterator& end, 78 | int* num_bytes_needed, std::string* what) 79 | { 80 | // initializes output variables 81 | if (num_bytes_needed != NULL) 82 | { 83 | *num_bytes_needed = 0; 84 | } 85 | if (what != NULL) 86 | { 87 | what->clear(); 88 | } 89 | 90 | // requires at least VESC_MIN_FRAME_SIZE bytes in buffer 91 | int buffer_size(std::distance(begin, end)); 92 | if (buffer_size < VescFrame::VESC_MIN_FRAME_SIZE) 93 | { 94 | return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame", 95 | VescFrame::VESC_MIN_FRAME_SIZE - buffer_size); 96 | } 97 | 98 | // checks whether buffer begins with a start-of-frame 99 | if (VescFrame::VESC_SOF_VAL_SMALL_FRAME != *begin && VescFrame::VESC_SOF_VAL_LARGE_FRAME != *begin) 100 | { 101 | return createFailed(num_bytes_needed, what, "Buffer must begin with start-of-frame character"); 102 | } 103 | 104 | // gets a view of the payload 105 | BufferRangeConst view_payload; 106 | if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *begin) 107 | { 108 | // payload size field is one byte 109 | view_payload.first = begin + 2; 110 | view_payload.second = view_payload.first + *(begin + 1); 111 | } 112 | else 113 | { 114 | assert(VescFrame::VESC_SOF_VAL_LARGE_FRAME == *begin); 115 | // payload size field is two bytes 116 | view_payload.first = begin + 3; 117 | view_payload.second = view_payload.first + (*(begin + 1) << 8) + *(begin + 2); 118 | } 119 | 120 | // checks the length 121 | if (boost::distance(view_payload) > VescFrame::VESC_MAX_PAYLOAD_SIZE) 122 | { 123 | return createFailed(num_bytes_needed, what, "Invalid payload length"); 124 | } 125 | 126 | // gets iterators to crc field, end-of-frame field, and a view of the whole frame 127 | Buffer::const_iterator iter_crc(view_payload.second); 128 | Buffer::const_iterator iter_eof(iter_crc + 2); 129 | BufferRangeConst view_frame(begin, iter_eof + 1); 130 | 131 | // chekcs whether enough data is loaded in the buffer to complete the frame 132 | int frame_size = boost::distance(view_frame); 133 | if (buffer_size < frame_size) 134 | return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame", frame_size - buffer_size); 135 | 136 | // checks whether the end-of-frame character is valid 137 | if (VescFrame::VESC_EOF_VAL != *iter_eof) 138 | return createFailed(num_bytes_needed, what, "Invalid end-of-frame character"); 139 | 140 | // checks whether the crc is valid 141 | uint16_t crc = (static_cast(*iter_crc) << 8) + *(iter_crc + 1); 142 | VescFrame::CRC crc_calc; 143 | crc_calc.process_bytes(&(*view_payload.first), boost::distance(view_payload)); 144 | if (crc != crc_calc.checksum()) 145 | return createFailed(num_bytes_needed, what, "Invalid checksum"); 146 | 147 | // constructs the raw frame 148 | std::shared_ptr raw_frame(new VescFrame(view_frame, view_payload)); 149 | 150 | // constructs the corresponding subclass if the packet has a payload 151 | if (boost::distance(view_payload) > 0) 152 | { 153 | // gets constructor function from payload id 154 | FactoryMap* p_map(getMap()); 155 | FactoryMap::const_iterator search(p_map->find(*view_payload.first)); 156 | 157 | if (search != p_map->end()) 158 | { 159 | return search->second(raw_frame); 160 | } 161 | else 162 | { 163 | // no subclass constructor for this packet 164 | return createFailed(num_bytes_needed, what, "Unkown payload type."); 165 | } 166 | } 167 | else 168 | { 169 | // no payload 170 | return createFailed(num_bytes_needed, what, "Frame does not have a payload"); 171 | } 172 | } 173 | 174 | /** 175 | * @brief Registers a type of the packet 176 | * @param payload_id Payload ID 177 | * @param fn Function pointer 178 | **/ 179 | void VescPacketFactory::registerPacketType(int payload_id, CreateFn fn) 180 | { 181 | FactoryMap* p_map(getMap()); 182 | assert(0 == p_map->count(payload_id)); 183 | (*p_map)[payload_id] = fn; 184 | } 185 | 186 | /** 187 | * @brief Construct a map on first use 188 | * @return Pointer to the constructed map 189 | **/ 190 | VescPacketFactory::FactoryMap* VescPacketFactory::getMap() 191 | { 192 | static FactoryMap m; 193 | return &m; 194 | } 195 | 196 | REGISTER_PACKET_TYPE(COMM_FW_VERSION, VescPacketFWVersion) 197 | REGISTER_PACKET_TYPE(COMM_GET_VALUES, VescPacketValues) 198 | REGISTER_PACKET_TYPE(COMM_GET_MCCONF, VescPacketMCConf) 199 | 200 | } // namespace vesc_driver 201 | -------------------------------------------------------------------------------- /vesc_hw_interface/README.md: -------------------------------------------------------------------------------- 1 | # vesc_hw_interface 2 | A ROS2 ros2_control hardware interface for controlling BLDC motors using a [VESC (Vedder Electronic Speed Controller)](https://vesc-project.com/). 3 | This interface supports multiple control modes, integrates with PID controllers, and is designed to work with common URDF joint types. 4 | 5 | ## Features 6 | - ROS2 hardware interface for BLDC motors using VESC 7 | - Supports the following command modes: 8 | - **Position control** 9 | - **Velocity control** 10 | - **Position duty control** 11 | - **Velocity duty control** 12 | - PID controller integration (either using VESC internal PID or external PID within this interface) 13 | - Homing support via current or duty mode 14 | 15 | ## Command Modes 16 | This hardware interface supports multiple command modes. 17 | Each has its own behavior, PID control source, and supported URDF joint types. 18 | | Command Mode | Description | PID Source | Supported Joint Types | 19 | |--------------|-------------|------------|------------------------| 20 | | `position` | Uses VESC's built-in position controller | VESC | `revolute`, `prismatic` | 21 | | `velocity` | Uses VESC's built-in velocity controller | VESC | `continuous` | 22 | | `position_duty` | Uses external PID controller in this interface to control duty cycle for position | External | `revolute`, `prismatic` | 23 | | `velocity_duty` | Uses external PID controller in this interface to control duty cycle for velocity | External | `continuous` | 24 | 25 | > [!NOTE] 26 | > - **position**/**velocity** modes rely on **VESC internal PID**. Configure gains using VESC Tool. 27 | > - **position_duty**/**velocity_duty** modes use **external PID control**, and PID gains must be set via URDF parameters. 28 | 29 | > [!WARNING] 30 | > URDF joint types must match the selected command mode; otherwise, the motor may behave unexpectedly. 31 | 32 | > [!IMPORTANT] 33 | > When using `position` mode, the joint range must map to a 0°–90° VESC range using the "Position Angle Division" setting. [More info.](#%EF%B8%8F-notes-on-vesc-pid-position-control) 34 | 35 | ## Parameters 36 | Parameters should be set under the `` tag in your URDF or `xacro` file. 37 | Check the `vesc_hw_interface/launch` directory for examples. 38 | 39 | ### Common Parameters (All command mode) 40 | | Name | Type | Requirement | Default | Description | 41 | |------|------|-------------|---------|-------------| 42 | | port | string | required | — | Serial port name to connect to the VESC | 43 | | command_mode | string | required | — | Control mode: `"position"`, `"velocity"`, `"position_duty"`, `"velocity_duty"` | 44 | 45 | ### Parameters for `position` and `position_duty` 46 | | Name | Type | Requirement | Default | Description | 47 | |------|------|-------------|---------|-------------| 48 | | servo/calibration | bool | optional | true | Runs homing operation before controller starts | 49 | | servo/calibration_mode | string | optional | current | Homing mode: `"current"` or `"duty"` | 50 | | servo/calibration_current | double | optional | 6.0 | Max current used in homing operation | 51 | | servo/calibration_strict_current | double | optional | same as `calibration_current` | Max current used in strict homing operation. If the motor reaches the origin with `calibration_current`, it backs off and re-homes using this value. If equal to `calibration_current`, strict homing operation is skipped. | 52 | | servo/calibration_duty | double | optional | 0.1 | Max duty used in homing operation | 53 | | servo/calibration_strict_duty | double | optional | same as `calibration_duty` | Max duty used in strict homing operation. If the motor reaches the origin with `calibration_duty`, it backs off and re-homes using this value. If equal to `calibration_duty`, strict homing operation is skipped. | 54 | | servo/calibration_position | double | optional | 0.0 | The position value assigned after homing operation | 55 | | servo/calibration_result_path | string | optional | "" | If not empty, the last position is saved to this path | 56 | | servo/last_position | double | optional | 0.0 | Used as the current position if `calibration` is false | 57 | | servo/use_endstop | bool | optional | false | Enables endstop sensor support. Subscribes to the `endstop` topic (`std_msgs/Bool`). | 58 | | servo/endstop_margin | double | optional | 0.02 | Position error margin to consider endstop as reached | 59 | | servo/endstop_window | int | optional | 1 | Number of recent endstop readings used to evaluate endstop status | 60 | | servo/endstop_threshold | double | optional | 0.8 | Threshold to determine endstop status based on average sensor values | 61 | 62 | > [!NOTE] 63 | > - If `calibration` is set to `true`, the servo will move to the limit using `calibration_current` or `calibration_duty`. Make sure it's safe for the servo to move. 64 | > - If `servo/calibration` is set to `false`, the servo will use `servo/last_position` as its current position. To preserve the last position, save it using `servo/calibration_result_path` and reload it at startup. 65 | > - If the servo is moved manually, controlled externally, or `servo/calibration_result_path` is unset, you should re-run calibration. 66 | 67 | ### Parameters for `position_duty` and `velocity_duty` 68 | | Name | Type | Requirement | Default | Description | 69 | |------|------|-------------|---------|-------------| 70 | | num_hall_sensors | int | optional | 3 | Number of hall sensors | 71 | | num_rotor_poles | int | optional | 2 | Number of rotor poles | 72 | | gear_ratio | double | optional | 1.0 | Gear reduction ratio | 73 | | torque_const | double | optional | 1.0| Torque constant of the motor | 74 | | screw_lead | double | optional | 1.0 | Screw lead (only used when joint type is `prismatic`) | 75 | 76 | > [!NOTE] 77 | > `position` and `velocity` command mode will use the gear ratio, num rotor poles, and torque constants from VESC. 78 | Configure these parameters using VESC tool. 79 | 80 | ### Parameters exclusive to `position_duty` 81 | | Name | Type | Requirement | Default | Description | 82 | |------|------|-------------|---------|-------------| 83 | | servo/Kp | double | optional | 50.0 | P gain | 84 | | servo/Ki | double | optional | 0.0 | I gain | 85 | | servo/Kd | double | optional | 1.0 | D gain | 86 | | servo/i_clamp | double | optional | 1.0 | I clamp | 87 | | servo/duty_limiter | double | optional | 1.0 | Duty cycle limiter | 88 | | servo/antiwindup | bool | optional | true | Enable anti-windup | 89 | | servo/control_rate | double | optional | 100.0 | PID control rate (Hz) | 90 | | servo/enable_smooth_diff | bool | optional | true | Enable smoothing | 91 | | servo/smooth_diff/max_sample_sec | double | optional | 1.0 | Max sampling window (sec) | 92 | | servo/smooth_diff/max_smooth_step | int | optional | 10 | Max smoothing steps | 93 | 94 | ### Parameters exclusive to `velocity_duty` 95 | | Name | Type | Requirement | Default | Description | 96 | |------|------|-------------|---------|-------------| 97 | | motor/Kp | double | optional | 0.005 | P gain | 98 | | motor/Ki | double | optional | 0.005 | I gain | 99 | | motor/Kd | double | optional | 0.0025 | D gain | 100 | | motor/i_clamp | double | optional | 0.2 | I clamp | 101 | | motor/duty_limiter | double | optional | 1.0 | Duty cycle limiter | 102 | | motor/antiwindup | bool | optional | true | Enable anti-windup | 103 | | motor/control_rate | double | optional | 100.0 | PID control rate (Hz) | 104 | | motor/enable_smooth_diff | bool | optional | true | Enable smoothing | 105 | | motor/smooth_diff/max_sample_sec | double | optional | 1.0 | Max sampling window (sec) | 106 | | motor/smooth_diff/max_smooth_step | int | optional | 10 | Max smoothing steps | 107 | 108 | ## ⚠️ Notes on VESC PID Position Control 109 | When using the `position` command mode, the hardware interface will use the VESC's internal PID position controller, which behaves as follows: 110 | - VESC uses a circular (modular) angle representation, keeping position values within [0°, 360°). If the value exceeds 360° or drops below 0°, it wraps around accordingly. 111 | - VESC moves the motor along the **shortest angular path**. For example, moving from 10° to 350° results in backward rotation. 112 | 113 | This behavior is often undesirable in ROS, where we expect absolute angular movement. 114 | To address this, the hardware interface remaps joint limits to align with the VESC’s internal behavior. 115 | 116 | **This hardware interface assumes your motor’s joint limits correspond to a 0° to 90° range in VESC.** 117 | You must set the **"Position Angle Division"** parameter in VESC Tool so that the physical joint motion between its lower and upper limits is scaled to exactly 90° in the VESC’s internal representation. (e.g., 0° is motor lower limit and 90° is motor upper limit) 118 | 119 | This setup ensures the motor stays within a clean operating range and avoids unintended wrap-around or shortest-path movement. 120 | 121 | ## License 122 | `vesc_hw_interface` is licensed under the [Apache 2.0 license](https://www.apache.org/licenses/LICENSE-2.0.html). 123 | -------------------------------------------------------------------------------- /vesc_driver/include/vesc_driver/vesc_packet.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of Softbank Corp. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | ********************************************************************/ 29 | 30 | /** NOTE ************************************************************* 31 | * This program had been developed by Michael T. Boulet at MIT under 32 | * the BSD 3-clause License until Dec. 2016. Since Nov. 2019, Softbank 33 | * Corp. takes over development as new packages. 34 | ********************************************************************/ 35 | 36 | #ifndef VESC_DRIVER_VESC_PACKET_HPP_ 37 | #define VESC_DRIVER_VESC_PACKET_HPP_ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include "vesc_driver/data_map.hpp" 53 | 54 | namespace vesc_driver 55 | { 56 | typedef std::vector Buffer; 57 | typedef std::pair BufferRange; 58 | typedef std::pair BufferRangeConst; 59 | 60 | /** 61 | * @brief The raw frame for communicating with the VESC 62 | **/ 63 | class VescFrame 64 | { 65 | public: 66 | /** 67 | * @brief Destructor 68 | **/ 69 | virtual ~VescFrame() 70 | { 71 | } 72 | 73 | /** 74 | * @brief Gets a reference of the frame 75 | * @return Reference of the frame 76 | **/ 77 | virtual const Buffer& getFrame() const 78 | { 79 | return frame_; 80 | } 81 | 82 | /* packet properties */ 83 | static const int16_t VESC_MAX_PAYLOAD_SIZE = 1024; // Maximum payload size (bytes) 84 | static const int16_t VESC_MIN_FRAME_SIZE = 5; // Smallest frame size (bytes) 85 | static const int16_t VESC_MAX_FRAME_SIZE = 6 + VESC_MAX_PAYLOAD_SIZE; // Largest frame size (bytes) 86 | static const int16_t VESC_SOF_VAL_SMALL_FRAME = 2; // Start of "small" frame value 87 | static const int16_t VESC_SOF_VAL_LARGE_FRAME = 3; // Start of "large" frame value 88 | static const int16_t VESC_EOF_VAL = 3; // End-of-frame value 89 | 90 | /** 91 | * @brief CRC parameters for the VESC 92 | **/ 93 | typedef boost::crc_optimal<16, 0x1021, 0, 0, false, false> CRC; 94 | 95 | protected: 96 | explicit VescFrame(const int16_t payload_size); 97 | 98 | Buffer frame_; 99 | // Stores frame data 100 | 101 | BufferRange payload_end_; 102 | // View into frame's payload section 103 | // .first: iterator which points the front of payload (in `frame_`/) 104 | // .second: iterator which points the tail of payload (in `frame_`) 105 | 106 | private: 107 | VescFrame(const BufferRangeConst& frame, const BufferRangeConst& payload); 108 | 109 | friend class VescPacketFactory; // gives VescPacketFactory access to private constructor 110 | }; 111 | 112 | /*------------------------------------------------------------------*/ 113 | 114 | /** 115 | * @brief VescFrame with a non-zero length payload 116 | **/ 117 | class VescPacket : public VescFrame 118 | { 119 | public: 120 | /** 121 | * @brief Destructor 122 | **/ 123 | virtual ~VescPacket() 124 | { 125 | } 126 | 127 | /** 128 | * @brief Gets the packet name 129 | * @return The packet name 130 | **/ 131 | virtual const std::string& getName() const 132 | { 133 | return name_; 134 | } 135 | 136 | protected: 137 | VescPacket(const std::string& name, const int16_t payload_size, const int16_t payload_id); 138 | VescPacket(const std::string& name, std::shared_ptr raw); 139 | double readBuffer(const uint8_t, const uint8_t) const; 140 | double readAutoBuffer(const int) const; 141 | 142 | private: 143 | std::string name_; 144 | }; 145 | 146 | typedef std::shared_ptr VescPacketPtr; 147 | typedef std::shared_ptr VescPacketConstPtr; 148 | 149 | /*------------------------------------------------------------------*/ 150 | 151 | /** 152 | * @brief Firmware version 153 | **/ 154 | class VescPacketFWVersion : public VescPacket 155 | { 156 | public: 157 | explicit VescPacketFWVersion(std::shared_ptr raw); 158 | 159 | int16_t fwMajor() const; 160 | int16_t fwMinor() const; 161 | }; 162 | 163 | /*------------------------------------------------------------------*/ 164 | 165 | /** 166 | * @brief Requests firmware version 167 | **/ 168 | class VescPacketRequestFWVersion : public VescPacket 169 | { 170 | public: 171 | VescPacketRequestFWVersion(); 172 | }; 173 | 174 | /*------------------------------------------------------------------*/ 175 | 176 | /** 177 | * @brief Gets values in COMM_GET_VALUES return packets 178 | **/ 179 | class VescPacketValues : public VescPacket 180 | { 181 | public: 182 | explicit VescPacketValues(std::shared_ptr raw); 183 | 184 | double getMosTemp() const; 185 | double getMotorTemp() const; 186 | double getMotorCurrent() const; 187 | double getInputCurrent() const; 188 | double getVelocityERPM() const; 189 | double getInputVoltage() const; 190 | double getDuty() const; 191 | double getConsumedCharge() const; 192 | double getInputCharge() const; 193 | double getConsumedPower() const; 194 | double getInputPower() const; 195 | double getTachometer() const; 196 | double getDisplacement() const; 197 | int getFaultCode() const; 198 | double getPosition() const; 199 | int getControllerID() const; 200 | }; 201 | 202 | /*------------------------------------------------------------------*/ 203 | 204 | /** 205 | * @brief Packet for requesting COMM_GET_VALUES return packets 206 | **/ 207 | class VescPacketRequestValues : public VescPacket 208 | { 209 | public: 210 | VescPacketRequestValues(); 211 | }; 212 | 213 | /*------------------------------------------------------------------*/ 214 | 215 | /** 216 | * @brief Gets values in COMM_GET_MCCONF return packets 217 | **/ 218 | class VescPacketMCConf : public VescPacket 219 | { 220 | public: 221 | explicit VescPacketMCConf(std::shared_ptr raw); 222 | 223 | MCConfiguration getConfig() const; 224 | 225 | private: 226 | MCConfiguration config_; 227 | }; 228 | 229 | /*------------------------------------------------------------------*/ 230 | 231 | /** 232 | * @brief Packet for requesting COMM_GET_MCCONF return packets 233 | **/ 234 | class VescPacketRequestMCConf : public VescPacket 235 | { 236 | public: 237 | VescPacketRequestMCConf(); 238 | }; 239 | 240 | /*------------------------------------------------------------------*/ 241 | 242 | /** 243 | * @brief Packet for setting duty 244 | **/ 245 | class VescPacketSetDuty : public VescPacket 246 | { 247 | public: 248 | explicit VescPacketSetDuty(double duty); 249 | }; 250 | 251 | /*------------------------------------------------------------------*/ 252 | 253 | /** 254 | * @brief Packet for setting reference current 255 | **/ 256 | class VescPacketSetCurrent : public VescPacket 257 | { 258 | public: 259 | explicit VescPacketSetCurrent(double current); 260 | }; 261 | 262 | /*------------------------------------------------------------------*/ 263 | 264 | /** 265 | * @brief Packet for setting current brake 266 | **/ 267 | class VescPacketSetCurrentBrake : public VescPacket 268 | { 269 | public: 270 | explicit VescPacketSetCurrentBrake(double current_brake); 271 | }; 272 | 273 | /*------------------------------------------------------------------*/ 274 | 275 | /** 276 | * @brief Packet for setting reference angular velocity 277 | **/ 278 | class VescPacketSetVelocityERPM : public VescPacket 279 | { 280 | public: 281 | explicit VescPacketSetVelocityERPM(double vel_erpm); 282 | }; 283 | 284 | /*------------------------------------------------------------------*/ 285 | 286 | /** 287 | * @brief Packet for setting a reference position 288 | **/ 289 | class VescPacketSetPos : public VescPacket 290 | { 291 | public: 292 | explicit VescPacketSetPos(double pos); 293 | }; 294 | 295 | /*------------------------------------------------------------------*/ 296 | 297 | /** 298 | * @brief Packet for setting a servo position 299 | **/ 300 | class VescPacketSetServoPos : public VescPacket 301 | { 302 | public: 303 | explicit VescPacketSetServoPos(double servo_pos); 304 | }; 305 | 306 | } // namespace vesc_driver 307 | 308 | #endif // VESC_DRIVER_VESC_PACKET_HPP_ 309 | -------------------------------------------------------------------------------- /vesc_hw_interface/src/vesc_wheel_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2022 SoftBank Corp. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | ********************************************************************/ 17 | 18 | #include "vesc_hw_interface/vesc_wheel_controller.hpp" 19 | #include 20 | #include 21 | 22 | namespace vesc_hw_interface 23 | { 24 | 25 | void VescWheelController::init(hardware_interface::HardwareInfo& info, 26 | const std::shared_ptr& interface_ptr) 27 | { 28 | if (!interface_ptr) 29 | { 30 | rclcpp::shutdown(); 31 | } 32 | else 33 | { 34 | interface_ptr_ = interface_ptr; 35 | } 36 | 37 | kp_ = 0.005; 38 | if (info.hardware_parameters.find("motor/Kp") != info.hardware_parameters.end()) 39 | { 40 | kp_ = std::stod(info.hardware_parameters["motor/Kp"]); 41 | } 42 | ki_ = 0.005; 43 | if (info.hardware_parameters.find("motor/Ki") != info.hardware_parameters.end()) 44 | { 45 | ki_ = std::stod(info.hardware_parameters["motor/Ki"]); 46 | } 47 | kd_ = 0.0025; 48 | if (info.hardware_parameters.find("motor/Kd") != info.hardware_parameters.end()) 49 | { 50 | kd_ = std::stod(info.hardware_parameters["motor/Kd"]); 51 | } 52 | i_clamp_ = 0.2; 53 | if (info.hardware_parameters.find("motor/i_clamp") != info.hardware_parameters.end()) 54 | { 55 | i_clamp_ = std::stod(info.hardware_parameters["motor/i_clamp"]); 56 | } 57 | duty_limiter_ = 1.0; 58 | if (info.hardware_parameters.find("motor/duty_limiter") != info.hardware_parameters.end()) 59 | { 60 | duty_limiter_ = std::stod(info.hardware_parameters["motor/duty_limiter"]); 61 | } 62 | antiwindup_ = true; 63 | if (info.hardware_parameters.find("motor/antiwindup") != info.hardware_parameters.end()) 64 | { 65 | antiwindup_ = info.hardware_parameters["motor/antiwindup"] == "true"; 66 | } 67 | control_rate_ = 100.0; 68 | if (info.hardware_parameters.find("motor/control_rate") != info.hardware_parameters.end()) 69 | { 70 | control_rate_ = std::stod(info.hardware_parameters["motor/control_rate"]); 71 | } 72 | 73 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[Motor Gains] P: %f, I: %f, D: %f", kp_, ki_, kd_); 74 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[Motor Gains] I clamp: %f, Antiwindup: %s", i_clamp_, 75 | antiwindup_ ? "true" : "false"); 76 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[Motor Control] control_rate: %f", control_rate_); 77 | 78 | // Smoothing differentiation when hall sensor resolution is insufficient 79 | bool smooth_diff = true; 80 | if (info.hardware_parameters.find("motor/enable_smooth_diff") != info.hardware_parameters.end()) 81 | { 82 | smooth_diff = info.hardware_parameters["motor/enable_smooth_diff"] == "true"; 83 | } 84 | if (smooth_diff) 85 | { 86 | double smooth_diff_max_sampling_time = 1.0; 87 | if (info.hardware_parameters.find("motor/smooth_diff/max_sample_sec") != info.hardware_parameters.end()) 88 | { 89 | smooth_diff_max_sampling_time = std::stod(info.hardware_parameters["motor/smooth_diff/max_sample_sec"]); 90 | } 91 | int counter_td_vw_max_step = 10; 92 | if (info.hardware_parameters.find("motor/smooth_diff/max_smooth_step") != info.hardware_parameters.end()) 93 | { 94 | counter_td_vw_max_step = std::stoi(info.hardware_parameters["motor/smooth_diff/max_smooth_step"]); 95 | } 96 | vesc_step_difference_.enableSmooth(control_rate_, smooth_diff_max_sampling_time, counter_td_vw_max_step); 97 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), 98 | "[Motor Control] Smooth differentiation enabled, max_sample_sec: %f, max_smooth_step: %d", 99 | smooth_diff_max_sampling_time, counter_td_vw_max_step); 100 | } 101 | 102 | sensor_initialize_ = true; 103 | pid_initialize_ = true; 104 | position_sens_ = 0.0; 105 | target_velocity_ = 0.0; 106 | position_steps_ = 0.0; 107 | prev_steps_ = 0.0; 108 | velocity_sens_ = 0.0; 109 | effort_sens_ = 0.0; 110 | 111 | // control_timer_ = nh.createTimer(ros::Duration(1.0 / control_rate_), &VescWheelController::controlTimerCallback, 112 | // this); 113 | } 114 | 115 | void VescWheelController::control(const double control_rate) 116 | { 117 | if (pid_initialize_) 118 | { 119 | if (!sensor_initialize_) 120 | { 121 | // Start PID control only when sensor initialization is complete 122 | pid_initialize_ = false; 123 | vesc_step_difference_.resetStepDifference(position_steps_); 124 | } 125 | target_steps_ = position_steps_; 126 | error_ = 0.0; 127 | error_dt_ = 0.0; 128 | error_integ_ = 0.0; 129 | vesc_step_difference_.getStepDifference(position_steps_); 130 | interface_ptr_->setDutyCycle(0.0); 131 | return; 132 | } 133 | 134 | // convert rad/s to steps 135 | target_steps_ += target_velocity_ * (num_rotor_poles_ * num_hall_sensors_) / (2 * M_PI) / control_rate / gear_ratio_; 136 | 137 | // overflow check 138 | if ((target_steps_ - position_steps_) > std::numeric_limits::max()) 139 | { 140 | target_steps_ += 141 | static_cast(std::numeric_limits::min()) - static_cast(std::numeric_limits::max()); 142 | } 143 | else if ((target_steps_ - position_steps_) < std::numeric_limits::min()) 144 | { 145 | target_steps_ += 146 | static_cast(std::numeric_limits::max()) - static_cast(std::numeric_limits::min()); 147 | } 148 | 149 | // PID control 150 | double step_diff = vesc_step_difference_.getStepDifference(position_steps_); 151 | double current_vel = step_diff * 2.0 * M_PI / (num_rotor_poles_ * num_hall_sensors_) * control_rate * gear_ratio_; 152 | error_dt_ = target_velocity_ - current_vel; 153 | error_ = (target_steps_ - position_steps_) * 2.0 * M_PI / (num_rotor_poles_ * num_hall_sensors_); 154 | error_integ_ += (error_ / control_rate); 155 | error_integ_ = std::clamp(error_integ_, -i_clamp_ / ki_, i_clamp_ / ki_); 156 | 157 | const double u_p = kp_ * error_; 158 | const double u_d = kd_ * error_dt_; 159 | const double u_i = ki_ * error_integ_; 160 | double u = u_p + u_d + u_i; 161 | 162 | // limit integral 163 | if (antiwindup_) 164 | { 165 | if (u > duty_limiter_ && error_integ_ > 0) 166 | { 167 | error_integ_ = std::max(0.0, (duty_limiter_ - u_p - u_d) / ki_); 168 | } 169 | else if (u < -duty_limiter_ && error_integ_ < 0) 170 | { 171 | error_integ_ = std::min(0.0, (-duty_limiter_ - u_p - u_d) / ki_); 172 | } 173 | } 174 | 175 | // limit duty value 176 | u = std::clamp(u, -duty_limiter_, duty_limiter_); 177 | 178 | interface_ptr_->setDutyCycle(u); 179 | 180 | pid_initialize_ = std::fabs(target_velocity_) < 0.0001; // disable PID control when command is 0 181 | } 182 | 183 | void VescWheelController::setTargetVelocity(const double velocity) 184 | { 185 | target_velocity_ = velocity; 186 | } 187 | 188 | void VescWheelController::setGearRatio(const double gear_ratio) 189 | { 190 | gear_ratio_ = gear_ratio; 191 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[VescWheelController]Gear ratio is set to %f", gear_ratio_); 192 | } 193 | 194 | void VescWheelController::setTorqueConst(const double torque_const) 195 | { 196 | torque_const_ = torque_const; 197 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[VescWheelController]Torque constant is set to %f", 198 | torque_const_); 199 | } 200 | 201 | void VescWheelController::setRotorPoles(const int rotor_poles) 202 | { 203 | num_rotor_poles_ = static_cast(rotor_poles); 204 | num_rotor_pole_pairs_ = num_rotor_poles_ / 2; 205 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[VescWheelController]The number of rotor poles is set to %d", 206 | rotor_poles); 207 | } 208 | 209 | void VescWheelController::setHallSensors(const int hall_sensors) 210 | { 211 | num_hall_sensors_ = static_cast(hall_sensors); 212 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[VescWheelController]The number of hall sensors is set to %d", 213 | hall_sensors); 214 | } 215 | 216 | double VescWheelController::getPositionSens() 217 | { 218 | return position_sens_; 219 | } 220 | 221 | double VescWheelController::getVelocitySens() 222 | { 223 | return velocity_sens_; 224 | } 225 | 226 | double VescWheelController::getEffortSens() 227 | { 228 | return effort_sens_; 229 | } 230 | 231 | // void VescWheelController::controlTimerCallback(const ros::TimerEvent& e) 232 | // { 233 | // control(); 234 | // interface_ptr_->requestState(); 235 | // pid_initialize_ = std::fabs(target_velocity_) < 0.0001; // disable PID control when command is 0 236 | // } 237 | 238 | void VescWheelController::updateSensor(const std::shared_ptr& packet) 239 | { 240 | if (packet->getName() == "Values") 241 | { 242 | std::shared_ptr values = std::dynamic_pointer_cast(packet); 243 | const double current = values->getMotorCurrent(); 244 | const double velocity_rpm = values->getVelocityERPM() / static_cast(num_rotor_pole_pairs_) * gear_ratio_; 245 | const int32_t steps = static_cast(values->getTachometer()); 246 | if (sensor_initialize_) 247 | { 248 | prev_steps_ = steps; 249 | sensor_initialize_ = false; 250 | } 251 | position_steps_ += static_cast(steps - prev_steps_); 252 | prev_steps_ = steps; 253 | 254 | position_sens_ = 255 | (position_steps_ * 2.0 * M_PI) / (num_rotor_poles_ * num_hall_sensors_) * gear_ratio_; // convert steps to rad 256 | velocity_sens_ = velocity_rpm * 2 * M_PI / 60; // convert rpm to rad/s 257 | effort_sens_ = current * torque_const_ / gear_ratio_; // unit: Nm or N 258 | } 259 | } 260 | } // namespace vesc_hw_interface 261 | -------------------------------------------------------------------------------- /vesc_driver/src/vesc_interface.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of Softbank Corp. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | ********************************************************************/ 29 | 30 | /** NOTE ************************************************************* 31 | * This program had been developed by Michael T. Boulet at MIT under 32 | * the BSD 3-clause License until Dec. 2016. Since Nov. 2019, Softbank 33 | * Corp. takes over development as new packages. 34 | ********************************************************************/ 35 | 36 | #include "vesc_driver/vesc_interface.hpp" 37 | #include 38 | #include 39 | 40 | namespace vesc_driver 41 | { 42 | class VescInterface::Impl 43 | { 44 | public: 45 | Impl() : owned_ctx(new IoContext(2)), serial_driver_(new drivers::serial_driver::SerialDriver(*owned_ctx)) 46 | { 47 | data_updated_ = false; 48 | } 49 | 50 | void* rxThread(void); 51 | 52 | static void* rxThreadHelper(void* context) 53 | { 54 | return ((VescInterface::Impl*)context)->rxThread(); 55 | } 56 | 57 | pthread_t rx_thread_; 58 | bool rx_thread_run_; 59 | PacketHandlerFunction packet_handler_; 60 | ErrorHandlerFunction error_handler_; 61 | std::unique_ptr owned_ctx{}; 62 | std::unique_ptr device_config_; 63 | VescFrame::CRC send_crc_; 64 | bool data_updated_; 65 | std::unique_ptr serial_driver_; 66 | }; 67 | 68 | void* VescInterface::Impl::rxThread(void) 69 | { 70 | Buffer buffer; 71 | buffer.reserve(4096); 72 | auto temp_buffer = Buffer(4096); 73 | 74 | while (rx_thread_run_) 75 | { 76 | int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; 77 | // attempt to read at least bytes_needed bytes from the serial port 78 | const auto bytes_read = serial_driver_->port()->receive(temp_buffer); 79 | buffer.reserve(buffer.size() + bytes_read); 80 | buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.begin() + bytes_read); 81 | // RCLCPP_INFO(rclcpp::get_logger("VescDriver"), "Read packets: %d", bytes_read); 82 | if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) 83 | { 84 | error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); 85 | } 86 | if (!buffer.empty()) 87 | { 88 | // search buffer for valid packet(s) 89 | Buffer::iterator iter(buffer.begin()); 90 | Buffer::iterator iter_begin(buffer.begin()); 91 | while (iter != buffer.end()) 92 | { 93 | // check if valid start-of-frame character 94 | if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) 95 | { 96 | // good start, now attempt to create packet 97 | std::string error; 98 | VescPacketConstPtr packet = VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); 99 | if (packet) 100 | { 101 | // Packet received; 102 | data_updated_ = true; 103 | // good packet, check if we skipped any data 104 | if (std::distance(iter_begin, iter) > 0) 105 | { 106 | std::ostringstream ss; 107 | ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " 108 | << std::distance(iter_begin, iter) << " bytes."; 109 | error_handler_(ss.str()); 110 | } 111 | // call packet handler 112 | packet_handler_(packet); 113 | // update state 114 | iter = iter + packet->getFrame().size(); 115 | iter_begin = iter; 116 | // continue to look for another frame in buffer 117 | continue; 118 | } 119 | else if (bytes_needed > 0) 120 | { 121 | // need more data, break out of while loop 122 | break; // for (iter_sof... 123 | } 124 | else 125 | { 126 | // else, this was not a packet, move on to next byte 127 | error_handler_(error); 128 | } 129 | } 130 | 131 | iter++; 132 | } 133 | 134 | // if iter is at the end of the buffer, more bytes are needed 135 | if (iter == buffer.end()) 136 | bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; 137 | 138 | // erase "used" buffer 139 | if (std::distance(iter_begin, iter) > 0) 140 | { 141 | std::ostringstream ss; 142 | ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; 143 | error_handler_(ss.str()); 144 | } 145 | buffer.erase(buffer.begin(), iter); 146 | } 147 | } 148 | } 149 | 150 | VescInterface::VescInterface(const std::string& port, const PacketHandlerFunction& packet_handler, 151 | const ErrorHandlerFunction& error_handler) 152 | : impl_(new Impl()) 153 | { 154 | setPacketHandler(packet_handler); 155 | setErrorHandler(error_handler); 156 | // attempt to conect if the port is specified 157 | if (!port.empty()) 158 | connect(port); 159 | } 160 | 161 | VescInterface::~VescInterface() 162 | { 163 | // stops the motor 164 | setDutyCycle(0.0); 165 | 166 | disconnect(); 167 | } 168 | 169 | void VescInterface::setPacketHandler(const PacketHandlerFunction& handler) 170 | { 171 | // todo - definately need mutex 172 | impl_->packet_handler_ = handler; 173 | } 174 | 175 | void VescInterface::setErrorHandler(const ErrorHandlerFunction& handler) 176 | { 177 | // todo - definately need mutex 178 | impl_->error_handler_ = handler; 179 | } 180 | 181 | void VescInterface::connect(const std::string& port) 182 | { 183 | // todo - mutex? 184 | 185 | if (isConnected()) 186 | { 187 | throw SerialException("Already connected to serial port."); 188 | } 189 | 190 | // connect to serial port 191 | try 192 | { 193 | const uint32_t baud_rate = 115200; 194 | const auto fc = drivers::serial_driver::FlowControl::NONE; 195 | const auto pt = drivers::serial_driver::Parity::NONE; 196 | const auto sb = drivers::serial_driver::StopBits::ONE; 197 | impl_->device_config_ = std::make_unique(baud_rate, fc, pt, sb); 198 | impl_->serial_driver_->init_port(port, *impl_->device_config_); 199 | if (!impl_->serial_driver_->port()->is_open()) 200 | { 201 | impl_->serial_driver_->port()->open(); 202 | } 203 | } 204 | catch (const std::exception& e) 205 | { 206 | std::stringstream ss; 207 | ss << "Failed to open the serial port to the VESC. " << e.what(); 208 | throw SerialException(ss.str().c_str()); 209 | } 210 | 211 | // start up a monitoring thread 212 | impl_->rx_thread_run_ = true; 213 | int result = pthread_create(&impl_->rx_thread_, NULL, &VescInterface::Impl::rxThreadHelper, impl_.get()); 214 | assert(0 == result); 215 | } 216 | 217 | void VescInterface::disconnect() 218 | { 219 | // todo - mutex? 220 | 221 | if (isConnected()) 222 | { 223 | // bring down read thread 224 | impl_->rx_thread_run_ = false; 225 | int result = pthread_join(impl_->rx_thread_, NULL); 226 | assert(0 == result); 227 | 228 | impl_->serial_driver_->port()->close(); 229 | } 230 | } 231 | 232 | bool VescInterface::isConnected() const 233 | { 234 | auto port = impl_->serial_driver_->port(); 235 | 236 | if (port) 237 | { 238 | return port->is_open(); 239 | } 240 | else 241 | { 242 | return false; 243 | } 244 | } 245 | 246 | bool VescInterface::isRxDataUpdated() const 247 | { 248 | bool output = impl_->data_updated_; 249 | impl_->data_updated_ = false; 250 | return output; 251 | } 252 | 253 | void VescInterface::send(const VescPacket& packet) 254 | { 255 | std::size_t written = impl_->serial_driver_->port()->send(packet.getFrame()); 256 | if (written != packet.getFrame().size()) 257 | { 258 | std::stringstream ss; 259 | ss << "Wrote " << written << " bytes, expected " << packet.getFrame().size() << "."; 260 | throw SerialException(ss.str().c_str()); 261 | } 262 | } 263 | 264 | void VescInterface::requestFWVersion() 265 | { 266 | send(VescPacketRequestFWVersion()); 267 | } 268 | 269 | void VescInterface::requestState() 270 | { 271 | send(VescPacketRequestValues()); 272 | } 273 | 274 | void VescInterface::requestMCConfiguration() 275 | { 276 | send(VescPacketRequestMCConf()); 277 | } 278 | 279 | void VescInterface::setDutyCycle(double duty_cycle) 280 | { 281 | send(VescPacketSetDuty(duty_cycle)); 282 | } 283 | 284 | void VescInterface::setCurrent(double current) 285 | { 286 | send(VescPacketSetCurrent(current)); 287 | } 288 | 289 | void VescInterface::setBrake(double brake) 290 | { 291 | send(VescPacketSetCurrentBrake(brake)); 292 | } 293 | 294 | void VescInterface::setSpeed(double speed) 295 | { 296 | send(VescPacketSetVelocityERPM(speed)); 297 | } 298 | 299 | void VescInterface::setPosition(double position) 300 | { 301 | send(VescPacketSetPos(position)); 302 | } 303 | 304 | void VescInterface::setServo(double servo) 305 | { 306 | send(VescPacketSetServoPos(servo)); 307 | } 308 | 309 | } // namespace vesc_driver 310 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright 2019 SoftBanck Corp. 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /vesc_driver/src/vesc_driver.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of Softbank Corp. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | ********************************************************************/ 29 | 30 | /** NOTE ************************************************************* 31 | * This program had been developed by Michael T. Boulet at MIT under 32 | * the BSD 3-clause License until Dec. 2016. Since Nov. 2019, Softbank 33 | * Corp. takes over development as new packages. 34 | ********************************************************************/ 35 | 36 | // TODO: Migrate to ROS2 37 | #include "vesc_driver/vesc_driver.h" 38 | 39 | namespace vesc_driver 40 | { 41 | VescDriver::VescDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) 42 | : vesc_(std::string(), std::bind(&VescDriver::vescPacketCallback, this, std::placeholders::_1), 43 | std::bind(&VescDriver::vescErrorCallback, this, std::placeholders::_1)) 44 | , duty_cycle_limit_(private_nh, "duty_cycle", -1.0, 1.0) 45 | , current_limit_(private_nh, "current") 46 | , brake_limit_(private_nh, "brake") 47 | , speed_limit_(private_nh, "speed") 48 | , position_limit_(private_nh, "position") 49 | , servo_limit_(private_nh, "servo", 0.0, 1.0) 50 | , driver_mode_(MODE_INITIALIZING) 51 | , fw_version_major_(-1) 52 | , fw_version_minor_(-1) 53 | { 54 | // get vesc serial port address 55 | std::string port; 56 | if (!private_nh.getParam("port", port)) 57 | { 58 | ROS_FATAL("VESC communication port parameter required."); 59 | ros::shutdown(); 60 | return; 61 | } 62 | 63 | // attempt to connect to the serial port 64 | try 65 | { 66 | vesc_.connect(port); 67 | } 68 | catch (SerialException e) 69 | { 70 | ROS_FATAL("Failed to connect to the VESC, %s.", e.what()); 71 | ros::shutdown(); 72 | return; 73 | } 74 | 75 | // get the number of motor pole pairs 76 | private_nh.param("num_motor_pole_pairs", num_motor_pole_pairs_, 1); 77 | ROS_INFO("The number of motor pole pairs is set to %d", num_motor_pole_pairs_); 78 | 79 | // create vesc state (telemetry) publisher 80 | state_pub_ = nh.advertise("sensors/core", 10); 81 | 82 | // since vesc state does not include the servo position, publish the commanded 83 | // servo position as a "sensor" 84 | servo_sensor_pub_ = nh.advertise("sensors/servo_position_command", 10); 85 | 86 | // subscribe to motor and servo command topics 87 | duty_cycle_sub_ = nh.subscribe("commands/motor/duty_cycle", 10, &VescDriver::dutyCycleCallback, this); 88 | current_sub_ = nh.subscribe("commands/motor/current", 10, &VescDriver::currentCallback, this); 89 | brake_sub_ = nh.subscribe("commands/motor/brake", 10, &VescDriver::brakeCallback, this); 90 | speed_sub_ = nh.subscribe("commands/motor/speed", 10, &VescDriver::speedCallback, this); 91 | position_sub_ = nh.subscribe("commands/motor/position", 10, &VescDriver::positionCallback, this); 92 | servo_sub_ = nh.subscribe("commands/servo/position", 10, &VescDriver::servoCallback, this); 93 | 94 | // create a 50Hz timer, used for state machine & polling VESC telemetry 95 | timer_ = nh.createTimer(ros::Duration(1.0 / 50.0), &VescDriver::timerCallback, this); 96 | } 97 | 98 | /* TODO or TO-THINKABOUT LIST 99 | - what should we do on startup? send brake or zero command? 100 | - what to do if the vesc interface gives an error? 101 | - check version number against know compatable? 102 | - should we wait until we receive telemetry before sending commands? 103 | - should we track the last motor command 104 | - what to do if no motor command received recently? 105 | - what to do if no servo command received recently? 106 | - what is the motor safe off state (0 current?) 107 | - what to do if a command parameter is out of range, ignore? 108 | - try to predict vesc bounds (from vesc config) and command detect bounds errors 109 | */ 110 | 111 | void VescDriver::timerCallback(const ros::TimerEvent& event) 112 | { 113 | // VESC interface should not unexpectedly disconnect, but test for it anyway 114 | if (!vesc_.isConnected()) 115 | { 116 | ROS_FATAL("Unexpectedly disconnected from serial port."); 117 | timer_.stop(); 118 | ros::shutdown(); 119 | return; 120 | } 121 | 122 | /* 123 | * Driver state machine, modes: 124 | * INITIALIZING - request and wait for vesc version 125 | * OPERATING - receiving commands from subscriber topics 126 | */ 127 | if (driver_mode_ == MODE_INITIALIZING) 128 | { 129 | // request version number, return packet will update the internal version numbers 130 | vesc_.requestFWVersion(); 131 | if (fw_version_major_ >= 0 && fw_version_minor_ >= 0) 132 | { 133 | ROS_INFO("Connected to VESC with firmware version %d.%d", fw_version_major_, fw_version_minor_); 134 | driver_mode_ = MODE_OPERATING; 135 | } 136 | } 137 | else if (driver_mode_ == MODE_OPERATING) 138 | { 139 | // poll for vesc state (telemetry) 140 | vesc_.requestState(); 141 | } 142 | else 143 | { 144 | // unknown mode, how did that happen? 145 | assert(false && "unknown driver mode"); 146 | } 147 | } 148 | 149 | void VescDriver::vescPacketCallback(const std::shared_ptr& packet) 150 | { 151 | if (packet->getName() == "Values") 152 | { 153 | std::shared_ptr values = std::dynamic_pointer_cast(packet); 154 | 155 | vesc_msgs::VescStateStamped::Ptr state_msg(new vesc_msgs::VescStateStamped); 156 | state_msg->header.stamp = ros::Time::now(); 157 | state_msg->state.voltage_input = values->getInputVoltage(); 158 | state_msg->state.temperature_pcb = values->getMosTemp(); 159 | state_msg->state.current_motor = values->getMotorCurrent(); 160 | state_msg->state.current_input = values->getInputCurrent(); 161 | state_msg->state.speed = values->getVelocityERPM() / static_cast(num_motor_pole_pairs_) / 60.0 * 2.0 * M_PI; 162 | state_msg->state.duty_cycle = values->getDuty(); 163 | state_msg->state.charge_drawn = values->getConsumedCharge(); 164 | state_msg->state.charge_regen = values->getInputCharge(); 165 | state_msg->state.energy_drawn = values->getConsumedPower(); 166 | state_msg->state.energy_regen = values->getInputPower(); 167 | state_msg->state.displacement = values->getPosition(); 168 | state_msg->state.distance_traveled = values->getDisplacement(); 169 | state_msg->state.fault_code = values->getFaultCode(); 170 | 171 | state_pub_.publish(state_msg); 172 | } 173 | else if (packet->getName() == "FWVersion") 174 | { 175 | std::shared_ptr fw_version = 176 | std::dynamic_pointer_cast(packet); 177 | // todo: might need lock here 178 | fw_version_major_ = fw_version->fwMajor(); 179 | fw_version_minor_ = fw_version->fwMinor(); 180 | } 181 | } 182 | 183 | void VescDriver::vescErrorCallback(const std::string& error) 184 | { 185 | ROS_ERROR("%s", error.c_str()); 186 | } 187 | 188 | /** 189 | * @param duty_cycle Commanded VESC duty cycle. Valid range for this driver is -1 to +1. However, 190 | * note that the VESC may impose a more restrictive bounds on the range depending 191 | * on its configuration, e.g. absolute value is between 0.05 and 0.95. 192 | */ 193 | void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle) 194 | { 195 | if (driver_mode_ = MODE_OPERATING) 196 | { 197 | vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data)); 198 | } 199 | } 200 | 201 | /** 202 | * @param current Commanded VESC current in Amps. Any value is accepted by this driver. However, 203 | * note that the VESC may impose a more restrictive bounds on the range depending on 204 | * its configuration. 205 | */ 206 | void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) 207 | { 208 | if (driver_mode_ = MODE_OPERATING) 209 | { 210 | vesc_.setCurrent(current_limit_.clip(current->data)); 211 | } 212 | } 213 | 214 | /** 215 | * @param brake Commanded VESC braking current in Amps. Any value is accepted by this driver. 216 | * However, note that the VESC may impose a more restrictive bounds on the range 217 | * depending on its configuration. 218 | */ 219 | void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) 220 | { 221 | if (driver_mode_ = MODE_OPERATING) 222 | { 223 | vesc_.setBrake(brake_limit_.clip(brake->data)); 224 | } 225 | } 226 | 227 | /** 228 | * @param speed Commanded VESC speed in rad/s. Although any value is accepted by this driver, the VESC may impose a 229 | * more restrictive bounds on the range depending on its configuration. 230 | */ 231 | void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) 232 | { 233 | if (driver_mode_ = MODE_OPERATING) 234 | { 235 | const double speed_erpm = speed->data / 2.0 / M_PI * 60.0 * static_cast(num_motor_pole_pairs_); 236 | vesc_.setSpeed(speed_limit_.clip(speed_erpm)); 237 | } 238 | } 239 | 240 | /** 241 | * @param position Commanded VESC motor position in radians. Any value is accepted by this driver. 242 | * Note that the VESC must be in encoder mode for this command to have an effect. 243 | */ 244 | void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) 245 | { 246 | if (driver_mode_ = MODE_OPERATING) 247 | { 248 | // ROS uses radians but VESC seems to use degrees. Convert to degrees. 249 | double position_deg = position_limit_.clip(position->data) * 180.0 / M_PI; 250 | vesc_.setPosition(position_deg); 251 | } 252 | } 253 | 254 | /** 255 | * @param servo Commanded VESC servo output position. Valid range is 0 to 1. 256 | */ 257 | void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo) 258 | { 259 | if (driver_mode_ = MODE_OPERATING) 260 | { 261 | double servo_clipped(servo_limit_.clip(servo->data)); 262 | vesc_.setServo(servo_clipped); 263 | // publish clipped servo value as a "sensor" 264 | std_msgs::Float64::Ptr servo_sensor_msg(new std_msgs::Float64); 265 | servo_sensor_msg->data = servo_clipped; 266 | servo_sensor_pub_.publish(servo_sensor_msg); 267 | } 268 | } 269 | 270 | VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::string& str, 271 | const boost::optional& min_lower, 272 | const boost::optional& max_upper) 273 | : name(str) 274 | { 275 | // check if user's minimum value is outside of the range min_lower to max_upper 276 | double param_min; 277 | if (nh.getParam(name + "_min", param_min)) 278 | { 279 | if (min_lower && param_min < *min_lower) 280 | { 281 | lower = *min_lower; 282 | ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << ") is less than the feasible minimum (" 283 | << *min_lower << ")."); 284 | } 285 | else if (max_upper && param_min > *max_upper) 286 | { 287 | lower = *max_upper; 288 | ROS_WARN_STREAM("Parameter " << name << "_min (" << param_min << ") is greater than the feasible maximum (" 289 | << *max_upper << ")."); 290 | } 291 | else 292 | { 293 | lower = param_min; 294 | } 295 | } 296 | else if (min_lower) 297 | { 298 | lower = *min_lower; 299 | } 300 | 301 | // check if the uers' maximum value is outside of the range min_lower to max_upper 302 | double param_max; 303 | if (nh.getParam(name + "_max", param_max)) 304 | { 305 | if (min_lower && param_max < *min_lower) 306 | { 307 | upper = *min_lower; 308 | ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << ") is less than the feasible minimum (" 309 | << *min_lower << ")."); 310 | } 311 | else if (max_upper && param_max > *max_upper) 312 | { 313 | upper = *max_upper; 314 | ROS_WARN_STREAM("Parameter " << name << "_max (" << param_max << ") is greater than the feasible maximum (" 315 | << *max_upper << ")."); 316 | } 317 | else 318 | { 319 | upper = param_max; 320 | } 321 | } 322 | else if (max_upper) 323 | { 324 | upper = *max_upper; 325 | } 326 | 327 | // check for min > max 328 | if (upper && lower && *lower > *upper) 329 | { 330 | ROS_WARN_STREAM("Parameter " << name << "_max (" << *upper << ") is less than parameter " << name << "_min (" 331 | << *lower << ")."); 332 | double temp(*lower); 333 | lower = *upper; 334 | upper = temp; 335 | } 336 | 337 | std::ostringstream oss; 338 | oss << " " << name << " limit: "; 339 | if (lower) 340 | oss << *lower << " "; 341 | else 342 | oss << "(none) "; 343 | if (upper) 344 | oss << *upper; 345 | else 346 | oss << "(none)"; 347 | ROS_DEBUG_STREAM(oss.str()); 348 | } 349 | 350 | double VescDriver::CommandLimit::clip(double value) 351 | { 352 | if (lower && value < lower) 353 | { 354 | ROS_INFO_THROTTLE(10, "%s command value (%f) below minimum limit (%f), clipping.", name.c_str(), value, *lower); 355 | return *lower; 356 | } 357 | if (upper && value > upper) 358 | { 359 | ROS_INFO_THROTTLE(10, "%s command value (%f) above maximum limit (%f), clipping.", name.c_str(), value, *upper); 360 | return *upper; 361 | } 362 | return value; 363 | } 364 | 365 | } // namespace vesc_driver 366 | -------------------------------------------------------------------------------- /vesc_driver/include/vesc_driver/data_map.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank corp. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | ********************************************************************/ 16 | 17 | #ifndef VESC_DRIVER_DATA_MAP_HPP_ 18 | #define VESC_DRIVER_DATA_MAP_HPP_ 19 | #include 20 | 21 | /** 22 | * @brief ID of communication commands 23 | **/ 24 | enum COMM_PACKET_ID 25 | { 26 | COMM_FW_VERSION = 0, 27 | COMM_JUMP_TO_BOOTLOADER, 28 | COMM_ERASE_NEW_APP, 29 | COMM_WRITE_NEW_APP_DATA, 30 | COMM_GET_VALUES, 31 | COMM_SET_DUTY, 32 | COMM_SET_CURRENT, 33 | COMM_SET_CURRENT_BRAKE, 34 | COMM_SET_ERPM, 35 | COMM_SET_POS, 36 | COMM_SET_HANDBRAKE, 37 | COMM_SET_DETECT, 38 | COMM_SET_SERVO_POS, 39 | COMM_SET_MCCONF, 40 | COMM_GET_MCCONF, 41 | COMM_GET_MCCONF_DEFAULT, 42 | COMM_SET_APPCONF, 43 | COMM_GET_APPCONF, 44 | COMM_GET_APPCONF_DEFAULT, 45 | COMM_SAMPLE_PRINT, 46 | COMM_TERMINAL_CMD, 47 | COMM_PRINT, 48 | COMM_ROTOR_POSITION, 49 | COMM_EXPERIMENT_SAMPLE, 50 | COMM_DETECT_MOTOR_PARAM, 51 | COMM_DETECT_MOTOR_R_L, 52 | COMM_DETECT_MOTOR_FLUX_LINKAGE, 53 | COMM_DETECT_ENCODER, 54 | COMM_DETECT_HALL_FOC, 55 | COMM_REBOOT, 56 | COMM_ALIVE, 57 | COMM_GET_DECODED_PPM, 58 | COMM_GET_DECODED_ADC, 59 | COMM_GET_DECODED_CHUK, 60 | COMM_FORWARD_CAN, 61 | COMM_SET_CHUCK_DATA, 62 | COMM_CUSTOM_APP_DATA, 63 | COMM_NRF_START_PAIRING, 64 | COMM_GPD_SET_FSW, 65 | COMM_GPD_BUFFER_NOTIFY, 66 | COMM_GPD_BUFFER_SIZE_LEFT, 67 | COMM_GPD_FILL_BUFFER, 68 | COMM_GPD_OUTPUT_SAMPLE, 69 | COMM_GPD_SET_MODE, 70 | COMM_GPD_FILL_BUFFER_INT8, 71 | COMM_GPD_FILL_BUFFER_INT16, 72 | COMM_GPD_SET_BUFFER_INT_SCALE, 73 | COMM_GET_VALUES_SETUP, 74 | COMM_SET_MCCONF_TEMP, 75 | COMM_SET_MCCONF_TEMP_SETUP, 76 | COMM_GET_VALUES_SELECTIVE, 77 | COMM_GET_VALUES_SETUP_SELECTIVE, 78 | COMM_EXT_NRF_PRESENT, 79 | COMM_EXT_NRF_ESB_SET_CH_ADDR, 80 | COMM_EXT_NRF_ESB_SEND_DATA, 81 | COMM_EXT_NRF_ESB_RX_DATA, 82 | COMM_EXT_NRF_SET_ENABLED, 83 | COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP, 84 | COMM_DETECT_APPLY_ALL_FOC, 85 | COMM_JUMP_TO_BOOTLOADER_ALL_CAN, 86 | COMM_ERASE_NEW_APP_ALL_CAN, 87 | COMM_WRITE_NEW_APP_DATA_ALL_CAN, 88 | COMM_PING_CAN, 89 | COMM_APP_DISABLE_OUTPUT, 90 | COMM_TERMINAL_CMD_SYNC, 91 | COMM_GET_IMU_DATA, 92 | COMM_BM_CONNECT, 93 | COMM_BM_ERASE_FLASH_ALL, 94 | COMM_BM_WRITE_FLASH, 95 | COMM_BM_REBOOT, 96 | COMM_BM_DISCONNECT, 97 | COMM_BM_MAP_PINS_DEFAULT, 98 | COMM_BM_MAP_PINS_NRF5X, 99 | COMM_ERASE_BOOTLOADER, 100 | COMM_ERASE_BOOTLOADER_ALL_CAN, 101 | COMM_PLOT_INIT, 102 | COMM_PLOT_DATA, 103 | COMM_PLOT_ADD_GRAPH, 104 | COMM_PLOT_SET_GRAPH, 105 | COMM_GET_DECODED_BALANCE, 106 | COMM_BM_MEM_READ, 107 | COMM_WRITE_NEW_APP_DATA_LZO, 108 | COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO, 109 | COMM_BM_WRITE_FLASH_LZO, 110 | COMM_SET_CURRENT_REL, 111 | COMM_CAN_FWD_FRAME, 112 | COMM_SET_BATTERY_CUT, 113 | COMM_SET_BLE_NAME, 114 | COMM_SET_BLE_PIN, 115 | COMM_SET_CAN_MODE, 116 | COMM_GET_IMU_CALIBRATION, 117 | COMM_GET_MCCONF_TEMP, 118 | 119 | // Custom configuration for hardware 120 | COMM_GET_CUSTOM_CONFIG_XML, 121 | COMM_GET_CUSTOM_CONFIG, 122 | COMM_GET_CUSTOM_CONFIG_DEFAULT, 123 | COMM_SET_CUSTOM_CONFIG, 124 | 125 | // BMS commands 126 | COMM_BMS_GET_VALUES, 127 | COMM_BMS_SET_CHARGE_ALLOWED, 128 | COMM_BMS_SET_BALANCE_OVERRIDE, 129 | COMM_BMS_RESET_COUNTERS, 130 | COMM_BMS_FORCE_BALANCE, 131 | COMM_BMS_ZERO_CURRENT_OFFSET, 132 | 133 | // FW updates commands for different HW types 134 | COMM_JUMP_TO_BOOTLOADER_HW, 135 | COMM_ERASE_NEW_APP_HW, 136 | COMM_WRITE_NEW_APP_DATA_HW, 137 | COMM_ERASE_BOOTLOADER_HW, 138 | COMM_JUMP_TO_BOOTLOADER_ALL_CAN_HW, 139 | COMM_ERASE_NEW_APP_ALL_CAN_HW, 140 | COMM_WRITE_NEW_APP_DATA_ALL_CAN_HW, 141 | COMM_ERASE_BOOTLOADER_ALL_CAN_HW, 142 | 143 | COMM_SET_ODOMETER, 144 | 145 | // Power switch commands 146 | COMM_PSW_GET_STATUS, 147 | COMM_PSW_SWITCH, 148 | 149 | COMM_BMS_FWD_CAN_RX, 150 | COMM_BMS_HW_DATA, 151 | COMM_GET_BATTERY_CUT, 152 | COMM_BM_HALT_REQ, 153 | COMM_GET_QML_UI_HW, 154 | COMM_GET_QML_UI_APP, 155 | COMM_CUSTOM_HW_DATA, 156 | COMM_QMLUI_ERASE, 157 | COMM_QMLUI_WRITE, 158 | 159 | // IO Board 160 | COMM_IO_BOARD_GET_ALL, 161 | COMM_IO_BOARD_SET_PWM, 162 | COMM_IO_BOARD_SET_DIGITAL, 163 | 164 | COMM_BM_MEM_WRITE, 165 | COMM_BMS_BLNC_SELFTEST, 166 | COMM_GET_EXT_HUM_TMP, 167 | COMM_GET_STATS, 168 | COMM_RESET_STATS, 169 | 170 | // Lisp 171 | COMM_LISP_READ_CODE, 172 | COMM_LISP_WRITE_CODE, 173 | COMM_LISP_ERASE_CODE, 174 | COMM_LISP_SET_RUNNING, 175 | COMM_LISP_GET_STATS, 176 | COMM_LISP_PRINT, 177 | 178 | COMM_BMS_SET_BATT_TYPE, 179 | COMM_BMS_GET_BATT_TYPE, 180 | 181 | COMM_LISP_REPL_CMD, 182 | COMM_LISP_STREAM_CODE, 183 | 184 | COMM_FILE_LIST, 185 | COMM_FILE_READ, 186 | COMM_FILE_WRITE, 187 | COMM_FILE_MKDIR, 188 | COMM_FILE_REMOVE, 189 | 190 | COMM_LOG_START, 191 | COMM_LOG_STOP, 192 | COMM_LOG_CONFIG_FIELD, 193 | COMM_LOG_DATA_F32, 194 | 195 | COMM_SET_APPCONF_NO_STORE, 196 | COMM_GET_GNSS, 197 | 198 | COMM_LOG_DATA_F64, 199 | }; 200 | 201 | /** 202 | * @brief Map of return packets of COMM_GET_VALUES 203 | **/ 204 | enum PACKET_MAP 205 | { 206 | TEMP_MOS = 1, 207 | TEMP_MOTOR = 3, 208 | CURRENT_MOTOR = 5, 209 | CURRENT_IN = 9, 210 | ID = 13, 211 | IQ = 17, 212 | DUTY_NOW = 21, 213 | ERPM = 23, 214 | VOLTAGE_IN = 27, 215 | AMP_HOURS = 29, 216 | AMP_HOURS_CHARGED = 33, 217 | WATT_HOURS = 37, 218 | WATT_HOURS_CHARGED = 41, 219 | TACHOMETER = 45, 220 | TACHOMETER_ABS = 49, 221 | FAULT_CODE = 53, 222 | PID_POS = 54, 223 | CONTROLLER_ID = 58, 224 | TEMP_MOS_MOTOR2 = 59, 225 | VD = 65, 226 | VQ = 69, 227 | }; 228 | 229 | /** 230 | * @brief Modes and types 231 | **/ 232 | enum PWM_MODE 233 | { 234 | PWM_MODE_NONSYNCHRONOUS_HISW = 0, 235 | PWM_MODE_SYNCHRONOUS, 236 | PWM_MODE_BIPOLAR, 237 | }; 238 | 239 | enum COMM_MODE 240 | { 241 | COMM_MODE_INTEGRATE = 0, 242 | COMM_MODE_DELAY, 243 | }; 244 | 245 | enum SENSOR_MODE 246 | { 247 | SENSOR_MODE_SENSORLESS = 0, 248 | SENSOR_MODE_SENSORED, 249 | SENSOR_MODE_HYBRID, 250 | }; 251 | 252 | enum FOC_SENSOR_MODE 253 | { 254 | FOC_SENSOR_MODE_SENSORLESS = 0, 255 | FOC_SENSOR_MODE_ENCODER, 256 | FOC_SENSOR_MODE_HALL, 257 | FOC_SENSOR_MODE_HFI, 258 | FOC_SENSOR_MODE_HFI_START, 259 | }; 260 | 261 | enum SENSOR_PORT_MODE 262 | { 263 | SENSOR_PORT_MODE_HALL = 0, 264 | SENSOR_PORT_MODE_ABI, 265 | SENSOR_PORT_MODE_AS5047_SPI, 266 | SENSOR_PORT_MODE_AD2S1205, 267 | SENSOR_PORT_MODE_SINCOS, 268 | SENSOR_PORT_MODE_TS5700N8501, 269 | SENSOR_PORT_MODE_TS5700N8501_MULTITURN, 270 | SENSOR_PORT_MODE_MT6816_SPI 271 | }; 272 | 273 | enum MOTOR_TYPE 274 | { 275 | MOTOR_TYPE_BLD = 0, 276 | MOTOR_TYPE_DC, 277 | MOTOR_TYPE_FOC, 278 | MOTOR_TYPE_GPD, 279 | }; 280 | 281 | enum FOC_CC_DECOUPLING_MODE 282 | { 283 | FOC_CC_DECOUPLING_DISABLED = 0, 284 | FOC_CC_DECOUPLING_CROSS, 285 | FOC_CC_DECOUPLING_BEMF, 286 | FOC_CC_DECOUPLING_CROSS_BEMF 287 | }; 288 | 289 | enum FOC_OBSERVER_TYPE 290 | { 291 | FOC_OBSERVER_ORTEGA_ORIGINAL = 0, 292 | }; 293 | 294 | enum FOC_HFI_SAMPLES 295 | { 296 | HFI_SAMPLES_8 = 0, 297 | HFI_SAMPLES_16, 298 | HFI_SAMPLES_32 299 | }; 300 | 301 | enum MTPA_MODE 302 | { 303 | MTPA_MODE_OFF = 0, 304 | MTPA_MODE_IQ_TARGET, 305 | MTPA_MODE_IQ_MEASURED 306 | }; 307 | 308 | enum PID_RATE 309 | { 310 | PID_RATE_25_HZ = 0, 311 | PID_RATE_50_HZ, 312 | PID_RATE_100_HZ, 313 | PID_RATE_250_HZ, 314 | PID_RATE_500_HZ, 315 | PID_RATE_1000_HZ, 316 | PID_RATE_2500_HZ, 317 | PID_RATE_5000_HZ, 318 | PID_RATE_10000_HZ, 319 | }; 320 | 321 | enum DRV8301_OC_MODE 322 | { 323 | DRV8301_OC_LIMIT = 0, 324 | DRV8301_OC_LATCH_SHUTDOWN, 325 | DRV8301_OC_REPORT_ONLY, 326 | DRV8301_OC_DISABLED 327 | }; 328 | 329 | enum OUT_AUX_MODE 330 | { 331 | OUT_AUX_MODE_OFF = 0, 332 | OUT_AUX_MODE_ON_AFTER_2S, 333 | OUT_AUX_MODE_ON_AFTER_5S, 334 | OUT_AUX_MODE_ON_AFTER_10S, 335 | OUT_AUX_MODE_UNUSED, 336 | OUT_AUX_MODE_ON_WHEN_RUNNING, 337 | OUT_AUX_MODE_ON_WHEN_NOT_RUNNING, 338 | OUT_AUX_MODE_MOTOR_50, 339 | OUT_AUX_MODE_MOSFET_50, 340 | OUT_AUX_MODE_MOTOR_70, 341 | OUT_AUX_MODE_MOSFET_70, 342 | OUT_AUX_MODE_MOTOR_MOSFET_50, 343 | OUT_AUX_MODE_MOTOR_MOSFET_70, 344 | }; 345 | 346 | enum TEMP_SENSOR_TYPE 347 | { 348 | TEMP_SENSOR_NTC_10K_25C = 0, 349 | TEMP_SENSOR_PTC_1K_100C, 350 | TEMP_SENSOR_KTY83_122, 351 | TEMP_SENSOR_NTC_100K_25C, 352 | TEMP_SENSOR_KTY84_130 353 | }; 354 | 355 | enum BATTERY_TYPE 356 | { 357 | BATTERY_TYPE_LIION_3_0__4_2, 358 | BATTERY_TYPE_LIIRON_2_6__3_6, 359 | BATTERY_TYPE_LEAD_ACID 360 | }; 361 | 362 | enum BMS_TYPE 363 | { 364 | BMS_TYPE_NONE = 0, 365 | BMS_TYPE_VESC 366 | }; 367 | 368 | enum BMS_FWD_CAN_MODE 369 | { 370 | BMS_FWD_CAN_MODE_DISABLED = 0, 371 | BMS_FWD_CAN_MODE_USB_ONLY, 372 | BMS_FWD_CAN_MODE_ANY 373 | }; 374 | 375 | /** 376 | * @brief BMS configuration 377 | **/ 378 | struct BMS_CONFIG 379 | { 380 | BMS_TYPE type; 381 | double t_limit_start; 382 | double t_limit_end; 383 | double soc_limit_start; 384 | double soc_limit_end; 385 | BMS_FWD_CAN_MODE fwd_can_mode; 386 | }; 387 | 388 | /** 389 | * @brief MC configuration 390 | **/ 391 | struct MCConfiguration 392 | { 393 | uint32_t signature; 394 | // Limits 395 | double l_current_max; 396 | double l_current_min; 397 | double l_in_current_max; 398 | double l_in_current_min; 399 | double l_abs_current_max; 400 | double l_min_erpm; 401 | double l_max_erpm; 402 | double l_erpm_start; 403 | double l_max_erpm_fbrake; 404 | double l_max_erpm_fbrake_cc; 405 | double l_min_vin; 406 | double l_max_vin; 407 | double l_battery_cut_start; 408 | double l_battery_cut_end; 409 | bool l_slow_abs_current; 410 | double l_temp_fet_start; 411 | double l_temp_fet_end; 412 | double l_temp_motor_start; 413 | double l_temp_motor_end; 414 | double l_temp_accel_dec; 415 | double l_min_duty; 416 | double l_max_duty; 417 | double l_watt_max; 418 | double l_watt_min; 419 | double l_current_max_scale; 420 | double l_current_min_scale; 421 | double l_duty_start; 422 | // Overridden limits (Computed during runtime) 423 | double lo_current_max; 424 | double lo_current_min; 425 | double lo_in_current_max; 426 | double lo_in_current_min; 427 | double lo_current_motor_max_now; 428 | double lo_current_motor_min_now; 429 | 430 | // BLDC switching and drive 431 | PWM_MODE pwm_mode; 432 | COMM_MODE comm_mode; 433 | MOTOR_TYPE motor_type; 434 | SENSOR_MODE sensor_mode; 435 | 436 | // Sensorless (bldc) 437 | double sl_min_erpm; 438 | double sl_min_erpm_cycle_int_limit; 439 | double sl_max_fullbreak_current_dir_change; 440 | double sl_cycle_int_limit; 441 | double sl_phase_advance_at_br; 442 | double sl_cycle_int_rpm_br; 443 | double sl_bemf_coupling_k; 444 | // Hall sensor 445 | int hall_table[8]; 446 | double hall_sl_erpm; 447 | 448 | // FOC 449 | double foc_current_kp; 450 | double foc_current_ki; 451 | double foc_f_zv; 452 | double foc_dt_us; 453 | double foc_encoder_offset; 454 | bool foc_encoder_inverted; 455 | double foc_encoder_ratio; 456 | double foc_encoder_sin_offset; 457 | double foc_encoder_sin_gain; 458 | double foc_encoder_cos_offset; 459 | double foc_encoder_cos_gain; 460 | double foc_encoder_sincos_filter_constant; 461 | double foc_motor_l; 462 | double foc_motor_ld_lq_diff; 463 | double foc_motor_r; 464 | double foc_motor_flux_linkage; 465 | double foc_observer_gain; 466 | double foc_observer_gain_slow; 467 | double foc_observer_offset; 468 | double foc_pll_kp; 469 | double foc_pll_ki; 470 | double foc_duty_dowmramp_kp; 471 | double foc_duty_dowmramp_ki; 472 | double foc_openloop_rpm; 473 | double foc_openloop_rpm_low; 474 | double foc_d_gain_scale_start; 475 | double foc_d_gain_scale_max_mod; 476 | double foc_sl_openloop_hyst; 477 | double foc_sl_openloop_time; 478 | double foc_sl_openloop_time_lock; 479 | double foc_sl_openloop_time_ramp; 480 | FOC_SENSOR_MODE foc_sensor_mode; 481 | int foc_hall_table[8]; 482 | double foc_hall_interp_erpm; 483 | double foc_sl_erpm; 484 | bool foc_sample_v0_v7; 485 | bool foc_sample_high_current; 486 | double foc_sat_comp; 487 | bool foc_temp_comp; 488 | double foc_temp_comp_base_temp; 489 | double foc_current_filter_const; 490 | FOC_CC_DECOUPLING_MODE foc_cc_decoupling; 491 | FOC_OBSERVER_TYPE foc_observer_type; 492 | double foc_hfi_voltage_start; 493 | double foc_hfi_voltage_run; 494 | double foc_hfi_voltage_max; 495 | double foc_sl_erpm_hfi; 496 | uint16_t foc_hfi_start_samples; 497 | double foc_hfi_obs_ovr_sec; 498 | FOC_HFI_SAMPLES foc_hfi_samples; 499 | bool foc_offsets_cal_on_boot; 500 | double foc_offsets_current[3]; 501 | double foc_offsets_voltage[3]; 502 | double foc_offsets_voltage_undriven[3]; 503 | bool foc_phase_filter_enable; 504 | double foc_phase_filter_max_erpm; 505 | MTPA_MODE foc_mtpa_mode; 506 | // Field Weakening 507 | double foc_fw_current_max; 508 | double foc_fw_duty_start; 509 | double foc_fw_ramp_time; 510 | double foc_fw_q_current_factor; 511 | 512 | // GPDrive 513 | int gpd_buffer_notify_left; 514 | int gpd_buffer_interpol; 515 | double gpd_current_filter_const; 516 | double gpd_current_kp; 517 | double gpd_current_ki; 518 | 519 | PID_RATE sp_pid_loop_rate; 520 | 521 | // Speed PID 522 | double s_pid_kp; 523 | double s_pid_ki; 524 | double s_pid_kd; 525 | double s_pid_kd_filter; 526 | double s_pid_min_erpm; 527 | bool s_pid_allow_braking; 528 | double s_pid_ramp_erpms_s; 529 | 530 | // Pos PID 531 | double p_pid_kp; 532 | double p_pid_ki; 533 | double p_pid_kd; 534 | double p_pid_kd_proc; 535 | double p_pid_kd_filter; 536 | double p_pid_ang_div; 537 | double p_pid_gain_dec_angle; 538 | double p_pid_offset; 539 | 540 | // Current controller 541 | double cc_startup_boost_duty; 542 | double cc_min_current; 543 | double cc_gain; 544 | double cc_ramp_step_max; 545 | 546 | // Misc 547 | int32_t m_fault_stop_time_ms; 548 | double m_duty_ramp_step; 549 | double m_current_backoff_gain; 550 | uint32_t m_encoder_counts; 551 | SENSOR_PORT_MODE m_sensor_port_mode; 552 | bool m_invert_direction; 553 | DRV8301_OC_MODE m_drv8301_oc_mode; 554 | int m_drv8301_oc_adj; 555 | double m_bldc_f_sw_min; 556 | double m_bldc_f_sw_max; 557 | double m_dc_f_sw; 558 | double m_ntc_motor_beta; 559 | OUT_AUX_MODE m_out_aux_mode; 560 | TEMP_SENSOR_TYPE m_motor_temp_sens_type; 561 | double m_ptc_motor_coeff; 562 | int m_hall_extra_samples; 563 | // Setup info 564 | int si_motor_poles; 565 | double si_gear_ratio; 566 | double si_wheel_diameter; 567 | BATTERY_TYPE si_battery_type; 568 | int si_battery_cells; 569 | double si_battery_ah; 570 | double si_motor_nl_current; 571 | 572 | // BMS Configuration 573 | BMS_CONFIG bms; 574 | 575 | // Protect from flash corruption. 576 | uint16_t crc; 577 | }; 578 | 579 | #endif // VESC_DRIVER_DATA_MAP_HPP_ 580 | -------------------------------------------------------------------------------- /vesc_hw_interface/src/vesc_hw_interface.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | ********************************************************************/ 16 | 17 | #include "vesc_hw_interface/vesc_hw_interface.hpp" 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace vesc_hw_interface 26 | { 27 | VescHwInterface::VescHwInterface() 28 | { 29 | vesc_interface_ = std::make_shared( 30 | std::string(), std::bind(&VescHwInterface::packetCallback, this, std::placeholders::_1), 31 | std::bind(&VescHwInterface::errorCallback, this, std::placeholders::_1)); 32 | } 33 | 34 | CallbackReturn VescHwInterface::on_init(const hardware_interface::HardwareInfo& info) 35 | { 36 | if (hardware_interface::ActuatorInterface::on_init(info) != CallbackReturn::SUCCESS) 37 | { 38 | return CallbackReturn::ERROR; 39 | } 40 | 41 | command_ = std::numeric_limits::quiet_NaN(); 42 | position_ = std::numeric_limits::quiet_NaN(); 43 | velocity_ = std::numeric_limits::quiet_NaN(); 44 | effort_ = std::numeric_limits::quiet_NaN(); 45 | 46 | // initializes commands and states 47 | position_ = 0.0; 48 | velocity_ = 0.0; 49 | effort_ = 0.0; 50 | sensor_initialize_ = false; 51 | position_steps_ = 0.0; 52 | 53 | // reads system parameters 54 | port_ = info_.hardware_parameters["port"]; 55 | gear_ratio_ = 1.0; 56 | if (info_.hardware_parameters.find("gear_ratio") != info_.hardware_parameters.end()) 57 | { 58 | gear_ratio_ = std::stod(info_.hardware_parameters["gear_ratio"]); 59 | } 60 | torque_const_ = 1.0; 61 | if (info_.hardware_parameters.find("torque_const") != info_.hardware_parameters.end()) 62 | { 63 | torque_const_ = std::stod(info_.hardware_parameters["torque_const"]); 64 | } 65 | num_hall_sensors_ = 3; 66 | if (info_.hardware_parameters.find("num_hall_sensors") != info_.hardware_parameters.end()) 67 | { 68 | num_hall_sensors_ = std::stoi(info_.hardware_parameters["num_hall_sensors"]); 69 | } 70 | screw_lead_ = 1.0; 71 | if (info_.hardware_parameters.find("screw_lead") != info_.hardware_parameters.end()) 72 | { 73 | screw_lead_ = std::stod(info_.hardware_parameters["screw_lead"]); 74 | } 75 | 76 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "Gear ratio is set to %f", gear_ratio_); 77 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "Torque constant is set to %f", torque_const_); 78 | 79 | // reads driving mode setting 80 | // - assigns an empty string if param. is not found 81 | 82 | num_rotor_poles_ = 2; 83 | if (info_.hardware_parameters.find("num_rotor_poles") != info_.hardware_parameters.end()) 84 | { 85 | num_rotor_poles_ = std::stoi(info_.hardware_parameters["num_rotor_poles"]); 86 | } 87 | 88 | if (num_rotor_poles_ % 2 != 0) 89 | { 90 | RCLCPP_ERROR(rclcpp::get_logger("VescHwInterface"), "There should be even number of rotor poles"); 91 | rclcpp::shutdown(); 92 | return CallbackReturn::ERROR; 93 | } 94 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "The number of motor pole pairs is set to %d", num_rotor_poles_); 95 | 96 | command_mode_ = info_.hardware_parameters["command_mode"]; 97 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "mode: %s", command_mode_.data()); 98 | 99 | const hardware_interface::ComponentInfo& joint = info_.joints[0]; 100 | joint_name_ = joint.name; 101 | 102 | // parse URDF for joint type 103 | auto urdf = info_.original_xml; 104 | if (!urdf.empty()) { 105 | tinyxml2::XMLDocument doc; 106 | if (doc.Parse(urdf.c_str()) != tinyxml2::XML_SUCCESS) { 107 | RCLCPP_ERROR_STREAM(get_logger(), "Failed to parse URDF XML"); 108 | return hardware_interface::CallbackReturn::ERROR; 109 | } 110 | const tinyxml2::XMLElement * joint_it = doc.RootElement()->FirstChildElement("joint"); 111 | while (joint_it) { 112 | const tinyxml2::XMLAttribute * name_attr = joint_it->FindAttribute("name"); 113 | const tinyxml2::XMLAttribute * type_attr = joint_it->FindAttribute("type"); 114 | if (name_attr && type_attr) { 115 | std::string name = joint_it->Attribute("name"); 116 | std::string type = joint_it->Attribute("type"); 117 | if (name == joint_name_) { 118 | joint_type_ = type; 119 | break; 120 | } 121 | } 122 | joint_it = joint_it->NextSiblingElement("joint"); 123 | } 124 | } 125 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "joint type: %s", joint_type_.data()); 126 | if ((joint_type_ != "revolute") && (joint_type_ != "continuous") && (joint_type_ != "prismatic")) 127 | { 128 | RCLCPP_FATAL(rclcpp::get_logger("VescHwInterface"), "Verify your joint type"); 129 | return CallbackReturn::ERROR; 130 | } 131 | 132 | if (joint.command_interfaces.size() != 3) 133 | { 134 | RCLCPP_FATAL(rclcpp::get_logger("VescHwInterface"), "Joint '%s' has %zu command interfaces found. 3 expected.", 135 | joint.name.c_str(), joint.command_interfaces.size()); 136 | return CallbackReturn::ERROR; 137 | } 138 | std::vector command_interface_order = { hardware_interface::HW_IF_POSITION, 139 | hardware_interface::HW_IF_VELOCITY, 140 | hardware_interface::HW_IF_EFFORT }; 141 | for (size_t i = 0; i < joint.command_interfaces.size(); ++i) 142 | { 143 | if (joint.command_interfaces[i].name != command_interface_order[i]) 144 | { 145 | RCLCPP_FATAL(rclcpp::get_logger("VescHwInterface"), 146 | "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(), 147 | joint.command_interfaces[i].name.c_str(), command_interface_order[i].c_str()); 148 | return hardware_interface::CallbackReturn::ERROR; 149 | } 150 | } 151 | 152 | if (joint.state_interfaces.size() != 3) 153 | { 154 | RCLCPP_FATAL(rclcpp::get_logger("VescHwInterface"), "Joint '%s' has %zu state interface. 3 expected.", 155 | joint.name.c_str(), joint.state_interfaces.size()); 156 | return CallbackReturn::ERROR; 157 | } 158 | std::vector state_interface_order = { hardware_interface::HW_IF_POSITION, 159 | hardware_interface::HW_IF_VELOCITY, 160 | hardware_interface::HW_IF_EFFORT }; 161 | for (size_t i = 0; i < joint.state_interfaces.size(); ++i) 162 | { 163 | if (joint.state_interfaces[i].name != state_interface_order[i]) 164 | { 165 | RCLCPP_FATAL(rclcpp::get_logger("VescHwInterface"), 166 | "Joint '%s' have '%s' as first state interface. '%s' expected.", joint.name.c_str(), 167 | joint.state_interfaces[i].name.c_str(), state_interface_order[i].c_str()); 168 | return hardware_interface::CallbackReturn::ERROR; 169 | } 170 | } 171 | 172 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "Successfully Initialized!"); 173 | 174 | return CallbackReturn::SUCCESS; 175 | } 176 | 177 | CallbackReturn VescHwInterface::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) 178 | { 179 | try 180 | { 181 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "connect to %s", port_.c_str()); 182 | vesc_interface_->connect(port_); 183 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "connected"); 184 | } 185 | catch (const vesc_driver::SerialException& exception) 186 | { 187 | RCLCPP_FATAL(rclcpp::get_logger("VescHwInterface"), "Failed to connect to the VESC, %s.", exception.what()); 188 | return CallbackReturn::FAILURE; 189 | } 190 | 191 | if ((command_mode_ == hardware_interface::HW_IF_POSITION) || (command_mode_ == hardware_interface::HW_IF_VELOCITY) || (command_mode_ == hardware_interface::HW_IF_EFFORT)) 192 | { 193 | vesc_interface_->requestMCConfiguration(); 194 | rclcpp::sleep_for(std::chrono::milliseconds(100)); 195 | } 196 | 197 | upper_limit_ = 0.0; 198 | lower_limit_ = 0.0; 199 | homing_offset_ = 0.0; 200 | homing_done_ = false; 201 | homing_enabled_ = false; 202 | if (command_mode_ == hardware_interface::HW_IF_POSITION || command_mode_ == "position_duty") 203 | { 204 | // parse URDF for limit parameters 205 | auto joint_limit_itr = info_.limits.find(joint_name_); 206 | if (joint_limit_itr != info_.limits.end()) 207 | { 208 | upper_limit_ = joint_limit_itr->second.max_position; 209 | lower_limit_ = joint_limit_itr->second.min_position; 210 | } else { 211 | RCLCPP_WARN(rclcpp::get_logger("VescHwInterface"), "No joint position limits found in URDF, using default limits"); 212 | } 213 | 214 | // initializes the servo controller 215 | servo_controller_.init(info_, vesc_interface_, gear_ratio_, torque_const_, num_rotor_poles_, num_hall_sensors_, 216 | joint_type_ == "revolute" ? 0 : 217 | joint_type_ == "continuous" ? 1 : 218 | 2, 219 | screw_lead_, upper_limit_, lower_limit_); 220 | 221 | auto calibration_params = servo_controller_.getCalibrationParameters(); 222 | homing_enabled_ = calibration_params.enable_calibration; 223 | homing_position_ = calibration_params.calibration_position; 224 | if (homing_enabled_) 225 | { 226 | while (rclcpp::ok()) 227 | { 228 | vesc_interface_->requestState(); 229 | servo_controller_.spinSensorData(); 230 | if (servo_controller_.calibrate()) 231 | break; 232 | rclcpp::sleep_for(std::chrono::milliseconds(10)); 233 | } 234 | } 235 | homing_done_ = true; 236 | if (command_mode_ == "position_duty") 237 | { 238 | position_ = servo_controller_.getPositionSens(); 239 | velocity_ = servo_controller_.getVelocitySens(); 240 | effort_ = servo_controller_.getEffortSens(); 241 | } 242 | } 243 | 244 | if (command_mode_ == "velocity_duty") 245 | { 246 | // initializes the wheel controller 247 | wheel_controller_.init(info_, vesc_interface_); 248 | wheel_controller_.setGearRatio(gear_ratio_); 249 | wheel_controller_.setTorqueConst(torque_const_); 250 | wheel_controller_.setRotorPoles(num_rotor_poles_); 251 | wheel_controller_.setHallSensors(num_hall_sensors_); 252 | } 253 | 254 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "Successfully configured!"); 255 | 256 | return CallbackReturn::SUCCESS; 257 | } 258 | 259 | CallbackReturn VescHwInterface::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/) 260 | { 261 | return CallbackReturn::SUCCESS; 262 | } 263 | 264 | CallbackReturn VescHwInterface::on_shutdown(const rclcpp_lifecycle::State& /*previous_state*/) 265 | { 266 | return CallbackReturn::SUCCESS; 267 | } 268 | 269 | CallbackReturn VescHwInterface::on_error(const rclcpp_lifecycle::State& /*previous_state*/) 270 | { 271 | return CallbackReturn::SUCCESS; 272 | } 273 | 274 | std::vector VescHwInterface::export_state_interfaces() 275 | { 276 | std::vector state_interfaces; 277 | state_interfaces.emplace_back( 278 | hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &position_)); 279 | state_interfaces.emplace_back( 280 | hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &velocity_)); 281 | state_interfaces.emplace_back( 282 | hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_EFFORT, &effort_)); 283 | return state_interfaces; 284 | } 285 | 286 | std::vector VescHwInterface::export_command_interfaces() 287 | { 288 | std::vector command_interfaces; 289 | command_interfaces.emplace_back( 290 | hardware_interface::CommandInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &command_)); 291 | command_interfaces.emplace_back( 292 | hardware_interface::CommandInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &command_)); 293 | command_interfaces.emplace_back( 294 | hardware_interface::CommandInterface(info_.joints[0].name, hardware_interface::HW_IF_EFFORT, &command_)); 295 | 296 | return command_interfaces; 297 | } 298 | 299 | CallbackReturn VescHwInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) 300 | { 301 | // Set some default values 302 | if (std::isnan(position_)) 303 | position_ = 0; 304 | if (std::isnan(velocity_)) 305 | velocity_ = 0; 306 | if (std::isnan(effort_)) 307 | effort_ = 0; 308 | 309 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "System successfully activated!"); 310 | return CallbackReturn::SUCCESS; 311 | } 312 | 313 | CallbackReturn VescHwInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) 314 | { 315 | return CallbackReturn::SUCCESS; 316 | } 317 | 318 | hardware_interface::return_type VescHwInterface::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) 319 | { 320 | // requests joint states 321 | // function `packetCallback` will be called after receiving return packets 322 | if (command_mode_ == "position_duty") 323 | { 324 | // For PID control, request packets are automatically sent in the control cycle. 325 | // The latest data is read in this function. 326 | vesc_interface_->requestState(); 327 | servo_controller_.spinSensorData(); 328 | position_ = servo_controller_.getPositionSens(); 329 | velocity_ = servo_controller_.getVelocitySens(); 330 | effort_ = servo_controller_.getEffortSens(); 331 | } 332 | else if (command_mode_ == "velocity_duty") 333 | { 334 | vesc_interface_->requestState(); 335 | position_ = wheel_controller_.getPositionSens(); 336 | velocity_ = wheel_controller_.getVelocitySens(); 337 | effort_ = wheel_controller_.getEffortSens(); 338 | } 339 | else 340 | { 341 | vesc_interface_->requestState(); 342 | } 343 | 344 | return hardware_interface::return_type::OK; 345 | } 346 | 347 | hardware_interface::return_type VescHwInterface::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& period) 348 | { 349 | // sends commands 350 | 351 | auto command = command_; 352 | if (std::isnan(command) && command_mode_ != "position_duty") { 353 | command = 0.0; 354 | } 355 | if (command_mode_ == "position_duty") 356 | { 357 | // Limit the speed using the parameters listed in xacro 358 | // limit_position_interface_.enforceLimits(period); 359 | // limit_position_handle_.enforceLimits(period); 360 | 361 | // executes PID control 362 | servo_controller_.setTargetPosition(command); 363 | servo_controller_.control(1.0 / period.seconds()); 364 | } 365 | else if (command_mode_ == "position") 366 | { 367 | command = VESC_POS_MAPPING_RANGE * (command - homing_position_) / (upper_limit_ - lower_limit_); 368 | command = std::fmod(command + homing_offset_ + VESC_POS_RANGE, VESC_POS_RANGE); 369 | vesc_interface_->setPosition(command); 370 | } 371 | else if (command_mode_ == "velocity") 372 | { 373 | // limit_velocity_interface_.enforceLimits(period); 374 | 375 | // converts the velocity unit: rad/s or m/s -> rpm -> erpm 376 | const double command_rpm = command * 60.0 / 2.0 / M_PI / gear_ratio_; 377 | const double command_erpm = command_rpm * static_cast(num_rotor_poles_) / 2; 378 | 379 | // sends a reference velocity command 380 | vesc_interface_->setSpeed(command_erpm); 381 | } 382 | else if (command_mode_ == "velocity_duty") 383 | { 384 | // limit_velocity_interface_.enforceLimits(period); 385 | 386 | // executes PID control 387 | wheel_controller_.setTargetVelocity(command); 388 | wheel_controller_.control(1.0 / period.seconds()); 389 | } 390 | else if (command_mode_ == "effort") 391 | { 392 | // limit_effort_interface_.enforceLimits(period); 393 | 394 | // converts the command unit: Nm or N -> A 395 | const double command_current = command * gear_ratio_ / torque_const_; 396 | 397 | // sends a reference current command 398 | vesc_interface_->setCurrent(command_current); 399 | } 400 | else if (command_mode_ == "effort_duty") 401 | { 402 | command = std::max(-1.0, command); 403 | command = std::min(1.0, command); 404 | 405 | // sends a duty command 406 | vesc_interface_->setDutyCycle(command); 407 | } 408 | return hardware_interface::return_type::OK; 409 | } 410 | 411 | rclcpp::Time VescHwInterface::getTime() const 412 | { 413 | auto clock = rclcpp::Clock(RCL_ROS_TIME); 414 | return clock.now(); 415 | } 416 | 417 | void VescHwInterface::packetCallback(const std::shared_ptr& packet) 418 | { 419 | if (!vesc_interface_->isRxDataUpdated()) 420 | { 421 | RCLCPP_WARN(rclcpp::get_logger("VescHwInterface"), "[VescHwInterface::packetCallback]packetCallcack called, but " 422 | "no packet received"); 423 | } 424 | if (command_mode_ == "position_duty") 425 | { 426 | servo_controller_.updateSensor(packet); 427 | return; 428 | } 429 | if (command_mode_ == "velocity_duty") 430 | { 431 | wheel_controller_.updateSensor(packet); 432 | return; 433 | } 434 | 435 | if (packet->getName() == "MCConfiguration") 436 | { 437 | std::shared_ptr mc_conf = std::dynamic_pointer_cast(packet); 438 | 439 | auto config = mc_conf->getConfig(); 440 | num_rotor_poles_ = config.si_motor_poles; 441 | gear_ratio_ = config.si_gear_ratio; 442 | if (config.motor_type == MOTOR_TYPE_FOC) { 443 | auto pole_pairs = num_rotor_poles_ / 2.0; 444 | auto flux_linkage = config.foc_motor_flux_linkage; 445 | torque_const_ = (60.0 / (2.0 * M_PI * pole_pairs)) * flux_linkage; 446 | } 447 | RCLCPP_INFO_STREAM(rclcpp::get_logger("VescHwInterface"), "Extracted configuration from VESC:"); 448 | RCLCPP_INFO_STREAM(rclcpp::get_logger("VescHwInterface"), " - Number of rotor poles: " << num_rotor_poles_); 449 | RCLCPP_INFO_STREAM(rclcpp::get_logger("VescHwInterface"), " - Gear ratio: " << gear_ratio_); 450 | RCLCPP_INFO_STREAM(rclcpp::get_logger("VescHwInterface"), " - Torque constant: " << torque_const_); 451 | } 452 | else if (packet->getName() == "Values") 453 | { 454 | std::shared_ptr values = std::dynamic_pointer_cast(packet); 455 | 456 | const auto current = values->getMotorCurrent(); 457 | const auto velocity_rpm = values->getVelocityERPM() / static_cast(num_rotor_poles_ / 2); 458 | const auto position = values->getPosition(); 459 | const auto steps = static_cast(values->getTachometer()); 460 | 461 | if (!homing_done_ && homing_enabled_) 462 | { 463 | servo_controller_.updateSensor(packet); 464 | homing_offset_ = position; 465 | return; 466 | } 467 | if (!sensor_initialize_) 468 | { 469 | if (joint_type_ == "revolute" || joint_type_ == "prismatic") 470 | { 471 | if (!homing_enabled_) 472 | { 473 | sensor_initialize_ = true; 474 | return; 475 | } 476 | sensor_initialize_ = (std::fabs(homing_offset_ - position) < std::numeric_limits::epsilon()) ? true : false; 477 | homing_offset_ = position; 478 | } 479 | else if (joint_type_ == "continuous") 480 | { 481 | sensor_initialize_ = (steps == prev_steps_) ? true : false; 482 | prev_steps_ = steps; 483 | } 484 | RCLCPP_INFO_STREAM(rclcpp::get_logger("VescHwInterface"), "waiting for values to settle..."); 485 | return; 486 | } 487 | 488 | // calculate position 489 | if (joint_type_ == "revolute" || joint_type_ == "prismatic") 490 | { 491 | // `position` is [deg] but here we mapped the position to the joint limits hence unit is irrelevant 492 | position_ = std::fmod(position - homing_offset_ + VESC_POS_RANGE, VESC_POS_RANGE); 493 | if (position_ > VESC_POS_WRAP_THRESHOLD) 494 | { 495 | position_ -= VESC_POS_RANGE; 496 | } 497 | position_ = homing_position_ + position_ * (upper_limit_ - lower_limit_) / VESC_POS_MAPPING_RANGE; 498 | } 499 | else if (joint_type_ == "continuous") 500 | { 501 | // use tachometer to calculate position 502 | position_steps_ += static_cast(steps - prev_steps_); 503 | prev_steps_ = steps; 504 | position_ = (position_steps_ * 2.0 * M_PI) / (num_rotor_poles_ * 3.0) * gear_ratio_; // unit: rad 505 | } 506 | 507 | // calculate velocity 508 | if (joint_type_ == "revolute" || joint_type_ == "continuous") 509 | { 510 | velocity_ = (velocity_rpm * gear_ratio_) / 60.0 * 2.0 * M_PI; // unit: rad/s 511 | } 512 | else if (joint_type_ == "prismatic") 513 | { 514 | velocity_ = (velocity_rpm * gear_ratio_) / 60.0; // unit: m/s 515 | } 516 | 517 | // calculate effort 518 | effort_ = current * torque_const_ / gear_ratio_; // unit: Nm or N 519 | } 520 | } 521 | 522 | void VescHwInterface::errorCallback(const std::string& error) 523 | { 524 | RCLCPP_ERROR(rclcpp::get_logger("VescHwInterface"), "%s", error.c_str()); 525 | return; 526 | } 527 | 528 | } // namespace vesc_hw_interface 529 | 530 | #include "pluginlib/class_list_macros.hpp" 531 | PLUGINLIB_EXPORT_CLASS(vesc_hw_interface::VescHwInterface, hardware_interface::ActuatorInterface) 532 | -------------------------------------------------------------------------------- /vesc_hw_interface/src/vesc_servo_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Copyright (c) 2019, SoftBank Corp. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | ********************************************************************/ 16 | 17 | #include "vesc_hw_interface/vesc_servo_controller.hpp" 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace vesc_hw_interface 27 | { 28 | 29 | VescServoController::VescServoController() : num_rotor_poles_(1), gear_ratio_(1.0), torque_const_(1.0) 30 | { 31 | } 32 | 33 | VescServoController::~VescServoController() 34 | { 35 | interface_ptr_->setDutyCycle(0.0); 36 | } 37 | 38 | void VescServoController::init(hardware_interface::HardwareInfo& info, 39 | const std::shared_ptr& interface_ptr, const double gear_ratio, 40 | const double torque_const, const int rotor_poles, const int hall_sensors, 41 | const int joint_type, const double screw_lead, const double upper_endstop_position, 42 | const double lower_endstop_position) 43 | { 44 | // initializes members 45 | if (!interface_ptr) 46 | { 47 | rclcpp::shutdown(); 48 | } 49 | else 50 | { 51 | interface_ptr_ = interface_ptr; 52 | } 53 | 54 | gear_ratio_ = gear_ratio; 55 | torque_const_ = torque_const; 56 | num_rotor_poles_ = rotor_poles; 57 | num_hall_sensors_ = hall_sensors; 58 | joint_type_ = joint_type; 59 | screw_lead_ = screw_lead; 60 | upper_endstop_position_ = upper_endstop_position; 61 | lower_endstop_position_ = lower_endstop_position; 62 | 63 | calibration_flag_ = true; 64 | sensor_initialize_ = true; 65 | zero_position_ = 0.0; 66 | error_integ_ = 0.0; 67 | steps_previous_ = 0; 68 | position_steps_ = 0; 69 | calibration_steps_ = 0; 70 | calibration_previous_position_ = 0.0; 71 | calibration_rewind_ = false; 72 | 73 | // reads parameters 74 | kp_ = 50.0; 75 | if (info.hardware_parameters.find("servo/Kp") != info.hardware_parameters.end()) 76 | { 77 | kp_ = std::stod(info.hardware_parameters["servo/Kp"]); 78 | } 79 | ki_ = 0.0; 80 | if (info.hardware_parameters.find("servo/Ki") != info.hardware_parameters.end()) 81 | { 82 | ki_ = std::stod(info.hardware_parameters["servo/Ki"]); 83 | } 84 | kd_ = 1.0; 85 | if (info.hardware_parameters.find("servo/Kd") != info.hardware_parameters.end()) 86 | { 87 | kd_ = std::stod(info.hardware_parameters["servo/Kd"]); 88 | } 89 | i_clamp_ = 1.0; 90 | if (info.hardware_parameters.find("servo/i_clamp") != info.hardware_parameters.end()) 91 | { 92 | i_clamp_ = std::stod(info.hardware_parameters["servo/i_clamp"]); 93 | } 94 | duty_limiter_ = 1.0; 95 | if (info.hardware_parameters.find("servo/duty_limiter") != info.hardware_parameters.end()) 96 | { 97 | duty_limiter_ = std::stod(info.hardware_parameters["servo/duty_limiter"]); 98 | } 99 | antiwindup_ = true; 100 | if (info.hardware_parameters.find("servo/antiwindup") != info.hardware_parameters.end()) 101 | { 102 | antiwindup_ = info.hardware_parameters["servo/antiwindup"] == "true"; 103 | } 104 | control_rate_ = 100.0; 105 | if (info.hardware_parameters.find("servo/control_rate") != info.hardware_parameters.end()) 106 | { 107 | control_rate_ = std::stod(info.hardware_parameters["servo/control_rate"]); 108 | } 109 | calibration_current_ = 0.0; 110 | if (info.hardware_parameters.find("servo/calibration_current") != info.hardware_parameters.end()) 111 | { 112 | calibration_current_ = std::stod(info.hardware_parameters["servo/calibration_current"]); 113 | } 114 | calibration_strict_current_ = calibration_current_; 115 | if (info.hardware_parameters.find("servo/calibration_strict_current") != info.hardware_parameters.end()) 116 | { 117 | calibration_strict_current_ = std::stod(info.hardware_parameters["servo/calibration_strict_current"]); 118 | } 119 | calibration_duty_ = 0.1; 120 | if (info.hardware_parameters.find("servo/calibration_duty") != info.hardware_parameters.end()) 121 | { 122 | calibration_duty_ = std::stod(info.hardware_parameters["servo/calibration_duty"]); 123 | } 124 | calibration_strict_duty_ = calibration_duty_; 125 | if (info.hardware_parameters.find("servo/calibration_strict_duty") != info.hardware_parameters.end()) 126 | { 127 | calibration_strict_duty_ = std::stod(info.hardware_parameters["servo/calibration_strict_duty"]); 128 | } 129 | calibration_mode_ = CURRENT_; 130 | if (info.hardware_parameters.find("servo/calibration_mode") != info.hardware_parameters.end()) 131 | { 132 | calibration_mode_ = info.hardware_parameters["servo/calibration_mode"]; 133 | } 134 | calibration_position_ = 0.0; 135 | if (info.hardware_parameters.find("servo/calibration_position") != info.hardware_parameters.end()) 136 | { 137 | calibration_position_ = std::stod(info.hardware_parameters["servo/calibration_position"]); 138 | } 139 | calibration_flag_ = true; 140 | if (info.hardware_parameters.find("servo/calibration") != info.hardware_parameters.end()) 141 | { 142 | calibration_flag_ = info.hardware_parameters["servo/calibration"] == "true"; 143 | } 144 | calibration_result_path_ = ""; 145 | if (info.hardware_parameters.find("servo/calibration_result_path") != info.hardware_parameters.end()) 146 | { 147 | calibration_result_path_ = info.hardware_parameters["servo/calibration_result_path"]; 148 | } 149 | if (!calibration_result_path_.empty()) 150 | { 151 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[Servo Control] Latest position will be saved to %s", 152 | calibration_result_path_.data()); 153 | } 154 | if (!calibration_flag_) 155 | { 156 | target_position_previous_ = target_position_; 157 | sens_position_ = target_position_; 158 | 159 | position_steps_ = sens_position_ * (num_hall_sensors_ * num_rotor_poles_) / gear_ratio_; 160 | 161 | if (joint_type_ == 0 || joint_type_ == 1) 162 | { 163 | position_steps_ /= 2.0 * M_PI; 164 | } 165 | else if (joint_type_ == 2) 166 | { 167 | position_steps_ /= screw_lead_; 168 | } 169 | vesc_step_difference_.resetStepDifference(position_steps_); 170 | } 171 | 172 | bool use_endstop = false; 173 | if (info.hardware_parameters.find("servo/use_endstop") != info.hardware_parameters.end()) 174 | { 175 | use_endstop = info.hardware_parameters["servo/use_endstop"] == "true"; 176 | } 177 | if (use_endstop) 178 | { 179 | rclcpp::NodeOptions options; 180 | std::string endstop_receiver_name = info.name + "_endstop_receiver"; 181 | std::transform( 182 | endstop_receiver_name.begin(), endstop_receiver_name.end(), endstop_receiver_name.begin(), 183 | [](unsigned char c) { return std::tolower(c); }); 184 | options.arguments({"--ros-args", "-r", "__node:=" + endstop_receiver_name}); 185 | node_ = rclcpp::Node::make_shared("_", options); 186 | endstop_sub_ = node_->create_subscription( 187 | "endstop", rclcpp::SensorDataQoS(), 188 | std::bind(&VescServoController::endstopCallback, this, std::placeholders::_1)); 189 | std_msgs::msg::Bool endstop_msg; 190 | rclcpp::wait_for_message(endstop_msg, node_, "endstop"); 191 | while (endstop_sub_->get_publisher_count() == 0) 192 | { 193 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[Servo Control] Waiting for endstop sensor publisher..."); 194 | rclcpp::sleep_for(std::chrono::milliseconds(100)); 195 | } 196 | } 197 | endstop_margin_ = 0.02; 198 | if (info.hardware_parameters.find("servo/endstop_margin") != info.hardware_parameters.end()) 199 | { 200 | endstop_margin_ = std::stod(info.hardware_parameters["servo/endstop_margin"]); 201 | } 202 | endstop_threshold_ = 0.8; 203 | if (info.hardware_parameters.find("servo/endstop_threshold") != info.hardware_parameters.end()) 204 | { 205 | endstop_threshold_ = std::stod(info.hardware_parameters["servo/endstop_threshold"]); 206 | } 207 | endstop_window_ = 1; 208 | if (info.hardware_parameters.find("servo/endstop_window") != info.hardware_parameters.end()) 209 | { 210 | endstop_window_ = std::stoi(info.hardware_parameters["servo/endstop_window"]); 211 | } 212 | endstop_deque_ = std::deque(endstop_window_, 0); 213 | position_resolution_ = 1.0 / (num_hall_sensors_ * num_rotor_poles_) * gear_ratio_; 214 | if (joint_type_ == 0 || joint_type_ == 1) 215 | { 216 | position_resolution_ = position_resolution_ * 2.0 * M_PI; // unit: rad 217 | } 218 | else if (joint_type_ == 2) 219 | { 220 | position_resolution_ = position_resolution_ * screw_lead_; // unit: m 221 | } 222 | 223 | // shows parameters 224 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[Servo Gains] P: %f, I: %f, D: %f", kp_, ki_, kd_); 225 | if (calibration_mode_ == CURRENT_) 226 | { 227 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[Servo Calibration] Mode: %s, value: %f", CURRENT_.data(), 228 | calibration_current_); 229 | } 230 | else if (calibration_mode_ == DUTY_) 231 | { 232 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[Servo Calibration] Mode: %s, value: %f", DUTY_.data(), 233 | calibration_duty_); 234 | } 235 | else 236 | { 237 | RCLCPP_ERROR(rclcpp::get_logger("VescHwInterface"), "[Servo Calibration] Invalid mode"); 238 | } 239 | 240 | // Smoothing differentiation when hall sensor resolution is insufficient 241 | bool smooth_diff = true; 242 | if (info.hardware_parameters.find("servo/enable_smooth_diff") != info.hardware_parameters.end()) 243 | { 244 | smooth_diff = info.hardware_parameters["servo/enable_smooth_diff"] == "true"; 245 | } 246 | if (smooth_diff) 247 | { 248 | double smooth_diff_max_sampling_time = 1.0; 249 | if (info.hardware_parameters.find("servo/smooth_diff/max_sample_sec") != info.hardware_parameters.end()) 250 | { 251 | smooth_diff_max_sampling_time = std::stod(info.hardware_parameters["servo/smooth_diff/max_sample_sec"]); 252 | } 253 | int counter_td_vw_max_step = 10; 254 | if (info.hardware_parameters.find("servo/smooth_diff/max_smooth_step") != info.hardware_parameters.end()) 255 | { 256 | counter_td_vw_max_step = std::stoi(info.hardware_parameters["servo/smooth_diff/max_smooth_step"]); 257 | } 258 | vesc_step_difference_.enableSmooth(control_rate_, smooth_diff_max_sampling_time, counter_td_vw_max_step); 259 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), 260 | "[Servo Control] Smooth differentiation enabled, max_sample_sec: %f, max_smooth_step: %d", 261 | smooth_diff_max_sampling_time, counter_td_vw_max_step); 262 | } 263 | // Create timer callback for PID servo control 264 | // control_timer_ = nh.createTimer(ros::Duration(1.0 / control_rate_), &VescServoController::controlTimerCallback, 265 | // this); 266 | 267 | return; 268 | } 269 | 270 | void VescServoController::control(const double control_rate) 271 | { 272 | if (sensor_initialize_) 273 | return; 274 | 275 | if (std::isnan(target_position_)) 276 | { 277 | target_position_ = calibration_position_; 278 | vesc_step_difference_.resetStepDifference(position_steps_); 279 | } 280 | double error = target_position_ - sens_position_; 281 | // PID control 282 | double step_diff = vesc_step_difference_.getStepDifference(position_steps_); 283 | double current_vel = step_diff * 2.0 * M_PI / (num_rotor_poles_ * num_hall_sensors_) * control_rate * gear_ratio_; 284 | double target_vel = (target_position_ - target_position_previous_) * control_rate; 285 | 286 | auto average = std::accumulate(endstop_deque_.begin(), endstop_deque_.end(), 0.0) / endstop_deque_.size(); 287 | if (error > 0 && average >= endstop_threshold_) 288 | { 289 | RCLCPP_WARN(rclcpp::get_logger("VescHwInterface"), "[Servo Control] Upper endstop signal received. Stop servo."); 290 | error = 0.0; 291 | target_vel = 0.0; 292 | // Wait for target position convergence 293 | if (std::fabs(target_position_ - target_position_previous_) < std::numeric_limits::epsilon() && 294 | std::fabs(target_position_ - upper_endstop_position_) < endstop_margin_) 295 | { 296 | zero_position_ = sens_position_ + zero_position_ - upper_endstop_position_; 297 | sens_position_ = upper_endstop_position_; 298 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[Servo Control] Reset position to %f.", 299 | upper_endstop_position_); 300 | } 301 | } 302 | else if (error < 0 && average <= -endstop_threshold_) 303 | { 304 | RCLCPP_WARN(rclcpp::get_logger("VescHwInterface"), "[Servo Control] Lower endstop signal received. Stop servo."); 305 | error = 0.0; 306 | target_vel = 0.0; 307 | // Wait for target position convergence 308 | 309 | if (std::fabs(target_position_ - target_position_previous_) < std::numeric_limits::epsilon() && 310 | std::fabs(target_position_ - lower_endstop_position_) < endstop_margin_) 311 | { 312 | zero_position_ = sens_position_ + zero_position_ - lower_endstop_position_; 313 | sens_position_ = lower_endstop_position_; 314 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[Servo Control] Reset position to %f.", 315 | lower_endstop_position_); 316 | } 317 | } 318 | 319 | if (std::fabs(error) < position_resolution_) 320 | { 321 | error = 0.0; 322 | } 323 | double error_dt = target_vel - current_vel; 324 | error_integ_ += (error / control_rate); 325 | error_integ_ = std::clamp(error_integ_, -i_clamp_ / ki_, i_clamp_ / ki_); 326 | 327 | const double u_p = kp_ * error; 328 | const double u_d = kd_ * error_dt; 329 | const double u_i = ki_ * error_integ_; 330 | double u = u_p + u_d + u_i; 331 | 332 | // limit integral 333 | if (antiwindup_) 334 | { 335 | if (u > duty_limiter_ && error_integ_ > 0) 336 | { 337 | error_integ_ = std::max(0.0, (duty_limiter_ - u_p - u_d) / ki_); 338 | } 339 | else if (u < -duty_limiter_ && error_integ_ < 0) 340 | { 341 | error_integ_ = std::min(0.0, (-duty_limiter_ - u_p - u_d) / ki_); 342 | } 343 | } 344 | 345 | // limit duty value 346 | u = std::clamp(u, -duty_limiter_, duty_limiter_); 347 | 348 | // updates previous data 349 | target_position_previous_ = target_position_; 350 | 351 | // command duty 352 | interface_ptr_->setDutyCycle(u); 353 | return; 354 | } 355 | 356 | void VescServoController::setTargetPosition(const double position) 357 | { 358 | target_position_ = std::clamp(position, lower_endstop_position_, upper_endstop_position_); 359 | } 360 | 361 | void VescServoController::setGearRatio(const double gear_ratio) 362 | { 363 | gear_ratio_ = gear_ratio; 364 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[VescServoController]Gear ratio is set to %f", gear_ratio_); 365 | } 366 | 367 | void VescServoController::setTorqueConst(const double torque_const) 368 | { 369 | torque_const_ = torque_const; 370 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[VescServoController]Torque constant is set to %f", 371 | torque_const_); 372 | } 373 | 374 | void VescServoController::setRotorPoles(const int rotor_poles) 375 | { 376 | num_rotor_poles_ = rotor_poles; 377 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[VescServoController]The number of rotor pole is set to %d", 378 | num_rotor_poles_); 379 | } 380 | 381 | void VescServoController::setHallSensors(const int hall_sensors) 382 | { 383 | num_hall_sensors_ = hall_sensors; 384 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[VescServoController]The number of hall sensors is set to %d", 385 | num_hall_sensors_); 386 | } 387 | 388 | void VescServoController::setJointType(const int joint_type) 389 | { 390 | joint_type_ = joint_type; 391 | } 392 | 393 | void VescServoController::setScrewLead(const double screw_lead) 394 | { 395 | screw_lead_ = screw_lead; 396 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "[VescServoController]Screw lead is set to %f", screw_lead_); 397 | } 398 | 399 | double VescServoController::getZeroPosition() const 400 | { 401 | return zero_position_; 402 | } 403 | 404 | void VescServoController::spinSensorData() 405 | { 406 | if (rclcpp::ok() && node_) 407 | { 408 | rclcpp::spin_some(node_); 409 | } 410 | } 411 | 412 | double VescServoController::getPositionSens() 413 | { 414 | return sens_position_; 415 | } 416 | 417 | double VescServoController::getVelocitySens() 418 | { 419 | return sens_velocity_; 420 | } 421 | 422 | double VescServoController::getEffortSens() 423 | { 424 | return sens_effort_; 425 | } 426 | 427 | void VescServoController::executeCalibration() 428 | { 429 | calibration_flag_ = true; 430 | return; 431 | } 432 | 433 | bool VescServoController::calibrate() 434 | { 435 | if (!calibration_flag_) 436 | { 437 | return true; 438 | } 439 | // sends a command for calibration 440 | if (calibration_mode_ == CURRENT_) 441 | { 442 | auto sign = calibration_rewind_ ? -1.0 : 1.0; 443 | interface_ptr_->setCurrent(sign * calibration_current_); 444 | } 445 | else if (calibration_mode_ == DUTY_) 446 | { 447 | auto sign = calibration_rewind_ ? -1.0 : 1.0; 448 | interface_ptr_->setDutyCycle(sign * calibration_duty_); 449 | } 450 | else 451 | { 452 | RCLCPP_ERROR(rclcpp::get_logger("VescHwInterface"), "Please set the calibration mode surely"); 453 | return false; 454 | } 455 | 456 | if (calibration_rewind_) 457 | { 458 | if (std::fabs(calibration_position_ - sens_position_) > (upper_endstop_position_ - lower_endstop_position_) / 10.0) 459 | { 460 | calibration_current_ = calibration_strict_current_; 461 | calibration_duty_ = calibration_strict_duty_; 462 | calibration_rewind_ = false; 463 | } 464 | return false; 465 | } 466 | 467 | if (std::accumulate(endstop_deque_.begin(), endstop_deque_.end(), 0.0) != 0.0) 468 | { 469 | zero_position_ = sens_position_ + zero_position_ - calibration_position_; 470 | if ((calibration_mode_ == CURRENT_ && 471 | std::fabs(calibration_current_ - calibration_strict_current_) < std::numeric_limits::epsilon()) || 472 | (calibration_mode_ == DUTY_ && 473 | std::fabs(calibration_duty_ - calibration_strict_duty_) < std::numeric_limits::epsilon())) 474 | { 475 | target_position_ = calibration_position_; 476 | vesc_step_difference_.resetStepDifference(position_steps_); 477 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "Calibration Finished"); 478 | if (calibration_mode_ == CURRENT_) 479 | { 480 | interface_ptr_->setCurrent(0.0); 481 | } 482 | else if (calibration_mode_ == DUTY_) 483 | { 484 | interface_ptr_->setDutyCycle(0.0); 485 | } 486 | calibration_flag_ = false; 487 | return true; 488 | } 489 | else 490 | { 491 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "Calibrate with strict current/duty."); 492 | calibration_rewind_ = true; 493 | return false; 494 | } 495 | } 496 | 497 | calibration_steps_++; 498 | 499 | if (calibration_steps_ % 20 == 0) 500 | { 501 | if (std::abs(sens_position_ - calibration_previous_position_) <= std::numeric_limits::epsilon()) 502 | { 503 | // finishes calibrating 504 | calibration_steps_ = 0; 505 | zero_position_ = sens_position_ + zero_position_ - calibration_position_; 506 | target_position_ = calibration_position_; 507 | vesc_step_difference_.resetStepDifference(position_steps_); 508 | RCLCPP_INFO(rclcpp::get_logger("VescHwInterface"), "Calibration Finished"); 509 | if (calibration_mode_ == CURRENT_) 510 | { 511 | interface_ptr_->setCurrent(0.0); 512 | } 513 | else if (calibration_mode_ == DUTY_) 514 | { 515 | interface_ptr_->setDutyCycle(0.0); 516 | } 517 | calibration_flag_ = false; 518 | return true; 519 | } 520 | else 521 | { 522 | calibration_previous_position_ = sens_position_; 523 | return false; 524 | } 525 | } 526 | else 527 | { 528 | // continues calibration 529 | return false; 530 | } 531 | } 532 | 533 | // void VescServoController::controlTimerCallback(const ros::TimerEvent& e) 534 | // { 535 | // control(); 536 | // interface_ptr_->requestState(); 537 | // } 538 | 539 | void VescServoController::updateSensor(const std::shared_ptr& packet) 540 | { 541 | if (packet->getName() == "Values") 542 | { 543 | std::shared_ptr values = std::dynamic_pointer_cast(packet); 544 | const double current = values->getMotorCurrent(); 545 | const double velocity_rpm = values->getVelocityERPM() / static_cast(num_rotor_poles_ / 2); 546 | const int32_t steps = static_cast(values->getTachometer()); 547 | if (sensor_initialize_) 548 | { 549 | steps_previous_ = steps; 550 | sensor_initialize_ = false; 551 | } 552 | const int32_t steps_diff = steps - steps_previous_; 553 | position_steps_ += static_cast(steps_diff); 554 | steps_previous_ = steps; 555 | 556 | sens_position_ = position_steps_ / (num_hall_sensors_ * num_rotor_poles_) * gear_ratio_; // unit: revolution 557 | sens_velocity_ = velocity_rpm * gear_ratio_; // unit: rpm 558 | sens_effort_ = current * torque_const_ / gear_ratio_; 559 | 560 | if (joint_type_ == 0 || joint_type_ == 1) 561 | { 562 | sens_position_ = sens_position_ * 2.0 * M_PI; // unit: rad 563 | sens_velocity_ = sens_velocity_ / 60.0 * 2.0 * M_PI; // unit: rad/s 564 | } 565 | else if (joint_type_ == 2) 566 | { 567 | sens_position_ = sens_position_ * screw_lead_; // unit: m 568 | sens_velocity_ = sens_velocity_ / 60.0 * screw_lead_; // unit: m/s 569 | } 570 | 571 | sens_position_ -= getZeroPosition(); 572 | 573 | if (!calibration_result_path_.empty()) 574 | { 575 | std::ofstream file; 576 | file.open(calibration_result_path_, std::ios::out); 577 | file << "servo/last_position: " << sens_position_ << std::endl; 578 | file.close(); 579 | } 580 | } 581 | return; 582 | } 583 | 584 | void VescServoController::endstopCallback(const std_msgs::msg::Bool::ConstSharedPtr& msg) 585 | { 586 | endstop_deque_.pop_front(); 587 | if (!msg->data) 588 | { 589 | endstop_deque_.push_back(0); 590 | } 591 | else 592 | { 593 | if (calibration_flag_) 594 | { 595 | if (calibration_mode_ == CURRENT_) 596 | { 597 | if (std::signbit(calibration_current_)) 598 | { 599 | endstop_deque_.push_back(-1); 600 | } 601 | else 602 | { 603 | endstop_deque_.push_back(1); 604 | } 605 | } 606 | else if (calibration_mode_ == DUTY_) 607 | { 608 | if (std::signbit(calibration_duty_)) 609 | { 610 | endstop_deque_.push_back(-1); 611 | } 612 | else 613 | { 614 | endstop_deque_.push_back(1); 615 | } 616 | } 617 | } 618 | else if (std::fabs(sens_position_ - upper_endstop_position_) < std::fabs(sens_position_ - lower_endstop_position_)) 619 | { 620 | endstop_deque_.push_back(1); 621 | } 622 | else 623 | { 624 | endstop_deque_.push_back(-1); 625 | } 626 | } 627 | } 628 | 629 | VescServoController::CalibrationParameters VescServoController::getCalibrationParameters() const 630 | { 631 | CalibrationParameters params; 632 | params.calibration_position = calibration_position_; 633 | params.enable_calibration = calibration_flag_; 634 | return params; 635 | } 636 | } // namespace vesc_hw_interface 637 | --------------------------------------------------------------------------------