├── .github └── workflows │ ├── build_master.yml │ ├── build_pr.yml │ ├── cpp_format.yml │ └── pr_todo_checks.yml ├── .gitignore ├── CHANGELOG.md ├── CMakeLists.txt ├── CONTRIBUTING.md ├── README.md ├── demos ├── const_torque_control.cpp ├── const_torque_control.hpp ├── demo_1_motor.cpp ├── demo_1_motor_print_everything.cpp ├── demo_2_motors.cpp ├── demo_3_motors.cpp ├── demo_8_motors.cpp ├── demo_const_torque_1_motor.cpp ├── demo_leg.cpp ├── demo_print_analog_sensors.cpp ├── demo_sine_position_1_motor.cpp ├── demo_sine_torque_1_motor.cpp ├── demo_single_board.cpp ├── pd_control.cpp ├── pd_control.hpp ├── sine_position_control.cpp ├── sine_position_control.hpp ├── sine_torque_control.cpp └── sine_torque_control.hpp ├── doc ├── getting_started.rst ├── homing.md ├── images │ └── device_class_diagram.svg ├── install.rst └── main.dox ├── include └── blmc_drivers │ ├── blmc_joint_module.hpp │ ├── devices │ ├── analog_sensor.hpp │ ├── can_bus.hpp │ ├── device_interface.hpp │ ├── leg.hpp │ ├── motor.hpp │ └── motor_board.hpp │ └── utils │ ├── os_interface.hpp │ ├── polynome.hpp │ └── polynome.hxx ├── license.txt ├── package.xml ├── scripts └── initialize_can_bus.sh ├── src ├── analog_sensors.cpp ├── blmc_joint_module.cpp ├── can_bus.cpp ├── motor.cpp ├── motor_board.cpp ├── programs │ └── can_encoder_index_test.cpp └── utils │ └── polynome.cpp └── tests └── test_polynome.cpp /.github/workflows/build_master.yml: -------------------------------------------------------------------------------- 1 | # Build the package and run tests, using the latest trifinger_user Apptainer 2 | # image. 3 | # The Apptainer image has a full ROBOT_FINGERS workspace installed, so all 4 | # dependencies are there and only the checked package needs to be build, not the 5 | # whole workspace. 6 | # 7 | # Note: The Apptainer image only gets automatically rebuild, if something in the 8 | # image definition changes, not when other packages are changed. So to avoid 9 | # that the workspace in the container gets outdated, manual builds need to be 10 | # triggered regularly. 11 | 12 | name: Build and Test 13 | 14 | on: 15 | push: 16 | branches: 17 | - master 18 | 19 | jobs: 20 | build_and_test: 21 | runs-on: ubuntu-latest 22 | 23 | # Grant GITHUB_TOKEN the permissions required to make a Pages deployment 24 | permissions: 25 | pages: write # to deploy to Pages 26 | id-token: write # to verify the deployment originates from an appropriate source 27 | 28 | # Deploy to the github-pages environment 29 | environment: 30 | name: github-pages 31 | url: ${{ steps.doc.outputs.page_url }} 32 | 33 | steps: 34 | - name: Check out code 35 | uses: actions/checkout@v4 36 | 37 | - name: Setup Apptainer 38 | uses: open-dynamic-robot-initiative/trifinger-build-action/setup_apptainer@v2 39 | 40 | - name: Build 41 | uses: open-dynamic-robot-initiative/trifinger-build-action/build@v2 42 | 43 | - name: Documentation 44 | id: doc 45 | uses: open-dynamic-robot-initiative/trifinger-build-action/build_and_deploy_docs@v2 46 | 47 | - name: Tests 48 | uses: open-dynamic-robot-initiative/trifinger-build-action/run_tests@v2 49 | -------------------------------------------------------------------------------- /.github/workflows/build_pr.yml: -------------------------------------------------------------------------------- 1 | # Build the package and run tests, using the latest trifinger_user Apptainer 2 | # image. 3 | # The Apptainer image has a full ROBOT_FINGERS workspace installed, so all 4 | # dependencies are there and only the checked package needs to be build, not the 5 | # whole workspace. 6 | # 7 | # Note: The Apptainer image only gets automatically rebuild, if something in the 8 | # image definition changes, not when other packages are changed. So to avoid 9 | # that the workspace in the container gets outdated, manual builds need to be 10 | # triggered regularly. 11 | 12 | name: Build and Test 13 | 14 | on: pull_request 15 | 16 | jobs: 17 | build_and_test: 18 | runs-on: ubuntu-latest 19 | steps: 20 | - name: Check out code 21 | uses: actions/checkout@v4 22 | 23 | - name: Setup Apptainer 24 | uses: open-dynamic-robot-initiative/trifinger-build-action/setup_apptainer@v2 25 | 26 | - name: Build 27 | uses: open-dynamic-robot-initiative/trifinger-build-action/build@v2 28 | 29 | - name: Tests 30 | uses: open-dynamic-robot-initiative/trifinger-build-action/run_tests@v2 31 | -------------------------------------------------------------------------------- /.github/workflows/cpp_format.yml: -------------------------------------------------------------------------------- 1 | name: Linters 2 | 3 | on: pull_request 4 | 5 | jobs: 6 | format: 7 | name: C++ Formatting 8 | runs-on: ubuntu-latest 9 | steps: 10 | - uses: actions/checkout@v4 11 | - uses: actions/setup-python@v5 12 | - run: wget https://raw.githubusercontent.com/machines-in-motion/mpi_cmake_modules/master/scripts/run-clang-format 13 | - run: wget https://raw.githubusercontent.com/machines-in-motion/mpi_cmake_modules/master/resources/_clang-format 14 | - run: python ./run-clang-format -r . 15 | -------------------------------------------------------------------------------- /.github/workflows/pr_todo_checks.yml: -------------------------------------------------------------------------------- 1 | name: PR TODO checks 2 | 3 | on: pull_request 4 | 5 | jobs: 6 | fixmes: 7 | name: New FIXMEs 8 | runs-on: ubuntu-latest 9 | steps: 10 | - uses: actions/checkout@v4 11 | - name: Check for FIXMEs 12 | uses: luator/github_action_check_new_todos@v2 13 | with: 14 | label: FIXME 15 | base_ref: origin/${{ github.base_ref }} 16 | 17 | todos: 18 | name: New TODOs 19 | runs-on: ubuntu-latest 20 | steps: 21 | - uses: actions/checkout@v4 22 | - name: Check for TODOs 23 | uses: luator/github_action_check_new_todos@v2 24 | with: 25 | label: TODO 26 | base_ref: origin/${{ github.base_ref }} 27 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.swp 3 | build/ 4 | tags 5 | *.user 6 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | All notable changes to this project will be documented in this file. 3 | 4 | The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), 5 | and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). 6 | 7 | ## [Unreleased] 8 | ### Added 9 | - Static `MotorBoardStatus::get_error_description(uint8_t error_code)` to get a 10 | description for a given error code. 11 | 12 | ### Removed 13 | - `SerialReader` and the slider box scripts. Both have been moved to the 14 | [slider_box](https://github.com/open-dynamic-robot-initiative/slider_box) 15 | package. 16 | 17 | ### Changed 18 | - `MotorBoardStatus::get_error_description()` now returns a `std::string_view` 19 | to avoid dynamic memory allocation. 20 | 21 | 22 | ## [2.0.0] - 2021-08-04 23 | ### Removed 24 | - Remove master-board/SPI-related modules and the corresponding 25 | dependencies (#20, #21). 26 | 27 | ### Fixed 28 | - Make sure CAN frames are initialised to zero (#22). 29 | 30 | 31 | ## [1.0.0] - 2021-07-21 32 | Version 1.0.0 33 | 34 | 35 | [Unreleased]: https://github.com/open-dynamic-robot-initiative/blmc_drivers/compare/2.0.0...HEAD 36 | [2.0.0]: https://github.com/open-dynamic-robot-initiative/blmc_drivers/compare/1.0.0...2.0.0 37 | [1.0.0]: https://github.com/open-dynamic-robot-initiative/blmc_drivers/releases/tag/v1.0.0 38 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019, New York University and Max Planck Gesellschaft. 3 | # 4 | # License BSD-3 clause 5 | # 6 | 7 | # 8 | # set up the project 9 | # 10 | cmake_minimum_required(VERSION 3.10.2) 11 | 12 | project(blmc_drivers) 13 | 14 | # Using C++17 15 | if(NOT CMAKE_C_STANDARD) 16 | set(CMAKE_C_STANDARD 99) 17 | endif() 18 | if(NOT CMAKE_CXX_STANDARD) 19 | set(CMAKE_CXX_STANDARD 17) 20 | endif() 21 | 22 | # 23 | # Dependencies 24 | # 25 | 26 | # Depend on ament macros. 27 | find_package(ament_cmake REQUIRED) 28 | 29 | # Usual dependencies. 30 | find_package(mpi_cmake_modules REQUIRED) 31 | find_package(real_time_tools REQUIRED) 32 | find_package(time_series REQUIRED) 33 | find_package(Eigen3 REQUIRED) 34 | find_package(rt REQUIRED) 35 | find_package(Threads REQUIRED) 36 | # Check for xenomai as these drivers are xenomai compatible. 37 | find_package(Xenomai QUIET) 38 | 39 | # Export de dependencies. 40 | ament_export_dependencies(real_time_tools time_series Eigen3) 41 | 42 | # Prepare the final export. 43 | set(all_targets) 44 | set(all_target_exports) 45 | 46 | 47 | if(Xenomai_FOUND) 48 | add_definitions(${Xenomai_DEFINITIONS}) 49 | endif() 50 | 51 | # 52 | # Manage the creation of the library. 53 | # 54 | 55 | set(blmc_drivers_src 56 | src/analog_sensors.cpp 57 | src/blmc_joint_module.cpp 58 | src/can_bus.cpp 59 | src/motor_board.cpp 60 | src/motor.cpp 61 | src/utils/polynome.cpp 62 | ) 63 | 64 | # Create the library. 65 | add_library(blmc_drivers SHARED ${blmc_drivers_src}) 66 | 67 | # Add the include dependencies. 68 | target_include_directories( 69 | ${PROJECT_NAME} PUBLIC $ 70 | $) 71 | 72 | if(Xenomai_FOUND) 73 | target_include_directories(${PROJECT_NAME} PUBLIC ${Xenomai_INCLUDE_DIR}) 74 | endif() 75 | 76 | # Link the catkin dependencies. 77 | ament_target_dependencies(blmc_drivers rt) 78 | target_link_libraries(blmc_drivers real_time_tools::real_time_tools) 79 | target_link_libraries(blmc_drivers time_series::time_series) 80 | target_link_libraries(blmc_drivers Eigen3::Eigen) 81 | target_link_libraries(blmc_drivers Threads::Threads) 82 | 83 | # If on xenomai we need to link to the real time os librairies. 84 | if(Xenomai_FOUND) 85 | target_link_libraries(blmc_drivers ${Xenomai_LIBRARY_XENOMAI} 86 | ${Xenomai_LIBRARY_NATIVE} ${Xenomai_LIBRARY_RTDM}) 87 | endif() 88 | 89 | 90 | # For the installation 91 | list(APPEND all_targets ${PROJECT_NAME}) 92 | list(APPEND all_target_exports export_${PROJECT_NAME}) 93 | 94 | # 95 | # Manage exectuables 96 | # 97 | 98 | add_executable(can_encoder_index_test 99 | src/programs/can_encoder_index_test.cpp 100 | ) 101 | target_link_libraries(can_encoder_index_test 102 | ${PROJECT_NAME} 103 | ) 104 | list(APPEND all_targets can_encoder_index_test) 105 | 106 | 107 | # 108 | # Manage the demos. 109 | # 110 | 111 | macro(add_demo demo_name) 112 | add_executable( 113 | ${demo_name} 114 | demos/pd_control.cpp demos/sine_torque_control.cpp 115 | demos/sine_position_control.cpp demos/const_torque_control.cpp 116 | demos/${demo_name}.cpp) 117 | target_include_directories( 118 | ${demo_name} 119 | PUBLIC $ 120 | $ 121 | $) 122 | target_link_libraries(${demo_name} blmc_drivers) 123 | list(APPEND all_targets ${demo_name}) 124 | endmacro() 125 | 126 | add_demo(demo_sine_torque_1_motor) 127 | add_demo(demo_sine_position_1_motor) 128 | add_demo(demo_const_torque_1_motor) 129 | add_demo(demo_single_board) 130 | add_demo(demo_leg) 131 | add_demo(demo_2_motors) 132 | add_demo(demo_1_motor_print_everything) 133 | add_demo(demo_1_motor) 134 | add_demo(demo_8_motors) 135 | add_demo(demo_print_analog_sensors) 136 | 137 | # 138 | # Install the package. 139 | # 140 | 141 | # Install the script to initialize the can bus 142 | install( 143 | FILES ${PROJECT_SOURCE_DIR}/scripts/initialize_can_bus.sh 144 | RENAME initialize_can_bus 145 | DESTINATION lib/${PROJECT_NAME} 146 | PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE) 147 | 148 | install(DIRECTORY include/ DESTINATION include) 149 | 150 | install( 151 | TARGETS ${all_targets} 152 | EXPORT ${all_target_exports} 153 | LIBRARY DESTINATION lib 154 | ARCHIVE DESTINATION lib 155 | RUNTIME DESTINATION lib/${PROJECT_NAME} 156 | INCLUDES 157 | DESTINATION include) 158 | 159 | 160 | # 161 | # Tests 162 | # 163 | if (BUILD_TESTING) 164 | find_package(ament_cmake_gtest REQUIRED) 165 | 166 | ament_add_gtest(test_polynome 167 | tests/test_polynome.cpp 168 | ) 169 | target_include_directories(test_polynome PRIVATE 170 | $ 171 | $ 172 | ) 173 | target_link_libraries(test_polynome ${PROJECT_NAME}) 174 | 175 | endif() 176 | 177 | 178 | # 179 | # Building documentation. 180 | # 181 | add_documentation() 182 | 183 | # 184 | # Export as an ament_package. 185 | # 186 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 187 | ament_package() 188 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | How to Contribute 2 | ================= 3 | 4 | Bug reports are always welcome. Please open an issue on GitHub. 5 | 6 | If you want to contribute code to this package, please see the 7 | [TriFinger Contribution Guide](https://open-dynamic-robot-initiative.github.io/trifinger_docs/contribute.html). 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Brushless Motor Control Real-Time CAN API 2 | ========================================= 3 | 4 | This package provides a real-time API for communication with the motor control 5 | board and the OptoForce sensor via CAN. 6 | 7 | A description of the CAN protocol that is implemented by this library can be 8 | found in the documentation of the 9 | [mw_dual_motor_torque_ctrl](https://github.com/open-dynamic-robot-initiative/mw_dual_motor_torque_ctrl) 10 | firmware. 11 | 12 | This package contains a executable blmc_can_demo, that shows how to use the 13 | library. 14 | 15 | 16 | Dependencies and other Requirements 17 | ----------------------------------- 18 | 19 | - Supports Ubuntu patched with PREEMPT_RT patched kernel (see documentation on 20 | [how to install the PREEMPT_RT kernel](https://open-dynamic-robot-initiative.github.io/robot_interfaces/doc/real_time.html)). 21 | - Experimental support for Mac OS and Ubuntu patched with Xenomai 22 | 23 | See also the [real_time_tools](https://github.com/machines-in-motion/real_time_tools) package. 24 | 25 | A real-time CAN driver has to be loaded and the interface to be set up. This 26 | can be done with the script `start_rtcan` that is included in the [xenomai-core 27 | repository](https://git-amd.tuebingen.mpg.de/amd-clmc/xenomai-core). 28 | With PREEMPT_RT we also made good experience using the default driver, though. 29 | 30 | Note that the board has to run a program that implements the same CAN protocol. 31 | At the time of writing this README, this is only the case for 32 | [mw_dual_motor_torque_ctrl](https://github.com/open-dynamic-robot-initiative/mw_dual_motor_torque_ctrl). 33 | 34 | 35 | Build Instructions 36 | ------------------ 37 | 38 | Please see the [build instructions](https://open-dynamic-robot-initiative.github.io/blmc_drivers/doc/install.html) in the documentation. 39 | 40 | 41 | Links 42 | ----- 43 | 44 | - [Documentation](https://open-dynamic-robot-initiative.github.io/blmc_drivers/) 45 | - [Source Code](https://github.com/open-dynamic-robot-initiative/blmc_drivers) 46 | - [Bug Tracker](https://github.com/open-dynamic-robot-initiative/blmc_drivers/issues) 47 | 48 | 49 | 50 | Authors 51 | ------- 52 | 53 | - Felix Widmaier 54 | - Manuel Wuethrich 55 | - Maximilien Naveau 56 | - Steve Heim 57 | - Diego Agudelo 58 | - Julian Viereck 59 | 60 | 61 | License 62 | ------- 63 | 64 | BSD 3-Clause License 65 | 66 | Copyright(c) 2018 Max Planck Gesellschaft, New York University 67 | -------------------------------------------------------------------------------- /demos/const_torque_control.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file const_torque_control.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include "const_torque_control.hpp" 8 | #include 9 | #include "real_time_tools/spinner.hpp" 10 | #include "real_time_tools/timer.hpp" 11 | 12 | namespace blmc_drivers 13 | { 14 | void ConstTorqueControl::loop() 15 | { 16 | const int& blmc_position_index = MotorInterface::MeasurementIndex::position; 17 | const int& blmc_velocity_index = MotorInterface::MeasurementIndex::velocity; 18 | const int& blmc_current_index = MotorInterface::MeasurementIndex::current; 19 | // some data 20 | double actual_position = 0.0; 21 | double actual_velocity = 0.0; 22 | double actual_current = 0.0; 23 | // here is the control in current (Ampere) 24 | double desired_current = 0.0; 25 | 26 | real_time_tools::Spinner spinner; 27 | spinner.set_period(0.001); // here we spin every 1ms 28 | real_time_tools::Timer time_logger; 29 | size_t count = 0; 30 | while (!stop_loop_) 31 | { 32 | time_logger.tic(); 33 | 34 | // compute the control 35 | for (std::size_t i = 0; i < motor_list_.size(); ++i) 36 | { 37 | actual_position = motor_list_[i] 38 | ->get_measurement(blmc_position_index) 39 | ->newest_element(); 40 | actual_velocity = motor_list_[i] 41 | ->get_measurement(blmc_velocity_index) 42 | ->newest_element(); 43 | actual_current = motor_list_[i] 44 | ->get_measurement(blmc_current_index) 45 | ->newest_element(); 46 | 47 | // desired_current = 0.1; 48 | motor_list_[i]->set_current_target(desired_current); 49 | motor_list_[i]->send_if_input_changed(); 50 | 51 | encoders_[i].push_back(actual_position); 52 | velocities_[i].push_back(actual_velocity); 53 | currents_[i].push_back(actual_current); 54 | control_buffer_[i].push_back(desired_current); 55 | 56 | // we sleep here 1ms. 57 | spinner.spin(); 58 | // measure the time spent. 59 | time_logger.tac(); 60 | 61 | // Printings 62 | if ((count % 1000) == 0) 63 | { 64 | rt_printf("sending current: %f\n", desired_current); 65 | // time_logger.print_statistics(); 66 | } 67 | ++count; 68 | } // endfor 69 | } // endwhile 70 | time_logger.dump_measurements("/tmp/demo_pd_control_time_measurement"); 71 | } 72 | 73 | void ConstTorqueControl::stop_loop() 74 | { 75 | // dumping stuff 76 | std::string file_name = "/tmp/Const_torque_xp.dat"; 77 | try 78 | { 79 | std::ofstream log_file(file_name, std::ios::binary | std::ios::out); 80 | log_file.precision(10); 81 | 82 | assert(encoders_[0].size() == velocities_[0].size() && 83 | velocities_[0].size() == control_buffer_[0].size() && 84 | control_buffer_[0].size() == currents_[0].size()); 85 | for (std::size_t j = 0; j < encoders_[0].size(); ++j) 86 | { 87 | for (std::size_t i = 0; i < encoders_.size(); ++i) 88 | { 89 | log_file << encoders_[i][j] << " " << velocities_[i][j] << " " 90 | << control_buffer_[i][j] << " " << currents_[i][j] 91 | << " "; 92 | } 93 | log_file << std::endl; 94 | } 95 | 96 | log_file.flush(); 97 | log_file.close(); 98 | } 99 | catch (...) 100 | { 101 | rt_printf( 102 | "fstream Error in dump_tic_tac_measurements(): " 103 | "no time measurment saved\n"); 104 | } 105 | 106 | rt_printf("dumped the trajectory"); 107 | } 108 | 109 | } // namespace blmc_drivers -------------------------------------------------------------------------------- /demos/const_torque_control.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file const_torque_control.hpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include "blmc_drivers/devices/analog_sensor.hpp" 8 | #include "blmc_drivers/devices/motor.hpp" 9 | 10 | namespace blmc_drivers 11 | { 12 | /** 13 | * @brief This is a simple shortcut 14 | */ 15 | typedef std::shared_ptr SafeMotor_ptr; 16 | 17 | /** 18 | * @brief This is a basic PD controller to be used in the demos of this package. 19 | */ 20 | class ConstTorqueControl 21 | { 22 | public: 23 | /** 24 | * @brief Construct a new ConstTorqueControl object. 25 | * 26 | * @param motor_slider_pairs 27 | */ 28 | ConstTorqueControl(std::vector motor_list) 29 | : motor_list_(motor_list) 30 | { 31 | encoders_.clear(); 32 | velocities_.clear(); 33 | currents_.clear(); 34 | control_buffer_.clear(); 35 | 36 | for (std::size_t i = 0; i < motor_list.size(); ++i) 37 | { 38 | encoders_.push_back(std::deque()); 39 | currents_.push_back(std::deque()); 40 | velocities_.push_back(std::deque()); 41 | control_buffer_.push_back(std::deque()); 42 | encoders_.back().clear(); 43 | velocities_.back().clear(); 44 | currents_.back().clear(); 45 | control_buffer_.back().clear(); 46 | } 47 | stop_loop_ = false; 48 | } 49 | 50 | /** 51 | * @brief Destroy the ConstTorqueControl object 52 | */ 53 | ~ConstTorqueControl() 54 | { 55 | stop_loop_ = true; 56 | rt_thread_.join(); 57 | } 58 | 59 | /** 60 | * @brief This method is a helper to start the thread loop. 61 | */ 62 | void start_loop() 63 | { 64 | rt_thread_.create_realtime_thread(&ConstTorqueControl::loop, this); 65 | } 66 | 67 | /** 68 | * @brief Stop the control and dump the data 69 | */ 70 | void stop_loop(); 71 | 72 | private: 73 | /** 74 | * @brief This is list of motors 75 | */ 76 | std::vector motor_list_; 77 | /** 78 | * @brief This is the real time thread object. 79 | */ 80 | real_time_tools::RealTimeThread rt_thread_; 81 | 82 | /** 83 | * @brief this function is just a wrapper around the actual loop function, 84 | * such that it can be spawned as a posix thread. 85 | */ 86 | static THREAD_FUNCTION_RETURN_TYPE loop(void* instance_pointer) 87 | { 88 | ((ConstTorqueControl*)(instance_pointer))->loop(); 89 | return THREAD_FUNCTION_RETURN_VALUE; 90 | } 91 | 92 | /** 93 | * @brief this is a simple control loop which runs at a kilohertz. 94 | * 95 | * it reads the measurement from the analog sensor, in this case the 96 | * slider. then it scales it and sends it as the current target to 97 | * the motor. 98 | */ 99 | void loop(); 100 | 101 | /** 102 | * @brief managing the stopping of the loop 103 | */ 104 | bool stop_loop_; 105 | 106 | /** 107 | * @brief memory_buffer_size_ is the max size of the memory buffer. 108 | */ 109 | unsigned memory_buffer_size_; 110 | 111 | /** 112 | * @brief Encoder data 113 | */ 114 | std::vector > encoders_; 115 | 116 | /** 117 | * @brief Velocity data 118 | */ 119 | std::vector > velocities_; 120 | 121 | /** 122 | * @brief current data 123 | */ 124 | std::vector > currents_; 125 | 126 | /** 127 | * @brief control_buffer_ 128 | */ 129 | std::vector > control_buffer_; 130 | 131 | }; // end class ConstTorqueControl definition 132 | 133 | } // namespace blmc_drivers 134 | -------------------------------------------------------------------------------- /demos/demo_1_motor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file demo_1_motor.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include 8 | 9 | #include "blmc_drivers/devices/analog_sensor.hpp" 10 | #include "blmc_drivers/devices/can_bus.hpp" 11 | #include "blmc_drivers/devices/motor.hpp" 12 | #include "blmc_drivers/devices/motor_board.hpp" 13 | 14 | typedef std::tuple, 15 | std::shared_ptr> 16 | MotorAndSlider; 17 | 18 | struct Hardware 19 | { 20 | std::shared_ptr can_bus; 21 | std::shared_ptr motor_board; 22 | std::shared_ptr motor; 23 | std::shared_ptr slider; 24 | }; 25 | 26 | static THREAD_FUNCTION_RETURN_TYPE control_loop(void *hardware_ptr) 27 | { 28 | // cast input arguments to the right format -------------------------------- 29 | Hardware &hardware = *(static_cast(hardware_ptr)); 30 | 31 | // torque controller ------------------------------------------------------- 32 | real_time_tools::Spinner spinner; 33 | spinner.set_period(0.001); 34 | while (true) 35 | { 36 | // We send the current to the motor 37 | hardware.motor->set_current_target(/*desired_current*/ 0.0); 38 | hardware.motor->send_if_input_changed(); 39 | 40 | spinner.spin(); 41 | } 42 | } 43 | 44 | static THREAD_FUNCTION_RETURN_TYPE printing_loop(void *hardware_ptr) 45 | { 46 | // cast input arguments to the right format -------------------------------- 47 | Hardware &hardware = *(static_cast(hardware_ptr)); 48 | 49 | // print info -------------------------------------------------------------- 50 | long int timeindex = 51 | hardware.can_bus->get_output_frame()->newest_timeindex(); 52 | 53 | while (true) 54 | { 55 | long int received_timeindex = timeindex; 56 | // this will return the element with the index received_timeindex, 57 | // if this element does not exist anymore, it will return the oldest 58 | // element it still has and change received_timeindex to the appropriate 59 | // index. 60 | blmc_drivers::CanBusFrame can_frame = 61 | (*hardware.can_bus->get_output_frame())[received_timeindex]; 62 | timeindex++; 63 | 64 | rt_printf("timeindex: %ld\n", timeindex); 65 | can_frame.print(); 66 | } 67 | return THREAD_FUNCTION_RETURN_VALUE; 68 | } 69 | 70 | int main(int argc, char **argv) 71 | { 72 | if (argc != 3) 73 | { 74 | std::cout << "expected 2 arguments: can bus and motor index" 75 | << std::endl; 76 | exit(-1); 77 | } 78 | 79 | std::string can_bus_name(argv[1]); 80 | int motor_index = std::atoi(argv[2]); 81 | 82 | std::cout << "can_bus_name: " << can_bus_name 83 | << " motor_index: " << motor_index << std::endl; 84 | 85 | Hardware hardware; 86 | // First of all one need to initialize the communication with the can bus. 87 | hardware.can_bus = std::make_shared(can_bus_name); 88 | 89 | // Then we create a motor board object that will use the can bus in order 90 | // communicate between this application and the actual motor board. 91 | // Important: the blmc motors are alinged during this stage. 92 | hardware.motor_board = 93 | std::make_shared(hardware.can_bus); 94 | 95 | // create the motor object that have an index that define the port on which 96 | // they are plugged on the motor board. This object takes also a MotorBoard 97 | // object to be able to get the sensors and send the control consistantly. 98 | // These safe motors have the ability to bound the current that is given 99 | // as input. 100 | hardware.motor = std::make_shared( 101 | hardware.motor_board, motor_index); 102 | 103 | // start real-time control loop -------------------------------------------- 104 | real_time_tools::RealTimeThread control_thread; 105 | control_thread.create_realtime_thread(&control_loop, &hardware); 106 | 107 | // start real-time printing loop ------------------------------------------- 108 | real_time_tools::RealTimeThread printing_thread; 109 | printing_thread.create_realtime_thread(&printing_loop, &hardware); 110 | 111 | rt_printf("control loop started \n"); 112 | control_thread.join(); 113 | printing_thread.join(); 114 | return 0; 115 | } 116 | -------------------------------------------------------------------------------- /demos/demo_1_motor_print_everything.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file demo_1_motor_print_everything.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include 8 | 9 | #include "blmc_drivers/devices/analog_sensor.hpp" 10 | #include "blmc_drivers/devices/can_bus.hpp" 11 | #include "blmc_drivers/devices/motor.hpp" 12 | #include "blmc_drivers/devices/motor_board.hpp" 13 | 14 | typedef std::tuple, 15 | std::shared_ptr> 16 | MotorAndSlider; 17 | 18 | struct Hardware 19 | { 20 | std::shared_ptr can_bus; 21 | std::shared_ptr motor_board; 22 | std::shared_ptr motor; 23 | std::shared_ptr slider; 24 | }; 25 | 26 | static THREAD_FUNCTION_RETURN_TYPE control_loop(void* hardware_ptr) 27 | { 28 | // cast input arguments to the right format -------------------------------- 29 | Hardware& hardware = *(static_cast(hardware_ptr)); 30 | 31 | // torque controller ------------------------------------------------------- 32 | real_time_tools::Spinner spinner; 33 | spinner.set_period(0.001); 34 | while (true) 35 | { 36 | // The sliders are giving values between 0.0 and 1.0 37 | // double slider_position = 38 | // hardware.slider->get_measurement()->newest_element(); 39 | 40 | // We transform it into a current between -2.0 and 2.0 41 | // double desired_current = (slider_position - 0.5) * 4.0; 42 | 43 | // We send the current to the motor 44 | hardware.motor->set_current_target(/*desired_current*/ 0.0); 45 | hardware.motor->send_if_input_changed(); 46 | 47 | spinner.spin(); 48 | } 49 | } 50 | 51 | static THREAD_FUNCTION_RETURN_TYPE printing_loop(void* hardware_ptr) 52 | { 53 | // cast input arguments to the right format -------------------------------- 54 | Hardware& hardware = *(static_cast(hardware_ptr)); 55 | 56 | // print info -------------------------------------------------------------- 57 | long int timeindex = 58 | hardware.can_bus->get_output_frame()->newest_timeindex(); 59 | 60 | while (true) 61 | { 62 | long int received_timeindex = timeindex; 63 | // this will return the element with the index received_timeindex, 64 | // if this element does not exist anymore, it will return the oldest 65 | // element it still has and change received_timeindex to the appropriate 66 | // index. 67 | blmc_drivers::CanBusFrame can_frame = 68 | (*hardware.can_bus->get_output_frame())[received_timeindex]; 69 | timeindex++; 70 | 71 | rt_printf("timeindex: %ld\n", timeindex); 72 | can_frame.print(); 73 | } 74 | return THREAD_FUNCTION_RETURN_VALUE; 75 | } 76 | 77 | int main(int, char**) 78 | { 79 | Hardware hardware; 80 | // First of all one need to initialize the communication with the can bus. 81 | hardware.can_bus = std::make_shared("can0"); 82 | 83 | // Then we create a motor board object that will use the can bus in order 84 | // communicate between this application and the actual motor board. 85 | // Important: the blmc motors are alinged during this stage. 86 | hardware.motor_board = 87 | std::make_shared(hardware.can_bus); 88 | 89 | // create the motor object that have an index that define the port on which 90 | // they are plugged on the motor board. This object takes also a MotorBoard 91 | // object to be able to get the sensors and send the control consistantly. 92 | // These safe motors have the ability to bound the current that is given 93 | // as input. 94 | hardware.motor = 95 | std::make_shared(hardware.motor_board, 0); 96 | 97 | // create analogue sensors object which happens to be slider here 98 | hardware.slider = 99 | std::make_shared(hardware.motor_board, 0); 100 | 101 | // start real-time control loop -------------------------------------------- 102 | real_time_tools::RealTimeThread control_thread; 103 | control_thread.create_realtime_thread(&control_loop, &hardware); 104 | 105 | // start real-time printing loop ------------------------------------------- 106 | real_time_tools::RealTimeThread printing_thread; 107 | printing_thread.create_realtime_thread(&printing_loop, &hardware); 108 | 109 | rt_printf("control loop started \n"); 110 | control_thread.join(); 111 | printing_thread.join(); 112 | return 0; 113 | } 114 | -------------------------------------------------------------------------------- /demos/demo_2_motors.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file demo_2_motors.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | /** 13 | * @brief This boolean is here to kill cleanly the application upon ctrl+c 14 | */ 15 | std::atomic_bool StopDemos(false); 16 | 17 | /** 18 | * @brief This function is the callback upon a ctrl+c call from the terminal. 19 | * 20 | * @param s 21 | */ 22 | void my_handler(int) 23 | { 24 | StopDemos = true; 25 | } 26 | 27 | /** 28 | * @brief This is the main demo program. 29 | * 30 | * @param argc 31 | * @param argv 32 | * @return int 33 | */ 34 | int main(int, char **) 35 | { 36 | // make sure we catch the ctrl+c signal to kill the application properly. 37 | struct sigaction sigIntHandler; 38 | sigIntHandler.sa_handler = my_handler; 39 | sigemptyset(&sigIntHandler.sa_mask); 40 | sigIntHandler.sa_flags = 0; 41 | sigaction(SIGINT, &sigIntHandler, NULL); 42 | StopDemos = false; 43 | 44 | // First of all one need to initialize the communication with the can bus. 45 | auto can_bus = std::make_shared("can0"); 46 | 47 | // Then we create a motor board object that will use the can bus in order 48 | // communicate between this application and the actual motor board. 49 | // Important: the blmc motors are alinged during this stage. 50 | auto board = std::make_shared(can_bus); 51 | 52 | // create the motors object that have an index that define the port on which 53 | // they are plugged on the motor board. This object takes also a MotorBoard 54 | // object to be able to get the sensors and send the control consistantly. 55 | // These safe motors have the ability to bound the current that is given 56 | // as input. 57 | auto motor_1 = std::make_shared(board, 0); 58 | auto motor_2 = std::make_shared(board, 1); 59 | 60 | rt_printf("motors are set up \n"); 61 | 62 | // create 2 analogue sensors onject which happen to be slider here, i.e. 63 | // 2 linear potentiometers. 64 | auto slider_1 = std::make_shared(board, 0); 65 | auto slider_2 = std::make_shared(board, 1); 66 | 67 | rt_printf("sensors are set up \n"); 68 | 69 | // construct the pairs of motors and sliders 70 | std::vector motor_and_sliders; 71 | motor_and_sliders.push_back( 72 | blmc_drivers::PairMotorSlider(motor_1, slider_1)); 73 | motor_and_sliders.push_back( 74 | blmc_drivers::PairMotorSlider(motor_2, slider_2)); 75 | 76 | // construct a simple PD controller. 77 | blmc_drivers::PDController controller(motor_and_sliders); 78 | 79 | rt_printf("controllers are set up \n"); 80 | 81 | controller.start_loop(); 82 | 83 | rt_printf("loops have started \n"); 84 | 85 | // Wait until the application is killed. 86 | while (!StopDemos) 87 | { 88 | real_time_tools::Timer::sleep_sec(0.01); 89 | } 90 | return 0; 91 | } 92 | -------------------------------------------------------------------------------- /demos/demo_3_motors.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file demo_3_motors.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include "pd_control.hpp" 11 | 12 | /** 13 | * @brief This boolean is here to kill cleanly the application upon ctrl+c 14 | */ 15 | std::atomic_bool StopDemos(false); 16 | 17 | /** 18 | * @brief This function is the callback upon a ctrl+c call from the terminal. 19 | * 20 | * @param s 21 | */ 22 | void my_handler(int s) 23 | { 24 | StopDemos = true; 25 | } 26 | 27 | /** 28 | * @brief This is the main demo program. 29 | * 30 | * @param argc 31 | * @param argv 32 | * @return int 33 | */ 34 | int main(int argc, char **argv) 35 | { 36 | // make sure we catch the ctrl+c signal to kill the application properly. 37 | struct sigaction sigIntHandler; 38 | sigIntHandler.sa_handler = my_handler; 39 | sigemptyset(&sigIntHandler.sa_mask); 40 | sigIntHandler.sa_flags = 0; 41 | sigaction(SIGINT, &sigIntHandler, NULL); 42 | StopDemos = false; 43 | 44 | // First of all one need to initialize the communication with the can bus. 45 | auto can_bus1 = std::make_shared("can0"); 46 | auto can_bus2 = std::make_shared("can1"); 47 | 48 | // Then we create a motor board object that will use the can bus in order 49 | // communicate between this application and the actual motor board. 50 | // Important: the blmc motors are alinged during this stage. 51 | auto board1 = std::make_shared(can_bus1); 52 | auto board2 = std::make_shared(can_bus2); 53 | 54 | // create the motors object that have an index that define the port on which 55 | // they are plugged on the motor board. This object takes also a MotorBoard 56 | // object to be able to get the sensors and send the control consistantly. 57 | // These safe motors have the ability to bound the current that is given 58 | // as input. 59 | auto motor_1 = std::make_shared(board1, 0); 60 | auto motor_2 = std::make_shared(board1, 1); 61 | auto motor_3 = std::make_shared(board2, 0); 62 | 63 | rt_printf("motors are set up \n"); 64 | 65 | // create 2 analogue sensors onject which happen to be slider here, i.e. 66 | // 2 linear potentiometers. 67 | auto slider_1 = std::make_shared(board1, 0); 68 | auto slider_2 = std::make_shared(board1, 1); 69 | auto slider_3 = std::make_shared(board2, 0); 70 | 71 | rt_printf("sensors are set up \n"); 72 | 73 | // construct the pairs of motors and sliders 74 | std::vector motor_and_sliders; 75 | motor_and_sliders.push_back( 76 | blmc_drivers::PairMotorSlider(motor_1, slider_1)); 77 | motor_and_sliders.push_back( 78 | blmc_drivers::PairMotorSlider(motor_2, slider_2)); 79 | motor_and_sliders.push_back( 80 | blmc_drivers::PairMotorSlider(motor_3, slider_3)); 81 | 82 | // construct a simple PD controller. 83 | blmc_drivers::PDController controller(motor_and_sliders); 84 | 85 | rt_printf("controllers are set up \n"); 86 | 87 | controller.start_loop(); 88 | 89 | rt_printf("loops have started \n"); 90 | 91 | // Wait until the application is killed. 92 | while (!StopDemos) 93 | { 94 | real_time_tools::Timer::sleep_sec(0.01); 95 | } 96 | return 0; 97 | } 98 | -------------------------------------------------------------------------------- /demos/demo_8_motors.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file demo_8_motors.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | /** 13 | * @brief This boolean is here to kill cleanly the application upon ctrl+c 14 | */ 15 | std::atomic_bool StopDemos(false); 16 | 17 | /** 18 | * @brief This function is the callback upon a ctrl+c call from the terminal. 19 | * 20 | * @param s 21 | */ 22 | void my_handler(int) 23 | { 24 | StopDemos = true; 25 | } 26 | 27 | /** 28 | * @brief This is the main demo program. 29 | * 30 | * @param argc 31 | * @param argv 32 | * @return int 33 | */ 34 | int main(int, char **) 35 | { 36 | // make sure we catch the ctrl+c signal to kill the application properly. 37 | struct sigaction sigIntHandler; 38 | sigIntHandler.sa_handler = my_handler; 39 | sigemptyset(&sigIntHandler.sa_mask); 40 | sigIntHandler.sa_flags = 0; 41 | sigaction(SIGINT, &sigIntHandler, NULL); 42 | StopDemos = false; 43 | 44 | // First of all one need to initialize the communication with the can bus. 45 | auto can_bus1 = std::make_shared("can0"); 46 | auto can_bus2 = std::make_shared("can1"); 47 | auto can_bus3 = std::make_shared("can2"); 48 | auto can_bus4 = std::make_shared("can3"); 49 | 50 | // Then we create a motor board object that will use the can bus in order 51 | // communicate between this application and the actual motor board. 52 | // Important: the blmc motors are alinged during this stage. 53 | auto board1 = std::make_shared(can_bus1); 54 | auto board2 = std::make_shared(can_bus2); 55 | auto board3 = std::make_shared(can_bus3); 56 | auto board4 = std::make_shared(can_bus4); 57 | 58 | // create the motors object that have an index that define the port on which 59 | // they are plugged on the motor board. This object takes also a MotorBoard 60 | // object to be able to get the sensors and send the control consistantly. 61 | // These safe motors have the ability to bound the current that is given 62 | // as input. 63 | auto motor_1 = std::make_shared(board1, 0); 64 | auto motor_2 = std::make_shared(board1, 1); 65 | auto motor_3 = std::make_shared(board2, 0); 66 | auto motor_4 = std::make_shared(board2, 1); 67 | auto motor_5 = std::make_shared(board3, 0); 68 | auto motor_6 = std::make_shared(board3, 1); 69 | auto motor_7 = std::make_shared(board4, 0); 70 | auto motor_8 = std::make_shared(board4, 1); 71 | 72 | rt_printf("motors are set up \n"); 73 | 74 | // create the analogue sensors onject which happen to be slider here, i.e. 75 | // linear potentiometers. 76 | auto slider_1 = std::make_shared(board1, 0); 77 | auto slider_2 = std::make_shared(board1, 1); 78 | auto slider_3 = std::make_shared(board2, 0); 79 | auto slider_4 = std::make_shared(board2, 1); 80 | auto slider_5 = std::make_shared(board3, 0); 81 | auto slider_6 = std::make_shared(board3, 1); 82 | auto slider_7 = std::make_shared(board4, 0); 83 | auto slider_8 = std::make_shared(board4, 1); 84 | 85 | rt_printf("sensors are set up \n"); 86 | 87 | // construct the pairs of motors and sliders 88 | std::vector motor_and_sliders; 89 | motor_and_sliders.push_back( 90 | blmc_drivers::PairMotorSlider(motor_1, slider_1)); 91 | motor_and_sliders.push_back( 92 | blmc_drivers::PairMotorSlider(motor_2, slider_2)); 93 | motor_and_sliders.push_back( 94 | blmc_drivers::PairMotorSlider(motor_3, slider_3)); 95 | motor_and_sliders.push_back( 96 | blmc_drivers::PairMotorSlider(motor_4, slider_4)); 97 | motor_and_sliders.push_back( 98 | blmc_drivers::PairMotorSlider(motor_5, slider_5)); 99 | motor_and_sliders.push_back( 100 | blmc_drivers::PairMotorSlider(motor_6, slider_6)); 101 | motor_and_sliders.push_back( 102 | blmc_drivers::PairMotorSlider(motor_7, slider_7)); 103 | motor_and_sliders.push_back( 104 | blmc_drivers::PairMotorSlider(motor_8, slider_8)); 105 | 106 | // construct a simple PD controller. 107 | blmc_drivers::PDController controller(motor_and_sliders); 108 | 109 | rt_printf("controllers are set up \n"); 110 | 111 | controller.start_loop(); 112 | 113 | rt_printf("loops have started \n"); 114 | 115 | // Wait until the application is killed. 116 | while (!StopDemos) 117 | { 118 | real_time_tools::Timer::sleep_sec(0.01); 119 | } 120 | return 0; 121 | } 122 | -------------------------------------------------------------------------------- /demos/demo_const_torque_1_motor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file demo_const_torque_1_motor.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include "const_torque_control.hpp" 11 | 12 | /** 13 | * @brief This boolean is here to kill cleanly the application upon ctrl+c 14 | */ 15 | std::atomic_bool StopDemos(false); 16 | 17 | /** 18 | * @brief This function is the callback upon a ctrl+c call from the terminal. 19 | * 20 | * @param s 21 | */ 22 | void my_handler(int) 23 | { 24 | StopDemos = true; 25 | } 26 | 27 | /** 28 | * @brief This is the main demo program. 29 | * 30 | * @param argc 31 | * @param argv 32 | * @return int 33 | */ 34 | int main(int, char **) 35 | { 36 | // make sure we catch the ctrl+c signal to kill the application properly. 37 | struct sigaction sigIntHandler; 38 | sigIntHandler.sa_handler = my_handler; 39 | sigemptyset(&sigIntHandler.sa_mask); 40 | sigIntHandler.sa_flags = 0; 41 | sigaction(SIGINT, &sigIntHandler, NULL); 42 | StopDemos = false; 43 | 44 | // First of all one need to initialize the communication with the can bus. 45 | auto can_bus = std::make_shared("can0"); 46 | 47 | // Then we create a motor board object that will use the can bus in order 48 | // communicate between this application and the actual motor board. 49 | // Important: the blmc motors are alinged during this stage. 50 | auto board = std::make_shared(can_bus); 51 | 52 | // create the motors object that have an index that define the port on which 53 | // they are plugged on the motor board. This object takes also a MotorBoard 54 | // object to be able to get the sensors and send the control consistantly. 55 | // These safe motors have the ability to bound the current that is given 56 | // as input. 57 | auto motor_1 = std::make_shared(board, 0); 58 | auto motor_2 = std::make_shared(board, 1); 59 | 60 | rt_printf("motors are set up \n"); 61 | 62 | // construct the pairs of motors and sliders 63 | std::vector motor_list; 64 | motor_list.push_back(motor_1); 65 | motor_list.push_back(motor_2); 66 | 67 | // construct a simple PD controller. 68 | blmc_drivers::ConstTorqueControl controller(motor_list); 69 | 70 | rt_printf("controllers are set up \n"); 71 | 72 | controller.start_loop(); 73 | 74 | rt_printf("loops have started \n"); 75 | 76 | // Wait until the application is killed. 77 | while (!StopDemos) 78 | { 79 | real_time_tools::Timer::sleep_sec(0.01); 80 | } 81 | 82 | controller.stop_loop(); 83 | 84 | return 0; 85 | } 86 | -------------------------------------------------------------------------------- /demos/demo_leg.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file demo_leg.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "real_time_tools/spinner.hpp" 12 | #include "real_time_tools/timer.hpp" 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | /** 20 | * @brief This boolean is here to kill cleanly the application upon ctrl+c 21 | */ 22 | std::atomic_bool StopDemos(false); 23 | 24 | /** 25 | * @brief This function is the callback upon a ctrl+c call from the terminal. 26 | * 27 | * @param s 28 | */ 29 | void my_handler(int) 30 | { 31 | StopDemos = true; 32 | } 33 | 34 | /** 35 | * @brief This is a simple PD control on one motor and one slider 36 | */ 37 | class Controller 38 | { 39 | private: 40 | /** 41 | * @brief one motor. 42 | */ 43 | std::shared_ptr motor_; 44 | /** 45 | * @brief one slider. 46 | */ 47 | std::shared_ptr analog_sensor_; 48 | /** 49 | * @brief the realt time thread object. 50 | */ 51 | real_time_tools::RealTimeThread rt_thread_; 52 | 53 | public: 54 | /** 55 | * @brief Construct a new Controller object. 56 | * 57 | * @param motor 58 | * @param analog_sensor 59 | */ 60 | Controller(std::shared_ptr motor, 61 | std::shared_ptr analog_sensor) 62 | : motor_(motor), analog_sensor_(analog_sensor) 63 | { 64 | } 65 | /** 66 | * @brief main control loop 67 | */ 68 | void start_loop() 69 | { 70 | rt_thread_.create_realtime_thread(&Controller::loop, this); 71 | } 72 | 73 | /** 74 | * @brief this function is just a wrapper around the actual loop function, 75 | * such that it can be spawned as a posix thread. 76 | */ 77 | static THREAD_FUNCTION_RETURN_TYPE loop(void* instance_pointer) 78 | { 79 | ((Controller*)(instance_pointer))->loop(); 80 | return THREAD_FUNCTION_RETURN_VALUE; 81 | } 82 | 83 | private: 84 | /** 85 | * @brief this is a simple control loop which runs at a kilohertz. 86 | * 87 | * it reads the measurement from the analog sensor, in this case the 88 | * slider. then it scales it and sends it as the current target to 89 | * the motor. 90 | */ 91 | void loop() 92 | { 93 | real_time_tools::Spinner time_spinner; 94 | time_spinner.set_period(0.001); // 1kz loop 95 | size_t count = 0; 96 | while (true) 97 | { 98 | double analog_measurement = 99 | analog_sensor_->get_measurement()->newest_element(); 100 | double current_target = 4 * (analog_measurement - 0.5); 101 | 102 | motor_->set_current_target(current_target); 103 | motor_->send_if_input_changed(); 104 | 105 | // print ----------------------------------------------------------- 106 | time_spinner.spin(); 107 | if ((count % 1000) == 0) 108 | { 109 | rt_printf("sending current: %f\n", current_target); 110 | // time_logger.print_status(); 111 | } 112 | count++; 113 | } 114 | } 115 | }; 116 | 117 | /** 118 | * @brief Simple PD control on the leg 119 | */ 120 | class LegController 121 | { 122 | private: 123 | /** 124 | * @brief is the leg to control 125 | */ 126 | std::shared_ptr leg_; 127 | /** 128 | * @brief is the list of sliders 129 | */ 130 | std::shared_ptr analog_sensor_; 131 | /** 132 | * @brief is the real time thread object. 133 | */ 134 | real_time_tools::RealTimeThread rt_thread_; 135 | 136 | /** 137 | * @brief manages the shutdown of the controller 138 | */ 139 | bool stop_loop_; 140 | 141 | public: 142 | /** 143 | * @brief Construct a new LegController object 144 | * 145 | * @param leg 146 | * @param analog_sensor 147 | */ 148 | LegController(std::shared_ptr leg, 149 | std::shared_ptr analog_sensor) 150 | : leg_(leg), analog_sensor_(analog_sensor) 151 | { 152 | stop_loop_ = false; 153 | } 154 | 155 | /** 156 | * @brief Destroy the LegController object 157 | */ 158 | ~LegController() 159 | { 160 | stop_loop_ = true; 161 | rt_thread_.join(); 162 | } 163 | 164 | /** 165 | * @brief helper to strat the real time thread. 166 | */ 167 | void start_loop() 168 | { 169 | rt_thread_.create_realtime_thread(&Controller::loop, this); 170 | } 171 | 172 | /** 173 | * @brief this function is just a wrapper around the actual loop function, 174 | * such that it can be spawned as a posix thread. 175 | */ 176 | static THREAD_FUNCTION_RETURN_TYPE loop(void* instance_pointer) 177 | { 178 | ((LegController*)(instance_pointer))->loop(); 179 | return THREAD_FUNCTION_RETURN_VALUE; 180 | } 181 | 182 | private: 183 | /** 184 | * @brief this is a simple control loop which runs at a kilohertz. 185 | * 186 | * it reads the measurement from the analog sensor, in this case the 187 | * slider. then it scales it and sends it as the current target to 188 | * the motor. 189 | */ 190 | void loop() 191 | { 192 | real_time_tools::Spinner spinner; 193 | spinner.set_period(0.001); // 1kz loop 194 | size_t count = 0; 195 | while (!stop_loop_) 196 | { 197 | double analog_measurement = 198 | analog_sensor_->get_measurement()->newest_element(); 199 | double position_target = (analog_measurement - 0.5); 200 | 201 | double position_hip = 202 | leg_->get_motor_measurement(blmc_drivers::Leg::hip, 203 | blmc_drivers::Leg::position) 204 | ->newest_element(); 205 | double velocity_hip = 206 | leg_->get_motor_measurement(blmc_drivers::Leg::hip, 207 | blmc_drivers::Leg::velocity) 208 | ->newest_element(); 209 | 210 | double position_knee = 211 | leg_->get_motor_measurement(blmc_drivers::Leg::knee, 212 | blmc_drivers::Leg::position) 213 | ->newest_element(); 214 | double velocity_knee = 215 | leg_->get_motor_measurement(blmc_drivers::Leg::knee, 216 | blmc_drivers::Leg::velocity) 217 | ->newest_element(); 218 | 219 | double kp = 5; 220 | double kd = 1; 221 | double current_target_knee = 222 | kp * (position_target - position_knee) - kd * (velocity_knee); 223 | double current_target_hip = 224 | kp * (position_target - position_hip) - kd * (velocity_hip); 225 | 226 | if (current_target_knee > 1.0) 227 | { 228 | current_target_knee = 1.0; 229 | } 230 | else if (current_target_knee < -1.0) 231 | { 232 | current_target_knee = -1.0; 233 | } 234 | 235 | if (current_target_hip > 1.0) 236 | { 237 | current_target_hip = 1.0; 238 | } 239 | else if (current_target_hip < -1.0) 240 | { 241 | current_target_hip = -1.0; 242 | } 243 | 244 | leg_->set_current_target(current_target_knee, 245 | blmc_drivers::Leg::knee); 246 | leg_->set_current_target(current_target_hip, 247 | blmc_drivers::Leg::hip); 248 | leg_->send_if_input_changed(); 249 | 250 | // print ----------------------------------------------------------- 251 | spinner.spin(); 252 | if ((count % 1000) == 0) 253 | { 254 | rt_printf("sending current: %f\n", current_target_knee); 255 | // time_logger.print_status(); 256 | } 257 | count++; 258 | } 259 | } 260 | }; 261 | 262 | int main(int, char**) 263 | { 264 | // make sure we catch the ctrl+c signal to kill the application properly. 265 | struct sigaction sigIntHandler; 266 | sigIntHandler.sa_handler = my_handler; 267 | sigemptyset(&sigIntHandler.sa_mask); 268 | sigIntHandler.sa_flags = 0; 269 | sigaction(SIGINT, &sigIntHandler, NULL); 270 | StopDemos = false; 271 | 272 | // create bus and boards ------------------------------------------------- 273 | 274 | // this is the id of the cans bus (plug behind the computer). 275 | // see 276 | // https://atlas.is.localnet/confluence/pages/viewpage.action?pageId=44958260 277 | // use netstat -i repeatedly to see wich can bus value is changing. 278 | // normally labels behind the computer identify it. 279 | auto can_bus = std::make_shared("can3"); 280 | 281 | // the board is used to communicate with the can bus, it takes one upon 282 | // creation 283 | auto board = std::make_shared(can_bus); 284 | 285 | // create motors and sensors --------------------------------------------- 286 | auto motor_hip = std::make_shared(board, 0); 287 | auto motor_knee = std::make_shared(board, 1); 288 | 289 | auto leg = std::make_shared(motor_hip, motor_knee); 290 | rt_printf("leg is set up \n"); 291 | 292 | // on the board there is an analog input (slider) at slot 0 (of the analog 293 | // input) 294 | auto analog_sensor = std::make_shared(board, 0); 295 | // on the board there is an analog input (slider) at slot 0 (of the analog 296 | // input) 297 | // auto analog_sensor = std::make_shared(board, 298 | // 1); 299 | rt_printf("sensors are set up \n"); 300 | 301 | LegController leg_controller(leg, analog_sensor); 302 | rt_printf("controllers are set up \n"); 303 | 304 | leg_controller.start_loop(); 305 | rt_printf("loops have started \n"); 306 | 307 | while (!StopDemos) 308 | { 309 | real_time_tools::Timer::sleep_sec(0.001); 310 | } 311 | 312 | return 0; 313 | } 314 | -------------------------------------------------------------------------------- /demos/demo_print_analog_sensors.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file 3 | * @brief Print ADC measurements in a loop. 4 | * @copyright 2018-2020, New York University and Max Planck Gesellschaft, 5 | * License BSD-3-Clause 6 | */ 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | int main(int argc, char** argv) 16 | { 17 | if (argc != 2) 18 | { 19 | std::cerr << "Invalid number of arguments." << std::endl; 20 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 21 | return 1; 22 | } 23 | 24 | std::string can_port(argv[1]); 25 | 26 | // First of all one need to initialize the communication with the can bus. 27 | auto can_bus = std::make_shared(can_port); 28 | 29 | // Then we create a motor board object that will use the can bus in order 30 | // communicate between this application and the actual motor board. 31 | // Important: The BLMC motors are aligned during this stage. 32 | auto motor_board = 33 | std::make_shared(can_bus); 34 | 35 | // create analogue sensors objects to get measurements 36 | auto adc_a = std::make_shared(motor_board, 0); 37 | auto adc_b = std::make_shared(motor_board, 1); 38 | 39 | std::cout << std::endl; 40 | std::cout << "Printing measurements of analogue sensors 'ADC A' and 'ADC " 41 | "B'. Press Ctrl+C to exit." 42 | << std::endl; 43 | std::cout << std::endl; 44 | 45 | // print measurements in a loop 46 | real_time_tools::Spinner spinner; 47 | spinner.set_period(0.05); 48 | while (true) 49 | { 50 | double measurement_a = adc_a->get_measurement()->newest_element(); 51 | double measurement_b = adc_b->get_measurement()->newest_element(); 52 | 53 | std::cout << std::setprecision(4) << std::fixed 54 | << "\rADC A: " << measurement_a 55 | << " | ADC B: " << measurement_b << " " << std::flush; 56 | 57 | spinner.spin(); 58 | } 59 | 60 | return 0; 61 | } 62 | -------------------------------------------------------------------------------- /demos/demo_sine_position_1_motor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file demo_sine_position_1_motor.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include "sine_position_control.hpp" 11 | 12 | /** 13 | * @brief This boolean is here to kill cleanly the application upon ctrl+c 14 | */ 15 | std::atomic_bool StopDemos(false); 16 | 17 | /** 18 | * @brief This function is the callback upon a ctrl+c call from the terminal. 19 | * 20 | * @param s 21 | */ 22 | void my_handler(int) 23 | { 24 | StopDemos = true; 25 | } 26 | 27 | /** 28 | * @brief This is the main demo program. 29 | * 30 | * @param argc 31 | * @param argv 32 | * @return int 33 | */ 34 | int main(int, char **) 35 | { 36 | // make sure we catch the ctrl+c signal to kill the application properly. 37 | struct sigaction sigIntHandler; 38 | sigIntHandler.sa_handler = my_handler; 39 | sigemptyset(&sigIntHandler.sa_mask); 40 | sigIntHandler.sa_flags = 0; 41 | sigaction(SIGINT, &sigIntHandler, NULL); 42 | StopDemos = false; 43 | 44 | // First of all one need to initialize the communication with the can bus. 45 | auto can_bus = std::make_shared("can0"); 46 | 47 | // Then we create a motor board object that will use the can bus in order 48 | // communicate between this application and the actual motor board. 49 | // Important: the blmc motors are alinged during this stage. 50 | auto board = std::make_shared(can_bus); 51 | 52 | // create the motors object that have an index that define the port on which 53 | // they are plugged on the motor board. This object takes also a MotorBoard 54 | // object to be able to get the sensors and send the control consistantly. 55 | // These safe motors have the ability to bound the current that is given 56 | // as input. 57 | auto motor_1 = std::make_shared(board, 0); 58 | auto motor_2 = std::make_shared(board, 1); 59 | 60 | rt_printf("motors are set up \n"); 61 | 62 | // construct the pairs of motors and sliders 63 | std::vector motor_list; 64 | motor_list.push_back(motor_1); 65 | motor_list.push_back(motor_2); 66 | 67 | // construct a simple PD controller. 68 | blmc_drivers::SinePositionControl controller(motor_list); 69 | 70 | rt_printf("controllers are set up \n"); 71 | 72 | controller.start_loop(); 73 | 74 | rt_printf("loops have started \n"); 75 | 76 | // Wait until the application is killed. 77 | while (!StopDemos) 78 | { 79 | real_time_tools::Timer::sleep_sec(0.01); 80 | } 81 | 82 | controller.stop_loop(); 83 | 84 | return 0; 85 | } 86 | -------------------------------------------------------------------------------- /demos/demo_sine_torque_1_motor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file demo_sine_torque_1_motor.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include "sine_torque_control.hpp" 11 | 12 | /** 13 | * @brief This boolean is here to kill cleanly the application upon ctrl+c 14 | */ 15 | std::atomic_bool StopDemos(false); 16 | 17 | /** 18 | * @brief This function is the callback upon a ctrl+c call from the terminal. 19 | * 20 | * @param s 21 | */ 22 | void my_handler(int) 23 | { 24 | StopDemos = true; 25 | } 26 | 27 | /** 28 | * @brief This is the main demo program. 29 | * 30 | * @param argc 31 | * @param argv 32 | * @return int 33 | */ 34 | int main(int, char **) 35 | { 36 | // make sure we catch the ctrl+c signal to kill the application properly. 37 | struct sigaction sigIntHandler; 38 | sigIntHandler.sa_handler = my_handler; 39 | sigemptyset(&sigIntHandler.sa_mask); 40 | sigIntHandler.sa_flags = 0; 41 | sigaction(SIGINT, &sigIntHandler, NULL); 42 | StopDemos = false; 43 | 44 | // First of all one need to initialize the communication with the can bus. 45 | auto can_bus = std::make_shared("can0"); 46 | 47 | // Then we create a motor board object that will use the can bus in order 48 | // communicate between this application and the actual motor board. 49 | // Important: the blmc motors are alinged during this stage. 50 | auto board = std::make_shared(can_bus); 51 | 52 | // create the motors object that have an index that define the port on which 53 | // they are plugged on the motor board. This object takes also a MotorBoard 54 | // object to be able to get the sensors and send the control consistantly. 55 | // These safe motors have the ability to bound the current that is given 56 | // as input. 57 | auto motor_1 = std::make_shared(board, 0); 58 | auto motor_2 = std::make_shared(board, 1); 59 | 60 | rt_printf("motors are set up \n"); 61 | 62 | // construct the pairs of motors and sliders 63 | std::vector motor_list; 64 | motor_list.push_back(motor_1); 65 | motor_list.push_back(motor_2); 66 | 67 | // construct a simple PD controller. 68 | blmc_drivers::SineTorqueControl controller(motor_list); 69 | 70 | rt_printf("controllers are set up \n"); 71 | 72 | controller.start_loop(); 73 | 74 | rt_printf("loops have started \n"); 75 | 76 | // Wait until the application is killed. 77 | while (!StopDemos) 78 | { 79 | real_time_tools::Timer::sleep_sec(0.01); 80 | } 81 | 82 | controller.stop_loop(); 83 | 84 | return 0; 85 | } 86 | -------------------------------------------------------------------------------- /demos/demo_single_board.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file demo_single_board.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include "pd_control.hpp" 11 | 12 | /** 13 | * @brief This boolean is here to kill cleanly the application upon ctrl+c 14 | */ 15 | std::atomic_bool StopDemos(false); 16 | 17 | /** 18 | * @brief This function is the callback upon a ctrl+c call from the terminal. 19 | * 20 | * @param s 21 | */ 22 | void my_handler(int) 23 | { 24 | StopDemos = true; 25 | } 26 | 27 | /** 28 | * @brief This is the main demo program. 29 | * 30 | * @param argc 31 | * @param argv 32 | * @return int 33 | */ 34 | int main(int, char **) 35 | { 36 | // make sure we catch the ctrl+c signal to kill the application properly. 37 | struct sigaction sigIntHandler; 38 | sigIntHandler.sa_handler = my_handler; 39 | sigemptyset(&sigIntHandler.sa_mask); 40 | sigIntHandler.sa_flags = 0; 41 | sigaction(SIGINT, &sigIntHandler, NULL); 42 | StopDemos = false; 43 | 44 | // First of all one need to initialize the communication with the can bus. 45 | auto can_bus = std::make_shared("can0"); 46 | 47 | // Then we create a motor board object that will use the can bus in order 48 | // communicate between this application and the actual motor board. 49 | // Important: the blmc motors are alinged during this stage. 50 | auto board = std::make_shared(can_bus); 51 | 52 | // create the motors object that have an index that define the port on which 53 | // they are plugged on the motor board. This object takes also a MotorBoard 54 | // object to be able to get the sensors and send the control consistantly. 55 | // These safe motors have the ability to bound the current that is given 56 | // as input. 57 | auto motor_1 = std::make_shared(board, 0); 58 | 59 | rt_printf("motors are set up \n"); 60 | 61 | // create 2 analogue sensors onject which happen to be slider here, i.e. 62 | // 2 linear potentiometers. 63 | auto slider_1 = std::make_shared(board, 0); 64 | 65 | rt_printf("sensors are set up \n"); 66 | 67 | // construct the pairs of motors and sliders 68 | std::vector motor_and_sliders; 69 | motor_and_sliders.push_back( 70 | blmc_drivers::PairMotorSlider(motor_1, slider_1)); 71 | 72 | // construct a simple PD controller. 73 | blmc_drivers::PDController controller(motor_and_sliders); 74 | 75 | rt_printf("controllers are set up \n"); 76 | 77 | controller.start_loop(); 78 | 79 | rt_printf("loops have started \n"); 80 | 81 | // Wait until the application is killed. 82 | while (!StopDemos) 83 | { 84 | real_time_tools::Timer::sleep_sec(0.01); 85 | } 86 | return 0; 87 | } 88 | -------------------------------------------------------------------------------- /demos/pd_control.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file pd_control.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include "pd_control.hpp" 8 | #include "real_time_tools/spinner.hpp" 9 | #include "real_time_tools/timer.hpp" 10 | 11 | namespace blmc_drivers 12 | { 13 | void PDController::loop() 14 | { 15 | const int& blmc_position_index = MotorInterface::MeasurementIndex::position; 16 | const int& blmc_velocity_index = MotorInterface::MeasurementIndex::velocity; 17 | double slider_pose = 0.0; 18 | double desired_pose = 0.0; 19 | double actual_pose = 0.0; 20 | double actual_velocity = 0.0; 21 | double kp = 5; 22 | double kd = 1; 23 | // here is the control in current (Amper) 24 | double desired_current = 0.0; 25 | 26 | real_time_tools::Spinner spinner; 27 | spinner.set_period(0.001); // here we spin every 1ms 28 | real_time_tools::Timer time_logger; 29 | size_t count = 0; 30 | while (!stop_loop) 31 | { 32 | time_logger.tic(); 33 | 34 | // compute the control 35 | for (std::vector::iterator pair_it = 36 | motor_slider_pairs_.begin(); 37 | pair_it != motor_slider_pairs_.end(); 38 | ++pair_it) 39 | { 40 | SafeMotor_ptr motor = pair_it->first; 41 | Slider_ptr slider = pair_it->second; 42 | 43 | slider_pose = slider->get_measurement()->newest_element(); 44 | // The sliders are giving values between 0.0 and 1.0. 45 | desired_pose = (slider_pose - 0.5); 46 | actual_pose = 47 | motor->get_measurement(blmc_position_index)->newest_element(); 48 | actual_velocity = 49 | motor->get_measurement(blmc_velocity_index)->newest_element(); 50 | 51 | desired_current = 52 | kp * (desired_pose - actual_pose) - kd * (actual_velocity); 53 | 54 | if (desired_current > 1.0) 55 | { 56 | desired_current = 1.0; 57 | } 58 | else if (desired_current < -1.0) 59 | { 60 | desired_current = -1.0; 61 | } 62 | 63 | motor->set_current_target(desired_current); 64 | motor->send_if_input_changed(); 65 | 66 | // we sleep here 1ms. 67 | spinner.spin(); 68 | // measure the time spent. 69 | time_logger.tac(); 70 | 71 | // Printings 72 | if ((count % 1000) == 0) 73 | { 74 | rt_printf("sending current: %f\n", desired_current); 75 | // time_logger.print_statistics(); 76 | } 77 | ++count; 78 | } // endfor 79 | } // endwhile 80 | time_logger.dump_measurements("/tmp/demo_pd_control_time_measurement"); 81 | } 82 | 83 | } // namespace blmc_drivers -------------------------------------------------------------------------------- /demos/pd_control.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file pd_control.hpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include "blmc_drivers/devices/analog_sensor.hpp" 8 | #include "blmc_drivers/devices/motor.hpp" 9 | 10 | namespace blmc_drivers 11 | { 12 | /** 13 | * @brief This is a simple shortcut 14 | */ 15 | typedef std::shared_ptr SafeMotor_ptr; 16 | /** 17 | * @brief This is a simple shortcut 18 | */ 19 | typedef std::shared_ptr Slider_ptr; 20 | /** 21 | * @brief This is a simple shortcut 22 | */ 23 | typedef std::pair PairMotorSlider; 24 | 25 | /** 26 | * @brief This is a basic PD controller to be used in the demos of this package. 27 | */ 28 | class PDController 29 | { 30 | public: 31 | /** 32 | * @brief Construct a new PDController object. 33 | * 34 | * @param motor_slider_pairs 35 | */ 36 | PDController(std::vector motor_slider_pairs) 37 | : motor_slider_pairs_(motor_slider_pairs) 38 | { 39 | stop_loop = false; 40 | } 41 | 42 | /** 43 | * @brief Destroy the PDController object 44 | */ 45 | ~PDController() 46 | { 47 | stop_loop = true; 48 | rt_thread_.join(); 49 | } 50 | 51 | /** 52 | * @brief This method is a helper to start the thread loop. 53 | */ 54 | void start_loop() 55 | { 56 | rt_thread_.create_realtime_thread(&PDController::loop, this); 57 | } 58 | 59 | private: 60 | /** 61 | * @brief This is a pair of motor and sliders so that we associate one with 62 | * the other. 63 | */ 64 | std::vector motor_slider_pairs_; 65 | /** 66 | * @brief This is the real time thread object. 67 | */ 68 | real_time_tools::RealTimeThread rt_thread_; 69 | 70 | /** 71 | * @brief this function is just a wrapper around the actual loop function, 72 | * such that it can be spawned as a posix thread. 73 | */ 74 | static THREAD_FUNCTION_RETURN_TYPE loop(void* instance_pointer) 75 | { 76 | ((PDController*)(instance_pointer))->loop(); 77 | return THREAD_FUNCTION_RETURN_VALUE; 78 | } 79 | 80 | /** 81 | * @brief this is a simple control loop which runs at a kilohertz. 82 | * 83 | * it reads the measurement from the analog sensor, in this case the 84 | * slider. then it scales it and sends it as the current target to 85 | * the motor. 86 | */ 87 | void loop(); 88 | 89 | /** 90 | * @brief managing the stopping of the loop 91 | */ 92 | bool stop_loop; 93 | 94 | }; // end class PDController definition 95 | 96 | } // namespace blmc_drivers 97 | -------------------------------------------------------------------------------- /demos/sine_position_control.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file sine_position_control.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include "sine_position_control.hpp" 8 | #include 9 | #include "real_time_tools/spinner.hpp" 10 | #include "real_time_tools/timer.hpp" 11 | 12 | namespace blmc_drivers 13 | { 14 | void SinePositionControl::loop() 15 | { 16 | const int& blmc_position_index = MotorInterface::MeasurementIndex::position; 17 | const int& blmc_velocity_index = MotorInterface::MeasurementIndex::velocity; 18 | const int& blmc_current_index = MotorInterface::MeasurementIndex::current; 19 | // some data 20 | double actual_position = 0.0; 21 | double actual_velocity = 0.0; 22 | double actual_current = 0.0; 23 | double local_time = 0.0; 24 | double control_period = 0.001; 25 | // sine torque params 26 | double amplitude = 0.0 /*3.1415*/; 27 | double frequence = 0.5; 28 | // here is the control in current (Ampere) 29 | double desired_position = 0.0; 30 | double desired_velocity = 0.0; 31 | double desired_current = 0.0; 32 | 33 | real_time_tools::Spinner spinner; 34 | spinner.set_period(control_period); // here we spin every 1ms 35 | real_time_tools::Timer time_logger; 36 | size_t count = 0; 37 | while (!stop_loop_) 38 | { 39 | time_logger.tic(); 40 | local_time = count * control_period; 41 | 42 | // compute the control 43 | for (size_t i = 0; i < motor_list_.size(); ++i) 44 | { 45 | actual_position = motor_list_[i] 46 | ->get_measurement(blmc_position_index) 47 | ->newest_element(); 48 | actual_velocity = motor_list_[i] 49 | ->get_measurement(blmc_velocity_index) 50 | ->newest_element(); 51 | actual_current = motor_list_[i] 52 | ->get_measurement(blmc_current_index) 53 | ->newest_element(); 54 | 55 | desired_position = 56 | amplitude * sin(2 * M_PI * frequence * local_time); 57 | desired_velocity = 0.0 /* 2 * M_PI * frequence * amplitude * 58 | cos(2 * M_PI * frequence * local_time)*/ 59 | ; 60 | desired_current = kp_ * (desired_position - actual_position) + 61 | kd_ * (desired_velocity - actual_velocity); 62 | motor_list_[i]->set_current_target(desired_current); 63 | } 64 | // Send the controls and log stuff 65 | 66 | for (size_t i = 0; i < motor_list_.size(); ++i) 67 | { 68 | motor_list_[i]->send_if_input_changed(); 69 | 70 | encoders_[i].push_back(actual_position); 71 | velocities_[i].push_back(actual_velocity); 72 | currents_[i].push_back(actual_current); 73 | control_buffer_[i].push_back(desired_current); 74 | } 75 | 76 | // we sleep here 1ms. 77 | spinner.spin(); 78 | // measure the time spent. 79 | time_logger.tac(); 80 | 81 | // Printings 82 | if ((count % (int)(0.2 / control_period)) == 0) 83 | { 84 | rt_printf("\33[H\33[2J"); // clear screen 85 | for (size_t i = 0; i < motor_list_.size(); ++i) 86 | { 87 | rt_printf("des_pose: %8f ; ", desired_position); 88 | motor_list_[i]->print(); 89 | } 90 | time_logger.print_statistics(); 91 | fflush(stdout); 92 | } 93 | ++count; 94 | 95 | } // endwhile 96 | time_logger.dump_measurements("/tmp/demo_pd_control_time_measurement"); 97 | } 98 | 99 | void SinePositionControl::stop_loop() 100 | { 101 | // dumping stuff 102 | std::string file_name = "/tmp/sine_position_xp.dat"; 103 | try 104 | { 105 | std::ofstream log_file(file_name, std::ios::binary | std::ios::out); 106 | log_file.precision(10); 107 | 108 | assert(encoders_[0].size() == velocities_[0].size() && 109 | velocities_[0].size() == control_buffer_[0].size() && 110 | control_buffer_[0].size() == currents_[0].size()); 111 | for (size_t j = 0; j < encoders_[0].size(); ++j) 112 | { 113 | for (size_t i = 0; i < encoders_.size(); ++i) 114 | { 115 | log_file << encoders_[i][j] << " " << velocities_[i][j] << " " 116 | << control_buffer_[i][j] << " " << currents_[i][j] 117 | << " "; 118 | } 119 | log_file << std::endl; 120 | } 121 | 122 | log_file.flush(); 123 | log_file.close(); 124 | } 125 | catch (...) 126 | { 127 | rt_printf( 128 | "fstream Error in dump_tic_tac_measurements(): " 129 | "no time measurment saved\n"); 130 | } 131 | 132 | rt_printf("dumped the trajectory"); 133 | } 134 | 135 | } // namespace blmc_drivers -------------------------------------------------------------------------------- /demos/sine_position_control.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file sine_position_control.hpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include "blmc_drivers/devices/analog_sensor.hpp" 8 | #include "blmc_drivers/devices/motor.hpp" 9 | 10 | namespace blmc_drivers 11 | { 12 | /** 13 | * @brief This is a simple shortcut 14 | */ 15 | typedef std::shared_ptr SafeMotor_ptr; 16 | 17 | /** 18 | * @brief This is a basic PD controller to be used in the demos of this package. 19 | */ 20 | class SinePositionControl 21 | { 22 | public: 23 | /** 24 | * @brief Construct a new SinePositionControl object. 25 | * 26 | * @param motor_slider_pairs 27 | */ 28 | SinePositionControl(std::vector motor_list) 29 | : motor_list_(motor_list) 30 | { 31 | encoders_.clear(); 32 | velocities_.clear(); 33 | currents_.clear(); 34 | control_buffer_.clear(); 35 | 36 | for (size_t i = 0; i < motor_list.size(); ++i) 37 | { 38 | encoders_.push_back(std::deque()); 39 | currents_.push_back(std::deque()); 40 | velocities_.push_back(std::deque()); 41 | control_buffer_.push_back(std::deque()); 42 | encoders_.back().clear(); 43 | velocities_.back().clear(); 44 | currents_.back().clear(); 45 | control_buffer_.back().clear(); 46 | } 47 | stop_loop_ = false; 48 | kp_ = 0.0; 49 | kd_ = 0.0; 50 | } 51 | 52 | /** 53 | * @brief Destroy the SinePositionControl object 54 | */ 55 | ~SinePositionControl() 56 | { 57 | stop_loop_ = true; 58 | rt_thread_.join(); 59 | } 60 | 61 | /** 62 | * @brief This method is a helper to start the thread loop. 63 | */ 64 | void start_loop() 65 | { 66 | rt_thread_.create_realtime_thread(&SinePositionControl::loop, this); 67 | } 68 | 69 | /** 70 | * @brief Stop the control and dump the data 71 | */ 72 | void stop_loop(); 73 | 74 | void set_gains(double kp, double kd) 75 | { 76 | kp_ = kp; 77 | kd_ = kd; 78 | } 79 | 80 | private: 81 | /** 82 | * @brief This is list of motors 83 | */ 84 | std::vector motor_list_; 85 | /** 86 | * @brief This is the real time thread object. 87 | */ 88 | real_time_tools::RealTimeThread rt_thread_; 89 | 90 | /** 91 | * @brief this function is just a wrapper around the actual loop function, 92 | * such that it can be spawned as a posix thread. 93 | */ 94 | static THREAD_FUNCTION_RETURN_TYPE loop(void* instance_pointer) 95 | { 96 | ((SinePositionControl*)(instance_pointer))->loop(); 97 | return THREAD_FUNCTION_RETURN_VALUE; 98 | } 99 | 100 | /** 101 | * @brief this is a simple control loop which runs at a kilohertz. 102 | * 103 | * it reads the measurement from the analog sensor, in this case the 104 | * slider. then it scales it and sends it as the current target to 105 | * the motor. 106 | */ 107 | void loop(); 108 | 109 | /** 110 | * @brief managing the stopping of the loop 111 | */ 112 | bool stop_loop_; 113 | 114 | /** 115 | * @brief memory_buffer_size_ is the max size of the memory buffer. 116 | */ 117 | unsigned memory_buffer_size_; 118 | 119 | /** 120 | * @brief Encoder data 121 | */ 122 | std::vector > encoders_; 123 | 124 | /** 125 | * @brief Velocity data 126 | */ 127 | std::vector > velocities_; 128 | 129 | /** 130 | * @brief current data 131 | */ 132 | std::vector > currents_; 133 | 134 | /** 135 | * @brief control_buffer_ 136 | */ 137 | std::vector > control_buffer_; 138 | 139 | /** 140 | * @brief Controller proportional gain. 141 | */ 142 | double kp_; 143 | 144 | /** 145 | * @brief Controller derivative gain. 146 | */ 147 | double kd_; 148 | 149 | }; // end class SinePositionControl definition 150 | 151 | } // namespace blmc_drivers 152 | -------------------------------------------------------------------------------- /demos/sine_torque_control.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file sine_torque_control.cpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include "sine_torque_control.hpp" 8 | #include 9 | #include "real_time_tools/spinner.hpp" 10 | #include "real_time_tools/timer.hpp" 11 | 12 | namespace blmc_drivers 13 | { 14 | void SineTorqueControl::loop() 15 | { 16 | const int& blmc_position_index = MotorInterface::MeasurementIndex::position; 17 | const int& blmc_velocity_index = MotorInterface::MeasurementIndex::velocity; 18 | const int& blmc_current_index = MotorInterface::MeasurementIndex::current; 19 | // some data 20 | double actual_position = 0.0; 21 | double actual_velocity = 0.0; 22 | double actual_current = 0.0; 23 | double local_time = 0.0; 24 | // sine torque params 25 | double current_amplitude = 0.0; 26 | double current_period = 2.0; 27 | double current_pulsation = 2 * 3.1415 / current_period; 28 | // here is the control in current (Ampere) 29 | double desired_current = 0.0; 30 | 31 | real_time_tools::Spinner spinner; 32 | spinner.set_period(0.001); // here we spin every 1ms 33 | real_time_tools::Timer time_logger; 34 | size_t count = 0; 35 | while (!stop_loop_) 36 | { 37 | time_logger.tic(); 38 | local_time = count * 0.001; 39 | 40 | // compute the control 41 | for (unsigned int i = 0; i < motor_list_.size(); ++i) 42 | { 43 | actual_position = motor_list_[i] 44 | ->get_measurement(blmc_position_index) 45 | ->newest_element(); 46 | actual_velocity = motor_list_[i] 47 | ->get_measurement(blmc_velocity_index) 48 | ->newest_element(); 49 | actual_current = motor_list_[i] 50 | ->get_measurement(blmc_current_index) 51 | ->newest_element(); 52 | 53 | desired_current = 54 | current_amplitude * sin(current_pulsation * local_time); 55 | 56 | motor_list_[i]->set_current_target(desired_current); 57 | motor_list_[i]->send_if_input_changed(); 58 | 59 | encoders_[i].push_back(actual_position); 60 | velocities_[i].push_back(actual_velocity); 61 | currents_[i].push_back(actual_current); 62 | control_buffer_[i].push_back(desired_current); 63 | 64 | // we sleep here 1ms. 65 | spinner.spin(); 66 | // measure the time spent. 67 | time_logger.tac(); 68 | 69 | // Printings 70 | if ((count % 1000) == 0) 71 | { 72 | rt_printf("sending current: %f\n", desired_current); 73 | // time_logger.print_statistics(); 74 | } 75 | ++count; 76 | } // endfor 77 | } // endwhile 78 | time_logger.dump_measurements("/tmp/demo_pd_control_time_measurement"); 79 | } 80 | 81 | void SineTorqueControl::stop_loop() 82 | { 83 | // dumping stuff 84 | std::string file_name = "/tmp/sine_torque_xp.dat"; 85 | try 86 | { 87 | std::ofstream log_file(file_name, std::ios::binary | std::ios::out); 88 | log_file.precision(10); 89 | 90 | assert(encoders_[0].size() == velocities_[0].size() && 91 | velocities_[0].size() == control_buffer_[0].size() && 92 | control_buffer_[0].size() == currents_[0].size()); 93 | for (unsigned int j = 0; j < encoders_[0].size(); ++j) 94 | { 95 | for (unsigned int i = 0; i < encoders_.size(); ++i) 96 | { 97 | log_file << encoders_[i][j] << " " << velocities_[i][j] << " " 98 | << control_buffer_[i][j] << " " << currents_[i][j] 99 | << " "; 100 | } 101 | log_file << std::endl; 102 | } 103 | 104 | log_file.flush(); 105 | log_file.close(); 106 | } 107 | catch (...) 108 | { 109 | rt_printf( 110 | "fstream Error in dump_tic_tac_measurements(): " 111 | "no time measurment saved\n"); 112 | } 113 | 114 | rt_printf("dumped the trajectory"); 115 | } 116 | 117 | } // namespace blmc_drivers -------------------------------------------------------------------------------- /demos/sine_torque_control.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file sine_torque_control.hpp 3 | * @copyright Copyright (c) 2018-2020, New York University and Max Planck 4 | * Gesellschaft, License BSD-3-Clause 5 | */ 6 | 7 | #include "blmc_drivers/devices/analog_sensor.hpp" 8 | #include "blmc_drivers/devices/motor.hpp" 9 | 10 | namespace blmc_drivers 11 | { 12 | /** 13 | * @brief This is a simple shortcut 14 | */ 15 | typedef std::shared_ptr SafeMotor_ptr; 16 | 17 | /** 18 | * @brief This is a basic PD controller to be used in the demos of this package. 19 | */ 20 | class SineTorqueControl 21 | { 22 | public: 23 | /** 24 | * @brief Construct a new SineTorqueControl object. 25 | * 26 | * @param motor_slider_pairs 27 | */ 28 | SineTorqueControl(std::vector motor_list) 29 | : motor_list_(motor_list) 30 | { 31 | encoders_.clear(); 32 | velocities_.clear(); 33 | currents_.clear(); 34 | control_buffer_.clear(); 35 | 36 | for (unsigned int i = 0; i < motor_list.size(); ++i) 37 | { 38 | encoders_.push_back(std::deque()); 39 | currents_.push_back(std::deque()); 40 | velocities_.push_back(std::deque()); 41 | control_buffer_.push_back(std::deque()); 42 | encoders_.back().clear(); 43 | velocities_.back().clear(); 44 | currents_.back().clear(); 45 | control_buffer_.back().clear(); 46 | } 47 | stop_loop_ = false; 48 | } 49 | 50 | /** 51 | * @brief Destroy the SineTorqueControl object 52 | */ 53 | ~SineTorqueControl() 54 | { 55 | stop_loop_ = true; 56 | rt_thread_.join(); 57 | } 58 | 59 | /** 60 | * @brief This method is a helper to start the thread loop. 61 | */ 62 | void start_loop() 63 | { 64 | rt_thread_.create_realtime_thread(&SineTorqueControl::loop, this); 65 | } 66 | 67 | /** 68 | * @brief Stop the control and dump the data 69 | */ 70 | void stop_loop(); 71 | 72 | private: 73 | /** 74 | * @brief This is list of motors 75 | */ 76 | std::vector motor_list_; 77 | /** 78 | * @brief This is the real time thread object. 79 | */ 80 | real_time_tools::RealTimeThread rt_thread_; 81 | 82 | /** 83 | * @brief this function is just a wrapper around the actual loop function, 84 | * such that it can be spawned as a posix thread. 85 | */ 86 | static THREAD_FUNCTION_RETURN_TYPE loop(void* instance_pointer) 87 | { 88 | ((SineTorqueControl*)(instance_pointer))->loop(); 89 | return THREAD_FUNCTION_RETURN_VALUE; 90 | } 91 | 92 | /** 93 | * @brief this is a simple control loop which runs at a kilohertz. 94 | * 95 | * it reads the measurement from the analog sensor, in this case the 96 | * slider. then it scales it and sends it as the current target to 97 | * the motor. 98 | */ 99 | void loop(); 100 | 101 | /** 102 | * @brief managing the stopping of the loop 103 | */ 104 | bool stop_loop_; 105 | 106 | /** 107 | * @brief memory_buffer_size_ is the max size of the memory buffer. 108 | */ 109 | unsigned memory_buffer_size_; 110 | 111 | /** 112 | * @brief Encoder data 113 | */ 114 | std::vector > encoders_; 115 | 116 | /** 117 | * @brief Velocity data 118 | */ 119 | std::vector > velocities_; 120 | 121 | /** 122 | * @brief current data 123 | */ 124 | std::vector > currents_; 125 | 126 | /** 127 | * @brief control_buffer_ 128 | */ 129 | std::vector > control_buffer_; 130 | 131 | }; // end class SineTorqueControl definition 132 | 133 | } // namespace blmc_drivers 134 | -------------------------------------------------------------------------------- /doc/getting_started.rst: -------------------------------------------------------------------------------- 1 | *************** 2 | Getting Started 3 | *************** 4 | 5 | Please see the several demos_ on how to use the library. 6 | 7 | 8 | .. _demos: https://github.com/open-dynamic-robot-initiative/blmc_drivers/tree/master/demos 9 | -------------------------------------------------------------------------------- /doc/homing.md: -------------------------------------------------------------------------------- 1 | # Homing (Joint Position Calibration) 2 | 3 | ## What is "Homing" and Why is it Needed? 4 | 5 | When using relative encoders (like the quadrature encoders, we are using), we 6 | only get relative position changes (e.g. "motor moved by 13 degree"). By 7 | accumulating these changes, the position can be computed. However, when turning 8 | the robot on, the motor controller does not know the absolute position of the 9 | motor so it just defines the current position to be zero. This means that while 10 | the position is consistent while the robot is running, the same physical 11 | position can get different position values when the robot is turned off in 12 | between. 13 | 14 | To solve this problem, the so called "homing" is performed in the beginning when 15 | turning the robot on. The idea is to search for a fixed physical position that 16 | can always be found, independent of where the joint is located when turned on 17 | (e.g. by having a switch that is triggered when the joint gets at that 18 | position). Once this "home position" is found, it can be set as zero position. 19 | This way, the same physical position will always correspond to the same position 20 | value, even if the robot is turned off between runs. 21 | 22 | ## How is the Homing Implemented for the BLMC Robots? 23 | 24 | In the `BlmcJointModule[s]` class, the following procedure is implemented: 25 | 26 | 1. From the current position move slowly in one direction (depending on 27 | parameters) until the next occurrence of the encoder index is reached. 28 | 2. Set this position as home position. 29 | 3. Set the zero position to home_position + home_offset. 30 | 31 | The _encoder index_ is a special tick on the encoder wheel that appears only 32 | once per motor revolution. This means once the encoder index is found, the 33 | absolute position of the motor is known. However, there is typically a gear box 34 | so that multiple motor revolutions are needed for one joint revolution. So 35 | while the motor position is now known, the absolute position of the joint is 36 | still unknown as there are multiple occurrences of the encoder index within the 37 | range of the joint. 38 | 39 | To solve this issue, it needs to be ensured that the "correct" encoder index is 40 | found. For robots with joint end-stops (e.g. the Finger robot) this is done by 41 | first moving in one direction until the end-stop is reached (detected by waiting 42 | for the velocity to become zero). Then the encoder index search is started from 43 | the end-stop position, thus guaranteeing that always the same index is found. 44 | For robots without end-stops (e.g. Solo) this can be solved by first moving the 45 | robot manually to a defined position before starting the homing. 46 | 47 | ### Home Position vs Zero Position – Meaning of the Home Offset 48 | 49 | The home position is typically given by the mechanics (e.g. based on where the 50 | encoder index is located) and can not be influenced by the user. This position 51 | is often not the one where the user wants to have the zero position. To place 52 | the zero at a different position, the "home offset" parameter can be used. Once 53 | the home position is found, the zero is set to 54 | 55 | zero_position = home_position + home_offset 56 | 57 | This means, the zero can be placed wherever is best for the application by 58 | simply setting the home offset accordingly. 59 | 60 | ## How to Determine the Home Offset 61 | 62 | To determine the desired home offset value (e.g. when setting up a new robot), 63 | simply follow these steps: 64 | 65 | 1. First set the home offset to zero. 66 | 2. Start the robot and perform the homing. 67 | 3. Manually move the robot to the desired zero position and print the position 68 | of the joints (after homing these positions are now relative to the home 69 | position). 70 | 4. Set the joint positions of the desired zero position as home offset. 71 | 72 | When restarting now, the actual zero position after homing should be at the 73 | desired one. 74 | -------------------------------------------------------------------------------- /doc/install.rst: -------------------------------------------------------------------------------- 1 | Build Instructions 2 | ================== 3 | 4 | Dependencies 5 | ------------ 6 | 7 | We are using colcon_ as build tool and typically use ``ros2 run`` to execute our 8 | applications. While we are not really depending on any ROS_ packages, this 9 | means a basic ROS 2 installation is recommended. 10 | 11 | We are testing on Ubuntu 20.04 with ROS Foxy. Other versions may work as well 12 | but are not officially supported. 13 | 14 | 15 | Get the Source 16 | -------------- 17 | 18 | **blmc_drivers** depends on several other of our packages which are 19 | organized in separate repositories. We therefore use a workspace management 20 | tool called treep_ which allows easy cloning of multi-repository projects. 21 | 22 | treep can be installed via pip:: 23 | 24 | pip install treep 25 | 26 | Clone the treep configuration containing the "BLMC_DRIVERS" project:: 27 | 28 | git clone git@github.com:machines-in-motion/treep_machines_in_motion.git 29 | 30 | **Note:** treep searches for a configuration directory from the current working 31 | directory upwards. So you can use treep in the directory in which you invoked 32 | the ``git clone`` command above or any subdirectory. 33 | 34 | Now clone the project:: 35 | 36 | treep --clone BLMC_DRIVERS 37 | 38 | **Important:** treep uses SSH to clone from github. So for the above command to 39 | work, you need a github account with a registered SSH key. Further this key 40 | needs to work without asking for a password everytime. To achieve this, run 41 | 42 | :: 43 | 44 | ssh-add 45 | 46 | first. 47 | 48 | 49 | Build 50 | ----- 51 | 52 | To build, cd into the ``workspace`` directory that was created by treep. If not 53 | done already, source the ROS setup:: 54 | 55 | source /opt/ros/foxy/setup.bash 56 | 57 | Then build with:: 58 | 59 | colcon build 60 | 61 | 62 | Build Documentation 63 | ~~~~~~~~~~~~~~~~~~~ 64 | 65 | If you want to build this documentation locally, run 66 | 67 | :: 68 | 69 | colcon build --cmake-args -DGENERATE_DOCUMENTATION=ON 70 | 71 | 72 | The documentation can then be found in 73 | ``/install//share//docs`` 74 | 75 | 76 | .. _colcon: https://colcon.readthedocs.io/en/released/index.html 77 | .. _ROS: https://www.ros.org 78 | .. _treep: https://pypi.org/project/treep/ 79 | -------------------------------------------------------------------------------- /doc/main.dox: -------------------------------------------------------------------------------- 1 | /*! \mainpage This is the documentation of the blmc_drivers package. 2 | 3 | This package is contains the drivers for the brushless 4 | motors developped in the Max Planck Institute for Intelligent System. 5 | The BLMC stands for BrushLess Motor 6 | Control. 7 | 8 | This package provides guidelines how a device should 9 | be implemented (see also the DeviceInterface class). 10 | Any device has a number of inputs and outputs, see 11 | the following diagram for an example with two inputs and outputs. 12 | @image html device_class_diagram.svg 13 | generally, we expect the following functions to be implemented: 14 | - a set function for each input (several inputs may share a set function 15 | which takes an index argument). 16 | - a send_if_input_changed() function which will send the inputs to the 17 | device if any of them have changed. 18 | - functions to access the current inputs and outputs, as well as the 19 | inputs which have been sent to the device. Rather than just returning 20 | the latest elements, these function should return a time series 21 | of these objects, such that the user can synchronize (e.g. wait for 22 | the next element or step through them one by one such that none of them is 23 | missed) 24 | 25 | This package provide a very simple API in order to acquire the sensors and send 26 | the controls to the actuators. 27 | Example of use of this package can be seens in different demos 28 | (PROJECT_SOURCE_DIR/demos) or in different unittests 29 | (PROJECT_SOURCE_DIR/tests). 30 | The demos contains the documentation inside the code. The unittests should be 31 | simple enough to understand wihtout additionnal documentation. 32 | 33 | \todo Manuel, can you explain how the blmc Can cards work? associated with the motor_boards? Thanks 34 | 35 | */ 36 | -------------------------------------------------------------------------------- /include/blmc_drivers/devices/analog_sensor.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file analog_sensor.hpp 3 | * @license License BSD-3-Clause 4 | * @copyright Copyright (c) 2019, New York University and Max Planck 5 | * Gesellschaft. 6 | * @date 2019-07-11 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include "real_time_tools/timer.hpp" 16 | 17 | #include "blmc_drivers/devices/device_interface.hpp" 18 | #include "blmc_drivers/devices/motor_board.hpp" 19 | 20 | namespace blmc_drivers 21 | { 22 | /** 23 | * @brief AnalogSensorInterface class is a simple abstract interface for using 24 | * blmc analog measurements. 25 | */ 26 | class AnalogSensorInterface : public DeviceInterface 27 | { 28 | public: 29 | /** 30 | * @brief This is just a short cut for the time series types 31 | */ 32 | typedef time_series::TimeSeries ScalarTimeseries; 33 | 34 | /** 35 | * @brief Get the measurement object which is the list of time stamped data. 36 | * 37 | * @return std::shared_ptr which is a pointer to the 38 | * a list of time stamped data 39 | */ 40 | virtual std::shared_ptr get_measurement() const = 0; 41 | 42 | /** 43 | * @brief Destroy the AnalogSensorInterface object 44 | * 45 | */ 46 | virtual ~AnalogSensorInterface() 47 | { 48 | } 49 | }; 50 | 51 | /** 52 | * @brief AnalogSensor class is the implementation of the above interface. 53 | */ 54 | class AnalogSensor : public AnalogSensorInterface 55 | { 56 | public: 57 | /** 58 | * @brief Construct a new AnalogSensor object 59 | * 60 | * @param board is a motor board which gives access to the motor sensors 61 | * (position, velocity, current, etc) and to the motor cotrols. 62 | * @param sensor_id is the id of the sensor on the control board 63 | */ 64 | AnalogSensor(std::shared_ptr board, bool sensor_id); 65 | 66 | /** 67 | * @brief Get the measurement object which is the list of time stamped data. 68 | * 69 | * @return std::shared_ptr which is a pointer to the 70 | * a list of time stamped data 71 | */ 72 | virtual std::shared_ptr get_measurement() const; 73 | 74 | private: 75 | /** 76 | * @brief board_ is the measurement object, it caontains the list 77 | * of the timed stamped data 78 | */ 79 | std::shared_ptr board_; 80 | 81 | /** 82 | * @brief sensor_id_ is the identification number of the sensor on the 83 | * control board, for now it is either 0 or 1 84 | */ 85 | size_t sensor_id_; 86 | }; 87 | 88 | } // namespace blmc_drivers 89 | -------------------------------------------------------------------------------- /include/blmc_drivers/devices/can_bus.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file can_bus.hpp 3 | * @license License BSD-3-Clause 4 | * @copyright Copyright (c) 2019, New York University and Max Planck 5 | * Gesellschaft. 6 | * @date 2019-07-11 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include "blmc_drivers/devices/device_interface.hpp" 23 | #include "blmc_drivers/utils/os_interface.hpp" 24 | 25 | namespace blmc_drivers 26 | { 27 | /** 28 | * @brief CanBusFrame is a class that contains a fixed sized amount of data 29 | * to be send or received via the can bus 30 | */ 31 | class CanBusFrame 32 | { 33 | public: 34 | /** 35 | * @brief data is the acutal data to be sent/received. 36 | */ 37 | std::array data; 38 | /** 39 | * @brief dlc is the size of the message. 40 | */ 41 | uint8_t dlc; 42 | /** 43 | * @brief id is the id number return by the CAN bus. 44 | */ 45 | can_id_t id; 46 | 47 | void print() const 48 | { 49 | rt_printf("---------------------------\n"); 50 | rt_printf("can bus frame data"); 51 | for (auto& datum : data) 52 | { 53 | rt_printf(" :%d", datum); 54 | } 55 | rt_printf("\n"); 56 | 57 | rt_printf("id: %d\n", id); 58 | 59 | rt_printf("dlc: %d\n", dlc); 60 | 61 | rt_printf("---------------------------\n"); 62 | } 63 | }; 64 | 65 | /** 66 | * @brief CanBusConnection is a data structure that contains the hardware 67 | * details for the connection between to can cards. 68 | */ 69 | class CanBusConnection 70 | { 71 | public: 72 | /** 73 | * @brief send_addr is the ip address where to send the the messages. 74 | */ 75 | struct sockaddr_can send_addr; 76 | /** 77 | * @brief socket is the port through which the messages will be processed 78 | */ 79 | int socket; 80 | }; 81 | 82 | /** 83 | * @brief CanBusInterface is an abstract class that defines an API for the 84 | * communication via Can bus. 85 | */ 86 | class CanBusInterface : public DeviceInterface 87 | { 88 | public: 89 | /** 90 | * @brief Destroy the CanBusInterface object 91 | */ 92 | virtual ~CanBusInterface() 93 | { 94 | } 95 | 96 | /** 97 | * @brief CanframeTimeseries is a simple sohortcut 98 | */ 99 | typedef time_series::TimeSeries CanframeTimeseries; 100 | 101 | /** 102 | * getters 103 | */ 104 | 105 | /** 106 | * @brief Get the output frame 107 | * 108 | * @return std::shared_ptr 109 | */ 110 | virtual std::shared_ptr get_output_frame() 111 | const = 0; 112 | 113 | /** 114 | * @brief Get the input frame 115 | * 116 | * @return std::shared_ptr 117 | */ 118 | virtual std::shared_ptr get_input_frame() = 0; 119 | 120 | /** 121 | * @brief Get the sent input frame 122 | * 123 | * @return std::shared_ptr 124 | */ 125 | virtual std::shared_ptr 126 | get_sent_input_frame() = 0; 127 | 128 | /** 129 | * setters 130 | */ 131 | 132 | /** 133 | * @brief Set the input frame saves the input frame to be sent in a queue. 134 | * 135 | * @param input_frame 136 | */ 137 | virtual void set_input_frame(const CanBusFrame& input_frame) = 0; 138 | 139 | /** 140 | * Sender 141 | */ 142 | 143 | /** 144 | * @brief send all the input frame to the can network 145 | */ 146 | virtual void send_if_input_changed() = 0; 147 | }; 148 | 149 | /** 150 | * @brief CanBus is the implementation of the CanBusInterface. 151 | */ 152 | class CanBus : public CanBusInterface 153 | { 154 | public: 155 | /** 156 | * @brief Construct a new CanBus object 157 | * 158 | * @param can_interface_name 159 | * @param history_length 160 | */ 161 | CanBus(const std::string& can_interface_name, 162 | const size_t& history_length = 1000); 163 | 164 | /** 165 | * @brief Destroy the CanBus object 166 | */ 167 | virtual ~CanBus(); 168 | 169 | /** 170 | * Getters 171 | */ 172 | 173 | /** 174 | * @brief Get the output frame 175 | * 176 | * @return std::shared_ptr 177 | */ 178 | std::shared_ptr get_output_frame() const 179 | { 180 | return output_; 181 | } 182 | 183 | /** 184 | * @brief Get the input frame 185 | * 186 | * @return std::shared_ptr 187 | */ 188 | virtual std::shared_ptr get_input_frame() 189 | { 190 | return input_; 191 | } 192 | 193 | /** 194 | * @brief Get the input frame thas has been sent 195 | * 196 | * @return std::shared_ptr 197 | */ 198 | virtual std::shared_ptr get_sent_input_frame() 199 | { 200 | return sent_input_; 201 | } 202 | 203 | /** 204 | * @brief Setters 205 | */ 206 | 207 | /** 208 | * @brief Set the input frame 209 | * 210 | * @param input_frame 211 | */ 212 | virtual void set_input_frame(const CanBusFrame& input_frame) 213 | { 214 | input_->append(input_frame); 215 | } 216 | 217 | /** 218 | * @brief Sender 219 | */ 220 | 221 | /** 222 | * @brief Send the queue of message to the can network 223 | */ 224 | virtual void send_if_input_changed(); 225 | 226 | /** 227 | * private attributes and methods 228 | */ 229 | private: 230 | /** 231 | * @brief This function is an helper that allows us to launch real-time 232 | * thread in xenaomai, ubunt, or rt-preempt seemlessly. 233 | * 234 | * @param instance_pointer 235 | * @return THREAD_FUNCTION_RETURN_TYPE (is void or void* depending on the 236 | * OS. 237 | */ 238 | static THREAD_FUNCTION_RETURN_TYPE loop(void* instance_pointer) 239 | { 240 | ((CanBus*)(instance_pointer))->loop(); 241 | return THREAD_FUNCTION_RETURN_VALUE; 242 | } 243 | 244 | /** 245 | * @brief Execute the communication loop with the can bus 246 | */ 247 | void loop(); 248 | 249 | /** 250 | * @brief Send input data 251 | * 252 | * @param unstamped_can_frame is a frame without id nor time. 253 | */ 254 | void send_frame(const CanBusFrame& unstamped_can_frame); 255 | 256 | /** 257 | * @brief Get the output frame from the bus 258 | * 259 | * @return CanBusFrame is the output frame data. 260 | */ 261 | CanBusFrame receive_frame(); 262 | 263 | /** 264 | * @brief Setup and initialize the CanBus object. 265 | * It connects to the can bus. This method is used once in the constructor. 266 | * 267 | * @param name is the can card name. 268 | * @param err_mask, always used with "0" so far (TODO: Manuel explain) 269 | * @return CanBusConnection 270 | */ 271 | CanBusConnection setup_can(std::string name, uint32_t err_mask); 272 | 273 | /** 274 | * Attributes 275 | */ 276 | 277 | /** 278 | * @brief can_connection_ is the communication object allowing to send or 279 | * receive can frames. 280 | */ 281 | real_time_tools::SingletypeThreadsafeObject 282 | can_connection_; 283 | 284 | /** 285 | * @brief input_ is a list of time stamped frame to be send to the can 286 | * network. 287 | */ 288 | std::shared_ptr > input_; 289 | 290 | /** 291 | * @brief sent_inupt_ is the list of the input already sent to the network. 292 | */ 293 | std::shared_ptr > sent_input_; 294 | 295 | /** 296 | * @brief output_ is the list of the frames received from the can network. 297 | */ 298 | std::shared_ptr > output_; 299 | 300 | /** 301 | * @brief This boolean makes sure that the loop is not active upon 302 | * destruction of the current object 303 | */ 304 | bool is_loop_active_; 305 | 306 | /** 307 | * @brief rt_thread_ is the thread object allowing us to spawn real-time 308 | * threads. 309 | */ 310 | real_time_tools::RealTimeThread rt_thread_; 311 | 312 | /** 313 | * @brief Log directory. 314 | */ 315 | std::string log_dir_; 316 | 317 | /** 318 | * @brief time_log_name is the name of the loggin 319 | */ 320 | std::string name_; 321 | }; 322 | 323 | } // namespace blmc_drivers 324 | -------------------------------------------------------------------------------- /include/blmc_drivers/devices/device_interface.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file device_interface.hpp 3 | * @license License BSD-3-Clause 4 | * @copyright Copyright (c) 2019, New York University and Max Planck 5 | * Gesellschaft. 6 | * @date 2019-07-11 7 | */ 8 | 9 | #pragma once 10 | 11 | /** 12 | * @brief This namespace is the standard namespace of the package. 13 | */ 14 | namespace blmc_drivers 15 | { 16 | /** 17 | * @brief this class exists purely for logical reasons, it does not in 18 | * itself implement anything. 19 | * 20 | * the purpose of this class is to provide guidelines how a device should 21 | * be implemented. any device has a number of inputs and outputs, see 22 | * the following diagram for an example with two inputs and outputs. 23 | * @image html device_class_diagram.svg 24 | * generally, we expect the following functions to be implemented: 25 | * - a set function for each input (several inputs may share a set function 26 | * which takes an index argument). 27 | * - a send_if_input_changed() function which will send the inputs to the 28 | * device if any of them have changed. 29 | * - functions to access the current inputs and outputs, as well as the 30 | * inputs which have been sent to the device. Rather than just returning 31 | * the latest elements, these function should return a time series 32 | * of these objects, such that the user can synchronize (e.g. wait for 33 | * the next element or step through them one by one such that none of them is 34 | * missed) 35 | */ 36 | class DeviceInterface 37 | { 38 | }; 39 | 40 | } // namespace blmc_drivers 41 | -------------------------------------------------------------------------------- /include/blmc_drivers/devices/leg.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file leg.hpp 3 | * @license License BSD-3-Clause 4 | * @copyright Copyright (c) 2019, New York University and Max Planck 5 | * Gesellschaft. 6 | * @date 2019-07-11 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | namespace blmc_drivers 21 | { 22 | /** 23 | * @brief This class defines an interface to control a leg. 24 | * This legg is composed of 2 motor, one for the hip and one for the knee. 25 | */ 26 | class LegInterface : public DeviceInterface 27 | { 28 | public: 29 | /** 30 | * @brief ScalarTimeseries is a simple shortcut for more intelligible code. 31 | */ 32 | typedef time_series::TimeSeries ScalarTimeseries; 33 | 34 | /** 35 | * @brief This is a shortcut for creating shared pointer in a simpler 36 | * writting expression. 37 | * 38 | * @tparam Type is the template paramer of the shared pointer. 39 | */ 40 | template 41 | using Ptr = std::shared_ptr; 42 | 43 | /** 44 | * @brief MotorMeasurementIndexing this enum allow to access the different 45 | * kind of sensor measurements in an understandable way in the code. 46 | */ 47 | enum MotorMeasurementIndexing 48 | { 49 | current, 50 | position, 51 | velocity, 52 | encoder_index, 53 | motor_measurement_count 54 | }; 55 | 56 | /** 57 | * @brief This enum list the motors in the leg 58 | */ 59 | enum MotorIndexing 60 | { 61 | hip, 62 | knee, 63 | motor_count 64 | }; 65 | 66 | /** 67 | * @brief Destroy the LegInterface object 68 | */ 69 | virtual ~LegInterface() 70 | { 71 | } 72 | 73 | /** 74 | * Getters 75 | */ 76 | 77 | /** 78 | * @brief Get the device output 79 | * 80 | * @param[in] motor_index designate the motor from which we want the data 81 | * from. 82 | * @param[in] measurement_index is teh kind of data we are looking for. 83 | * @return Ptr is the list of the lasts time 84 | * stamped acquiered. 85 | */ 86 | virtual Ptr get_motor_measurement( 87 | const int& motor_index, const int& measurement_index) const = 0; 88 | 89 | /** 90 | * @brief Get the actual target current 91 | * 92 | * @param[in] motor_index designate the motor from which we want the data 93 | * from. 94 | * @return Ptr is the list of the lasts time 95 | * stamped acquiered. 96 | */ 97 | virtual Ptr get_current_target( 98 | const int& motor_index) const = 0; 99 | 100 | /** 101 | * @brief Get the last sent target current. 102 | * 103 | * @param[in] motor_index designate the motor from which we want the data 104 | * from. 105 | * @return Ptr is the list of the lasts time 106 | * stamped acquiered. 107 | */ 108 | virtual Ptr get_sent_current_target( 109 | const int& motor_index) const = 0; 110 | 111 | /** 112 | * Setters 113 | */ 114 | 115 | /** 116 | * @brief Set the current target saves internally the desired current. This 117 | * data is not send to the motor yet. Please call send_if_input_changed in 118 | * order to actually send the data to the card. 119 | * 120 | * @param current_target is the current to achieve on the motor card. 121 | * @param motor_index is the motor to control. 122 | */ 123 | virtual void set_current_target(const double& current_target, 124 | const int& motor_index) = 0; 125 | 126 | /** 127 | * Sender 128 | */ 129 | 130 | /** 131 | * @brief Actually send the target current to the motor cards. 132 | */ 133 | virtual void send_if_input_changed() = 0; 134 | }; 135 | 136 | /** 137 | * @brief The leg class is the implementation of the LegInterface. This is 138 | * the decalartion and the definition of the class as it is very simple. 139 | */ 140 | class Leg : public LegInterface 141 | { 142 | public: 143 | /** 144 | * @brief Construct a new Leg object 145 | * 146 | * @param hip_motor is the pointer to the hip motor 147 | * @param knee_motor is the pointer to the knee motor 148 | */ 149 | Leg(std::shared_ptr hip_motor, 150 | std::shared_ptr knee_motor) 151 | { 152 | motors_[hip] = hip_motor; 153 | motors_[knee] = knee_motor; 154 | } 155 | 156 | /** 157 | * @brief Destroy the Leg object 158 | */ 159 | virtual ~Leg() 160 | { 161 | } 162 | 163 | /** 164 | * Getters 165 | */ 166 | 167 | /** 168 | * @brief Get the motor measurements. 169 | * 170 | * @param motor_index 171 | * @param measurement_index 172 | * @return Ptr 173 | */ 174 | virtual Ptr get_motor_measurement( 175 | const int& motor_index, const int& measurement_index) const 176 | { 177 | return motors_[motor_index]->get_measurement(measurement_index); 178 | } 179 | 180 | // input logs -------------------------------------------------------------- 181 | virtual Ptr get_current_target( 182 | const int& motor_index) const 183 | { 184 | return motors_[motor_index]->get_current_target(); 185 | } 186 | virtual Ptr get_sent_current_target( 187 | const int& motor_index) const 188 | { 189 | return motors_[motor_index]->get_sent_current_target(); 190 | } 191 | 192 | /// setters ================================================================ 193 | virtual void set_current_target(const double& current_target, 194 | const int& motor_index) 195 | { 196 | motors_[motor_index]->set_current_target(current_target); 197 | } 198 | 199 | /// sender ================================================================= 200 | virtual void send_if_input_changed() 201 | { 202 | for (size_t i = 0; i < motors_.size(); i++) 203 | motors_[i]->send_if_input_changed(); 204 | } 205 | 206 | /// ======================================================================== 207 | private: 208 | /** 209 | * @brief This list contains pointers to two motors. This motors are 210 | * respectively the hip and the knee of the leg. 211 | */ 212 | std::array, 2> motors_; 213 | }; 214 | 215 | } // namespace blmc_drivers 216 | -------------------------------------------------------------------------------- /include/blmc_drivers/devices/motor.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file motor.hpp 3 | * @license License BSD-3-Clause 4 | * @copyright Copyright (c) 2019-2020, New York University and Max Planck 5 | * Gesellschaft. 6 | * @date 2019-07-11 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include "blmc_drivers/devices/device_interface.hpp" 18 | #include "blmc_drivers/devices/motor_board.hpp" 19 | 20 | namespace blmc_drivers 21 | { 22 | /** 23 | * @brief This class declares an interface to the motor. It allows the user to 24 | * access the sensors data as well as sending controls. The only control 25 | * supported for now is the current. 26 | */ 27 | class MotorInterface : public DeviceInterface 28 | { 29 | public: 30 | /** 31 | * @brief This is a useful alias. 32 | */ 33 | typedef time_series::TimeSeries ScalarTimeseries; 34 | /** 35 | * @brief This a useful alias for the shared Pointer creation. 36 | * 37 | * @tparam Type is the Class to crate the pointer from. 38 | */ 39 | template 40 | using Ptr = std::shared_ptr; 41 | 42 | /** 43 | * @brief Here is a list of the different measurement available on the 44 | * blmc card. 45 | */ 46 | enum MeasurementIndex 47 | { 48 | current, 49 | position, 50 | velocity, 51 | encoder_index, 52 | measurement_count 53 | }; 54 | 55 | /** 56 | * @brief Destroy the MotorInterface object 57 | */ 58 | virtual ~MotorInterface() 59 | { 60 | } 61 | 62 | /** 63 | * @brief Actually send the commands and controls. 64 | */ 65 | virtual void send_if_input_changed() = 0; 66 | 67 | /** 68 | * Getters 69 | */ 70 | 71 | /** 72 | * @brief Get the measurements. 73 | * 74 | * @param index 75 | * @return Ptr the pointer to the desired 76 | * measurement history. 77 | */ 78 | virtual Ptr get_measurement( 79 | const int& index = 0) const = 0; 80 | 81 | /** 82 | * @brief Get the current target object 83 | * 84 | * @return Ptr the list of the current values to 85 | * be sent. 86 | */ 87 | virtual Ptr get_current_target() const = 0; 88 | 89 | /** 90 | * @brief Get the history of the sent current targets. 91 | * 92 | * @return Ptr 93 | */ 94 | virtual Ptr get_sent_current_target() const = 0; 95 | 96 | /** 97 | * Setters 98 | */ 99 | 100 | /** 101 | * @brief Set the current target. This function saves the data internally. 102 | * Please call send_if_input_changed() to actually send the data. 103 | * 104 | * @param current_target 105 | */ 106 | virtual void set_current_target(const double& current_target) = 0; 107 | 108 | /** 109 | * @brief Set the command. Save internally a command to be apply by the 110 | * motor board. This function save the command internally. Please call 111 | * send_if_input_changed() to actually send the data. 112 | * 113 | * @param command 114 | */ 115 | virtual void set_command(const MotorBoardCommand& command) = 0; 116 | }; 117 | 118 | /** 119 | * @brief This class implements the MotorInterface. 120 | */ 121 | class Motor : public MotorInterface 122 | { 123 | public: 124 | /** 125 | * @brief Construct a new Motor object 126 | * 127 | * @param board is the MotorBoard to be used. 128 | * @param motor_id is the id of the motor on the on-board card 129 | */ 130 | Motor(Ptr board, bool motor_id); 131 | 132 | /** 133 | * @brief Destroy the Motor object 134 | * 135 | */ 136 | virtual ~Motor() 137 | { 138 | } 139 | 140 | /** 141 | * @brief Actually send the command and controls via the network, 142 | * See MotorInterface for more information. 143 | */ 144 | virtual void send_if_input_changed() 145 | { 146 | board_->send_if_input_changed(); 147 | } 148 | 149 | /** 150 | * Getters 151 | */ 152 | 153 | /** 154 | * @brief Get the measurements 155 | * 156 | * @param index is the kind of measurement we are instersted in. 157 | * see MotorInterface::MeasurementIndex. 158 | * @return Ptr The history of the measurement 159 | */ 160 | virtual Ptr get_measurement( 161 | const int& index = 0) const; 162 | 163 | /** 164 | * @brief Get the current target to be sent. 165 | * 166 | * @return Ptr the list of current values to be 167 | * sent. 168 | */ 169 | virtual Ptr get_current_target() const; 170 | 171 | /** 172 | * @brief Get the already sent current target values. 173 | * 174 | * @return Ptr 175 | */ 176 | virtual Ptr get_sent_current_target() const; 177 | 178 | /** 179 | * Setters 180 | */ 181 | 182 | /** 183 | * @brief Set the current (Ampere) target. See MotorInterface for more 184 | * information. 185 | * 186 | * @param current_target in Ampere 187 | */ 188 | virtual void set_current_target(const double& current_target); 189 | 190 | /** 191 | * @brief Set the command. See MotorInterface for more information. 192 | * 193 | * @param command 194 | */ 195 | virtual void set_command(const MotorBoardCommand& command) 196 | { 197 | board_->set_command(command); 198 | } 199 | 200 | /** @brief Print the motor status and state. */ 201 | virtual void print() const; 202 | 203 | protected: 204 | /** 205 | * @brief The MotorBoard to be used for the communication. 206 | */ 207 | Ptr board_; 208 | 209 | /** 210 | * @brief The id of the motor on the MotorBoard. 211 | */ 212 | bool motor_id_; 213 | }; 214 | 215 | /** 216 | * @brief This class is a safe implementation of the Motor class. 217 | * It contains utilities to bound the control input. 218 | * It could also contains some velocity limits at the motor level and why not 219 | * some temperature management. 220 | * 221 | * \todo the velocity limit should be implemented in a smoother way, 222 | * and the parameters should be passed in the constructor. 223 | */ 224 | class SafeMotor : public Motor 225 | { 226 | public: 227 | /** 228 | * @brief Construct a new SafeMotor object 229 | * 230 | * @param board 231 | * @param motor_id 232 | * @param max_current_target 233 | * @param history_length 234 | */ 235 | SafeMotor( 236 | Ptr board, 237 | bool motor_id, 238 | const double& max_current_target = 2.0, 239 | const size_t& history_length = 1000, 240 | const double& max_velocity = std::numeric_limits::quiet_NaN()); 241 | 242 | /** 243 | * Getters 244 | */ 245 | 246 | /** 247 | * @brief Get the _current_target object 248 | * 249 | * @return Ptr 250 | */ 251 | virtual Ptr get_current_target() const 252 | { 253 | return current_target_; 254 | } 255 | 256 | /** 257 | * Setters 258 | */ 259 | 260 | /** 261 | * @brief Set the current target (Ampere) 262 | * 263 | * @param current_target 264 | */ 265 | virtual void set_current_target(const double& current_target); 266 | 267 | /** 268 | * @brief Set the max_current_target_ object 269 | * 270 | * @param max_current_target 271 | */ 272 | void set_max_current(double max_current_target) 273 | { 274 | max_current_target_ = max_current_target; 275 | } 276 | 277 | /** 278 | * @brief Set the max_velocity_ constant. 279 | * 280 | * @param max_velocity 281 | */ 282 | void set_max_velocity(double max_velocity) 283 | { 284 | max_velocity_ = max_velocity; 285 | } 286 | 287 | private: 288 | /** 289 | * @brief max_current_target_ is the limit of the current. 290 | */ 291 | double max_current_target_; 292 | 293 | /** 294 | * @brief max_velocity_ limits the motor velocity. 295 | */ 296 | double max_velocity_; 297 | 298 | /** 299 | * @brief History of the target current sent. 300 | */ 301 | Ptr current_target_; 302 | }; 303 | 304 | } // namespace blmc_drivers 305 | -------------------------------------------------------------------------------- /include/blmc_drivers/utils/os_interface.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file os_interface.hpp 3 | * @author Manuel Wuthrich (manuel.wuthrich@gmail.com) 4 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 5 | * @brief 6 | * @version 0.1 7 | * @date 2018-11-27 8 | * 9 | * @copyright Copyright (c) 2018 10 | * 11 | */ 12 | 13 | #pragma once 14 | 15 | /** 16 | * xeno specific include 17 | */ 18 | #ifdef __XENO__ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | /** 28 | * Ubuntu posix rt_prempt based include 29 | */ 30 | #else 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | #include 47 | 48 | #include 49 | 50 | #include 51 | 52 | /** 53 | * Define typedefs to make code compatible with Xenomai code. 54 | */ 55 | 56 | /** 57 | * @brief Create a common type_def to wrap xenomai and posix. 58 | */ 59 | typedef struct can_frame can_frame_t; 60 | /** 61 | * @brief Create a common type_def to wrap xenomai and posix. 62 | */ 63 | typedef canid_t can_id_t; 64 | /** 65 | * @brief Create a common type_def to wrap xenomai and posix. 66 | */ 67 | typedef uint64_t nanosecs_abs_t; 68 | 69 | /** 70 | * @brief Create a common type_def to wrap xenomai and posix. 71 | */ 72 | #define rt_fprintf fprintf 73 | /** 74 | * @brief Create a common type_def to wrap xenomai and posix. 75 | */ 76 | #define rt_printf printf 77 | 78 | /** 79 | * @brief Create a common type_def to wrap xenomai and posix. 80 | */ 81 | #define rt_dev_socket socket 82 | /** 83 | * @brief Create a common type_def to wrap xenomai and posix. 84 | */ 85 | #define rt_dev_ioctl ioctl 86 | /** 87 | * @brief Create a common type_def to wrap xenomai and posix. 88 | */ 89 | #define rt_dev_close close 90 | /** 91 | * @brief Create a common type_def to wrap xenomai and posix. 92 | */ 93 | #define rt_dev_setsockopt setsockopt 94 | /** 95 | * @brief Create a common type_def to wrap xenomai and posix. 96 | */ 97 | #define rt_dev_bind bind 98 | /** 99 | * @brief Create a common type_def to wrap xenomai and posix. 100 | */ 101 | #define rt_dev_recvmsg recvmsg 102 | /** 103 | * @brief Create a common type_def to wrap xenomai and posix. 104 | */ 105 | #define rt_dev_sendto sendto 106 | 107 | /** 108 | * Common include 109 | */ 110 | #endif 111 | 112 | #include 113 | #include 114 | #include 115 | 116 | #include 117 | 118 | /** 119 | * @brief osi stands for Operating System Interface. 120 | * \todo This workspace should be replaced eventually by the real_time_tools 121 | * package. 122 | */ 123 | namespace osi 124 | { 125 | #ifdef __XENO__ 126 | 127 | /** 128 | * Namespace to wrap the xenomai specific implementation xenomai 129 | */ 130 | namespace xenomai 131 | { 132 | /** 133 | * @brief The mutex class is a specific implementation of the mutex class 134 | * for xenomai 135 | */ 136 | class mutex 137 | { 138 | public: 139 | /** 140 | * @brief Create a xenomai mutex object. 141 | */ 142 | RT_MUTEX rt_mutex_; 143 | 144 | /** 145 | * @brief Construct a new mutex object 146 | */ 147 | mutex() 148 | { 149 | rt_mutex_create(&rt_mutex_, NULL); 150 | } 151 | 152 | /** 153 | * @brief lock the mutex. 154 | */ 155 | void lock() 156 | { 157 | rt_mutex_acquire(&rt_mutex_, TM_INFINITE); 158 | } 159 | 160 | /** 161 | * @brief Unlock the mutex. 162 | */ 163 | void unlock() 164 | { 165 | rt_mutex_release(&rt_mutex_); 166 | } 167 | }; 168 | 169 | /** 170 | * @brief Implementation of a condition variable specific to xenomai 171 | */ 172 | class condition_variable 173 | { 174 | public: 175 | /** 176 | * @brief Create the xenomai condition variable object 177 | */ 178 | RT_COND rt_condition_variable_; 179 | 180 | /** 181 | * @brief Construct a new condition_variable object 182 | */ 183 | condition_variable() 184 | { 185 | rt_cond_create(&rt_condition_variable_, NULL); 186 | } 187 | 188 | /** 189 | * @brief Put the condition variable to wait mode. 190 | * 191 | * @param lock is the mutex to be used for locking the scope. 192 | */ 193 | void wait(std::unique_lock &lock) 194 | { 195 | rt_cond_wait( 196 | &rt_condition_variable_, &lock.mutex()->rt_mutex_, TM_INFINITE); 197 | } 198 | 199 | /** 200 | * @brief Notify all condition variable owning the same mutex. 201 | */ 202 | void notify_all() 203 | { 204 | rt_cond_broadcast(&rt_condition_variable_); 205 | } 206 | }; 207 | } // namespace xenomai 208 | /** 209 | * @brief Wrapper around the xenomai specific Mutex implementation. 210 | */ 211 | typedef xenomai::mutex Mutex; 212 | 213 | /** 214 | * @brief Wrapper around the xenomai specific ConditionVariable 215 | * implementation 216 | */ 217 | typedef xenomai::condition_variable ConditionVariable; 218 | 219 | #else 220 | /** 221 | * @brief Wrapper around the posix specific Mutex implementation. 222 | */ 223 | typedef std::mutex Mutex; 224 | 225 | /** 226 | * @brief Wrapper around the posix specific ConditionVariable 227 | * implementation 228 | */ 229 | typedef std::condition_variable ConditionVariable; 230 | #endif 231 | 232 | /** 233 | * @brief Use the osi workspace API to communicate with the can bus. 234 | * /todo Manuel can you describe the argument of this function? 235 | * @param fd 236 | * @param buf 237 | * @param len 238 | * @param flags 239 | * @param to 240 | * @param tolen 241 | */ 242 | inline void send_to_can_device(int fd, 243 | const void *buf, 244 | size_t len, 245 | int flags, 246 | const struct sockaddr *to, 247 | socklen_t tolen) 248 | { 249 | // int ret = rt_dev_sendto(fd, buf, len, flags, to, tolen); 250 | 251 | // if (ret < 0) 252 | // { 253 | // std::ostringstream oss; 254 | // oss << "something went wrong with sending " 255 | // << "CAN frame, error code: " 256 | // << ret << ", errno=" << errno << std::endl; 257 | // throw std::runtime_error(oss.str()); 258 | // } 259 | 260 | for (size_t i = 0; true; i++) 261 | { 262 | int ret = rt_dev_sendto(fd, buf, len, flags, to, tolen); 263 | if (ret >= 0) 264 | { 265 | if (i > 0) 266 | { 267 | std::cout << " Managed to send after " << i << " attempts." 268 | << std::endl; 269 | } 270 | return; 271 | } 272 | 273 | if (i == 0) 274 | { 275 | std::cout << "WARNING: Something went wrong with sending " 276 | << "CAN frame, error code: " << ret 277 | << ", errno: " << errno << ". Possibly you have " 278 | << "been attempting to send at a rate which is too " 279 | << "high. We keep trying" << std::flush; 280 | } 281 | 282 | real_time_tools::Timer::sleep_ms(0.1); 283 | } 284 | } 285 | 286 | /** 287 | * @brief This function is closing a socket on the Can device. It is os 288 | * independent. 289 | * 290 | * @param socket 291 | */ 292 | inline void close_can_device(int socket) 293 | { 294 | int ret = rt_dev_close(socket); 295 | if (ret) 296 | { 297 | rt_fprintf(stderr, "rt_dev_close: %s\n", strerror(-ret)); 298 | exit(-1); 299 | } 300 | } 301 | 302 | /** 303 | * @brief Poll? a message from the CAN device. 304 | * \todo Manuel can you confrim this? And precise the arguments of the function? 305 | * 306 | * @param fd 307 | * @param msg 308 | * @param flags 309 | */ 310 | inline void receive_message_from_can_device(int fd, 311 | struct msghdr *msg, 312 | int flags) 313 | { 314 | int ret = rt_dev_recvmsg(fd, msg, flags); 315 | if (ret < 0) 316 | { 317 | std::ostringstream oss; 318 | oss << "something went wrong with receiving " 319 | << "CAN frame, error code: " << ret << ", errno=" << errno 320 | << std::endl; 321 | throw std::runtime_error(oss.str()); 322 | } 323 | } 324 | 325 | /** 326 | * @brief This function is needed in xenomai to initialize the real time console 327 | * display of text. 328 | */ 329 | inline void initialize_realtime_printing() 330 | { 331 | #ifdef __XENO__ 332 | rt_print_auto_init(1); 333 | #endif 334 | } 335 | 336 | /** 337 | * @brief This function uses eather the xenomai API or the posix one. 338 | * 339 | * @param sleep_time_ms is the sleeping time in milli seconds. 340 | */ 341 | inline void sleep_ms(const double &sleep_time_ms) 342 | { 343 | #ifdef __XENO__ 344 | rt_task_sleep(int(sleep_time_ms * 1000000.)); 345 | #else 346 | usleep(sleep_time_ms * 1000.); // nano_sleep 347 | #endif 348 | } 349 | 350 | /** 351 | * @brief Get the current time in millisecond. 352 | * \todo remove as the one form the Timer class is much better embeded. 353 | * 354 | * @return double which is the time in milli second 355 | */ 356 | inline double get_current_time_ms() 357 | { 358 | #ifdef __XENO__ 359 | return double(rt_timer_read()) / 1000000.; 360 | #else 361 | struct timespec now; 362 | clock_gettime(CLOCK_REALTIME, &now); 363 | double current_time_ms = (double)(now.tv_sec * 1e3) + (now.tv_nsec / 1e6); 364 | 365 | return current_time_ms; 366 | #endif 367 | } 368 | 369 | /** 370 | * @brief This methd is requiered in xenomai to create a real time thread. 371 | */ 372 | inline void make_this_thread_realtime() 373 | { 374 | #ifdef __XENO__ 375 | 376 | rt_task_shadow(NULL, NULL, 0, 0); 377 | #endif 378 | } 379 | 380 | } // namespace osi 381 | -------------------------------------------------------------------------------- /include/blmc_drivers/utils/polynome.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file polynome.hpp 3 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 4 | * @brief Polynomes object for trajectories. 5 | * @version 0.1 6 | * @date 2019-11-06 7 | * 8 | * @copyright Copyright (c) 2019 9 | * 10 | */ 11 | #pragma once 12 | 13 | #include 14 | 15 | namespace blmc_drivers 16 | { 17 | /** 18 | * @brief Simple class that defines \f$ P(x) \f$ a polynome of order ORDER. 19 | * It provide simple methods to compute \f$ P(x) \f$, 20 | * \f$ \frac{dP}{dx}(x) \f$, and \f$ \frac{dP^2}{dx^2}(x) \f$ 21 | * 22 | * @tparam ORDER is the order of the polynome 23 | */ 24 | template 25 | class Polynome 26 | { 27 | /*! Type of the container for the poynome coefficients */ 28 | typedef std::array Coefficients; 29 | 30 | public: 31 | /*! Constructor */ 32 | Polynome(); 33 | 34 | /*! Destructor */ 35 | ~Polynome(); 36 | 37 | /*! Compute the value. */ 38 | double compute(double x); 39 | 40 | /*! Compute the value of the derivative. */ 41 | double compute_derivative(double x); 42 | 43 | /*! Compute the value of the second derivative. */ 44 | double compute_sec_derivative(double x); 45 | 46 | /*! Get the coefficients. */ 47 | void get_coefficients(Coefficients &coefficients) const; 48 | 49 | /*! Set the coefficients. */ 50 | void set_coefficients(const Coefficients &coefficients); 51 | 52 | inline int degree() 53 | { 54 | return ORDER; 55 | }; 56 | 57 | /*! Print the coefficient. */ 58 | void print() const; 59 | 60 | protected: 61 | /*! Vector of coefficients. */ 62 | std::array coefficients_; 63 | }; 64 | 65 | /** 66 | * @brief Simple class that defines \f$ P(t) \f$ a polynome of order ORDER. 67 | * With \f$ t \f$ being the time in any units. 68 | * It provide simple methods to compute safely \f$ P(time) \f$, 69 | * \f$ \frac{dP}{dt}(t) \f$, and \f$ \frac{dP^2}{dt^2}(t) \f$ 70 | * 71 | * @tparam ORDER 72 | */ 73 | template 74 | class TimePolynome : public Polynome 75 | { 76 | public: 77 | /*! Constructor */ 78 | TimePolynome() 79 | { 80 | final_time_ = 0.0; 81 | init_pose_ = 0.0; 82 | init_speed_ = 0.0; 83 | init_acc_ = 0.0; 84 | final_pose_ = 0.0; 85 | final_speed_ = 0.0; 86 | final_acc_ = 0.0; 87 | }; 88 | 89 | /*! Destructor */ 90 | ~TimePolynome(){}; 91 | 92 | /*! Compute the value. */ 93 | double compute(double t); 94 | 95 | /*! Compute the value of the derivative. */ 96 | double compute_derivative(double t); 97 | 98 | /*! Compute the value of the second derivative. */ 99 | double compute_sec_derivative(double t); 100 | 101 | double get_final_time() const 102 | { 103 | return final_time_; 104 | } 105 | double get_init_pose() const 106 | { 107 | return init_pose_; 108 | } 109 | double get_init_speed() const 110 | { 111 | return init_speed_; 112 | } 113 | double get_init_acc() const 114 | { 115 | return init_acc_; 116 | } 117 | double get_final_pose() const 118 | { 119 | return final_pose_; 120 | } 121 | double get_final_speed() const 122 | { 123 | return final_speed_; 124 | } 125 | double get_final_acc() const 126 | { 127 | return final_acc_; 128 | } 129 | 130 | /** 131 | * @brief Computes a polynome trajectory according to the following 132 | * constraints: 133 | * \f{eqnarray*}{ 134 | * P(0) &=& init_pose \\ 135 | * P(0) &=& init_speed = 0 \\ 136 | * P(0) &=& init_acc = 0 \\ 137 | * P(final_time_) &=& final_pose \\ 138 | * P(final_time_) &=& final_speed = 0 \\ 139 | * P(final_time_) &=& final_acc = 0 140 | * \f} 141 | * 142 | * @param final_time is used in the constraints. 143 | * @param init_pose is used in the constraints. 144 | * @param init_speed is used in the constraints. 145 | * @param final_pose is used in the constraints. 146 | */ 147 | void set_parameters(double final_time, 148 | double init_pose, 149 | double init_speed, 150 | double final_pose); 151 | 152 | protected: 153 | double final_time_; /**< store the inputs for later access */ 154 | double init_pose_; /**< store the inputs for later access */ 155 | double init_speed_; /**< store the inputs for later access */ 156 | double init_acc_; /**< store the inputs for later access */ 157 | double final_pose_; /**< store the inputs for later access */ 158 | double final_speed_; /**< store the inputs for later access */ 159 | double final_acc_; /**< store the inputs for later access */ 160 | }; 161 | 162 | } // namespace blmc_drivers 163 | 164 | #include "blmc_drivers/utils/polynome.hxx" 165 | -------------------------------------------------------------------------------- /include/blmc_drivers/utils/polynome.hxx: -------------------------------------------------------------------------------- 1 | /** 2 | * @file polynome_impl.hh 3 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 4 | * @brief Polynomes object for trajectories. 5 | * @version 0.1 6 | * @date 2019-11-06 7 | * 8 | * @copyright Copyright (c) 2019 9 | * 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | 17 | namespace blmc_drivers 18 | { 19 | /** 20 | * Polynome definitions 21 | */ 22 | 23 | template 24 | Polynome::Polynome() 25 | { 26 | coefficients_.fill(0.0); 27 | } 28 | 29 | template 30 | Polynome::~Polynome() 31 | { 32 | } 33 | 34 | template 35 | double Polynome::compute(double x) 36 | { 37 | double res = 0.0; 38 | double pt = 1.0; 39 | for (size_t i = 0; i < coefficients_.size(); ++i) 40 | { 41 | res += coefficients_[i] * pt; 42 | pt *= x; 43 | } 44 | return res; 45 | } 46 | 47 | template 48 | double Polynome::compute_derivative(double x) 49 | { 50 | double res = 0.0; 51 | double pt = 1.0; 52 | for (size_t i = 1; i < coefficients_.size(); ++i) 53 | { 54 | res += i * coefficients_[i] * pt; 55 | pt *= x; 56 | } 57 | return res; 58 | } 59 | 60 | template 61 | double Polynome::compute_sec_derivative(double x) 62 | { 63 | double res = 0.0; 64 | double pt = 1.0; 65 | for (size_t i = 2; i < coefficients_.size(); ++i) 66 | { 67 | res += i * (i - 1) * coefficients_[i] * pt; 68 | pt *= x; 69 | } 70 | return res; 71 | } 72 | 73 | template 74 | void Polynome::get_coefficients(Coefficients &coefficients) const 75 | { 76 | coefficients = coefficients_; 77 | } 78 | 79 | template 80 | void Polynome::set_coefficients(const Coefficients &coefficients) 81 | { 82 | coefficients_ = coefficients; 83 | } 84 | 85 | template 86 | void Polynome::print() const 87 | { 88 | for (size_t i = 0; i < ORDER; ++i) 89 | { 90 | std::cout << coefficients_[i] << " "; 91 | } 92 | std::cout << std::endl; 93 | } 94 | 95 | /** 96 | * TimePolynome definitions 97 | */ 98 | 99 | template 100 | double TimePolynome::compute(double t) 101 | { 102 | if (t <= 0.0) 103 | { 104 | return init_pose_; 105 | } 106 | else if (t >= final_time_) 107 | { 108 | return final_pose_; 109 | } 110 | else 111 | { 112 | return Polynome::compute(t); 113 | } 114 | } 115 | 116 | template 117 | double TimePolynome::compute_derivative(double t) 118 | { 119 | if (t <= 0.0) 120 | { 121 | return init_speed_; 122 | } 123 | else if (t >= final_time_) 124 | { 125 | return final_speed_; 126 | } 127 | else 128 | { 129 | return Polynome::compute_derivative(t); 130 | } 131 | } 132 | 133 | template 134 | double TimePolynome::compute_sec_derivative(double t) 135 | { 136 | if (t <= 0.0) 137 | { 138 | return init_acc_; 139 | } 140 | else if (t >= final_time_) 141 | { 142 | return final_acc_; 143 | } 144 | else 145 | { 146 | return Polynome::compute_sec_derivative(t); 147 | } 148 | } 149 | 150 | } // namespace blmc_drivers 151 | -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, New York University and Max Planck Gesellschaft. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | blmc_drivers 10 | 2.0.0 11 | 12 | 13 | Drivers for CAN-communication with BLMC related devices developed in 14 | https://github.com/open-dynamic-robot-initiative/ . 15 | 16 | 17 | Vincent Berenz 18 | Felix Widmaier 19 | 20 | BSD 3-clause 21 | 22 | ament_cmake 23 | 24 | mpi_cmake_modules 25 | real_time_tools 26 | time_series 27 | Eigen3 28 | rt 29 | Threads 30 | 31 | ament_cmake_gtest 32 | 33 | 34 | ament_cmake 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /scripts/initialize_can_bus.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | sudo ip link set down can0 4 | sudo ip link set down can1 5 | sudo ip link set down can2 6 | sudo ip link set down can3 7 | sudo ip link set down can4 8 | sudo ip link set down can5 9 | sudo ip link set down can6 10 | sudo ip link set down can7 11 | 12 | sudo ip link set can0 type can bitrate 1000000 13 | sudo ip link set can1 type can bitrate 1000000 14 | sudo ip link set can2 type can bitrate 1000000 15 | sudo ip link set can3 type can bitrate 1000000 16 | sudo ip link set can4 type can bitrate 1000000 17 | sudo ip link set can5 type can bitrate 1000000 18 | sudo ip link set can6 type can bitrate 1000000 19 | sudo ip link set can7 type can bitrate 1000000 20 | 21 | sudo ip link set up can0 22 | sudo ip link set up can1 23 | sudo ip link set up can2 24 | sudo ip link set up can3 25 | sudo ip link set up can4 26 | sudo ip link set up can5 27 | sudo ip link set up can6 28 | sudo ip link set up can7 29 | 30 | ifconfig can0 txqueuelen 1000 31 | ifconfig can1 txqueuelen 1000 32 | ifconfig can2 txqueuelen 1000 33 | ifconfig can3 txqueuelen 1000 34 | ifconfig can4 txqueuelen 1000 35 | ifconfig can5 txqueuelen 1000 36 | ifconfig can6 txqueuelen 1000 37 | ifconfig can7 txqueuelen 1000 38 | 39 | ip link show 40 | 41 | netstat -i 42 | -------------------------------------------------------------------------------- /src/analog_sensors.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file analog_sensors.cpp 3 | * @author Manuel Wuthrich (manuel.wuthrich@gmail.com) 4 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 5 | * @brief This file defines a class to get access to analogue sensors. 6 | * @version 0.1 7 | * @date 2018-11-23 8 | * 9 | * @copyright Copyright (c) 2018 10 | * 11 | */ 12 | 13 | #include 14 | 15 | namespace blmc_drivers 16 | { 17 | AnalogSensor::AnalogSensor(std::shared_ptr board, 18 | bool sensor_id) 19 | : board_(board), sensor_id_(sensor_id) 20 | { 21 | } 22 | 23 | std::shared_ptr 24 | AnalogSensor::get_measurement() const 25 | { 26 | if (sensor_id_ == 0) 27 | { 28 | return board_->get_measurement(MotorBoardInterface::analog_0); 29 | } 30 | else 31 | { 32 | return board_->get_measurement(MotorBoardInterface::analog_1); 33 | } 34 | } 35 | 36 | } // namespace blmc_drivers -------------------------------------------------------------------------------- /src/blmc_joint_module.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file blmc_joint_module.cpp 3 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 4 | * @author Manuel Wuthrich 5 | * @license License BSD-3-Clause 6 | * @copyright Copyright (c) 2019, New York University and Max Planck 7 | * Gesellschaft. 8 | * @date 2019-07-11 9 | */ 10 | 11 | #include "blmc_drivers/blmc_joint_module.hpp" 12 | #include 13 | #include "real_time_tools/iostream.hpp" 14 | #include "real_time_tools/spinner.hpp" 15 | 16 | namespace blmc_drivers 17 | { 18 | BlmcJointModule::BlmcJointModule( 19 | std::shared_ptr motor, 20 | const double& motor_constant, 21 | const double& gear_ratio, 22 | const double& zero_angle, 23 | const bool& reverse_polarity, 24 | const double& max_current) 25 | { 26 | motor_ = motor; 27 | motor_constant_ = motor_constant; 28 | gear_ratio_ = gear_ratio; 29 | set_zero_angle(zero_angle); 30 | polarity_ = reverse_polarity ? -1.0 : 1.0; 31 | max_current_ = max_current; 32 | 33 | position_control_gain_p_ = 0; 34 | position_control_gain_d_ = 0; 35 | } 36 | 37 | void BlmcJointModule::set_torque(const double& desired_torque) 38 | { 39 | double desired_current = joint_torque_to_motor_current(desired_torque); 40 | 41 | if (std::fabs(desired_current) > max_current_) 42 | { 43 | std::cout << "something went wrong, it should never happen" 44 | " that desired_current > " 45 | << max_current_ << ". desired_current: " << desired_current 46 | << std::endl; 47 | exit(-1); 48 | } 49 | motor_->set_current_target(polarity_ * desired_current); 50 | } 51 | 52 | void BlmcJointModule::set_zero_angle(const double& zero_angle) 53 | { 54 | zero_angle_ = zero_angle; 55 | } 56 | 57 | void BlmcJointModule::set_joint_polarity(const bool& reverse_polarity) 58 | { 59 | polarity_ = reverse_polarity ? -1.0 : 1.0; 60 | } 61 | void BlmcJointModule::send_torque() 62 | { 63 | motor_->send_if_input_changed(); 64 | } 65 | 66 | double BlmcJointModule::get_max_torque() const 67 | { 68 | return motor_current_to_joint_torque(max_current_); 69 | } 70 | 71 | double BlmcJointModule::get_sent_torque() const 72 | { 73 | auto measurement_history = motor_->get_sent_current_target(); 74 | 75 | if (measurement_history->length() == 0) 76 | { 77 | return std::numeric_limits::quiet_NaN(); 78 | } 79 | return motor_current_to_joint_torque(measurement_history->newest_element()); 80 | } 81 | 82 | double BlmcJointModule::get_measured_torque() const 83 | { 84 | return motor_current_to_joint_torque(get_motor_measurement(mi::current)); 85 | } 86 | 87 | double BlmcJointModule::get_measured_angle() const 88 | { 89 | return get_motor_measurement(mi::position) / gear_ratio_ - zero_angle_; 90 | } 91 | 92 | double BlmcJointModule::get_measured_velocity() const 93 | { 94 | return get_motor_measurement(mi::velocity) / gear_ratio_; 95 | } 96 | 97 | double BlmcJointModule::joint_torque_to_motor_current(double torque) const 98 | { 99 | return torque / gear_ratio_ / motor_constant_; 100 | } 101 | 102 | double BlmcJointModule::motor_current_to_joint_torque(double current) const 103 | { 104 | return current * gear_ratio_ * motor_constant_; 105 | } 106 | 107 | double BlmcJointModule::get_measured_index_angle() const 108 | { 109 | return get_motor_measurement(mi::encoder_index) / gear_ratio_; 110 | } 111 | 112 | double BlmcJointModule::get_zero_angle() const 113 | { 114 | return zero_angle_; 115 | } 116 | 117 | double BlmcJointModule::get_motor_measurement(const mi& measurement_id) const 118 | { 119 | auto measurement_history = motor_->get_measurement(measurement_id); 120 | 121 | if (measurement_history->length() == 0) 122 | { 123 | // rt_printf("get_motor_measurement returns NaN\n"); 124 | return std::numeric_limits::quiet_NaN(); 125 | } 126 | return polarity_ * measurement_history->newest_element(); 127 | } 128 | 129 | long int BlmcJointModule::get_motor_measurement_index( 130 | const mi& measurement_id) const 131 | { 132 | auto measurement_history = motor_->get_measurement(measurement_id); 133 | 134 | if (measurement_history->length() == 0) 135 | { 136 | // rt_printf("get_motor_measurement_index returns NaN\n"); 137 | return -1; 138 | } 139 | return measurement_history->newest_timeindex(); 140 | } 141 | 142 | void BlmcJointModule::set_position_control_gains(double kp, double kd) 143 | { 144 | position_control_gain_p_ = kp; 145 | position_control_gain_d_ = kd; 146 | } 147 | 148 | double BlmcJointModule::execute_position_controller( 149 | double target_position_rad) const 150 | { 151 | double diff = target_position_rad - get_measured_angle(); 152 | 153 | // simple PD control 154 | double desired_torque = position_control_gain_p_ * diff - 155 | position_control_gain_d_ * get_measured_velocity(); 156 | 157 | // clamp torque 158 | const double max_torque = motor_current_to_joint_torque(max_current_) * 0.9; 159 | if (desired_torque > max_torque) 160 | { 161 | desired_torque = max_torque; 162 | } 163 | else if (desired_torque < -max_torque) 164 | { 165 | desired_torque = -max_torque; 166 | } 167 | 168 | return desired_torque; 169 | } 170 | 171 | bool BlmcJointModule::calibrate(double& angle_zero_to_index, 172 | double& index_angle, 173 | bool mechanical_calibration) 174 | { 175 | // save the current position 176 | double starting_position = get_measured_angle(); 177 | rt_printf("Starting pose is=%f\n", starting_position); 178 | 179 | // reset the ouput 180 | index_angle = 0.0; 181 | 182 | // we reset the internal zero angle. 183 | zero_angle_ = 0.0; 184 | 185 | long int last_index_time = get_motor_measurement_index(mi::encoder_index); 186 | if (std::isnan(last_index_time)) 187 | { 188 | last_index_time = -1; 189 | } 190 | bool reached_next_index = false; 191 | real_time_tools::Spinner spinner; 192 | spinner.set_period(0.001); 193 | rt_printf("Search for the index\n"); 194 | while (!reached_next_index) 195 | { 196 | // Small D gain 197 | double k_d = 0.2; 198 | // Small desired velocity 199 | double joint_vel_des = 0.8; 200 | // Velocity controller 201 | double actual_velocity = get_measured_velocity(); 202 | double torque = +k_d * (joint_vel_des - actual_velocity); 203 | // rt_printf("error=%f, vel_des=%f, vel=%f, tau=%f\n", joint_vel_des - 204 | // actual_velocity, joint_vel_des, actual_velocity, torque); Send the 205 | // torque command 206 | set_torque(torque); 207 | send_torque(); 208 | // check stop 209 | long int actual_index_time = 210 | get_motor_measurement_index(mi::encoder_index); 211 | double actual_index_angle = get_measured_index_angle(); 212 | 213 | reached_next_index = (actual_index_time > last_index_time); 214 | // rt_printf("last_index_time=%ld, actual_index_time=%ld, 215 | // actual_index_angle=%f\n", last_index_time, actual_index_time, 216 | // actual_index_angle); 217 | if (reached_next_index) 218 | { 219 | index_angle = actual_index_angle; 220 | } 221 | spinner.spin(); 222 | } 223 | // reset the control to zero torque 224 | set_torque(0.0); 225 | send_torque(); 226 | spinner.spin(); 227 | 228 | // get the indexes and stuff 229 | if (mechanical_calibration) 230 | { 231 | angle_zero_to_index = index_angle - starting_position; 232 | } 233 | zero_angle_ = index_angle - angle_zero_to_index; 234 | 235 | rt_printf("Zero angle is=%f\n", zero_angle_); 236 | rt_printf("Zero angle to index angle is=%f\n", angle_zero_to_index); 237 | rt_printf("Index angle is=%f\n", index_angle); 238 | 239 | rt_printf("Position Control\n"); 240 | // Go to 0 241 | double init_pose = get_measured_angle(); 242 | double final_pose = 0.0; 243 | double final_angle = 0.0; 244 | int traj_time = 2.0 / 0.001; 245 | int counter = 0; 246 | double torque_int = 0.0; 247 | double torque_sat = 0.1; // Nm 248 | bool reached_zero_pose = 0; 249 | while (!reached_zero_pose) 250 | { 251 | // Small P gain 252 | double k_p = 2.5; 253 | // Integrale gain 254 | double k_i = 0.5; 255 | // desired pose 256 | double alpha = 1.0 - (double)((double)counter / (double)traj_time); 257 | double des_angle = alpha * init_pose + (1.0 - alpha) * final_pose; 258 | // compute the error 259 | double current_angle = get_measured_angle(); 260 | double err = des_angle - current_angle; 261 | // small saturation in intensity 262 | torque_int += k_i * err * 0.001; // 1 ms sampling period 263 | if (torque_int > torque_sat) 264 | { 265 | torque_int = torque_sat; 266 | } 267 | if (torque_int < -torque_sat) 268 | { 269 | torque_int = -torque_sat; 270 | } 271 | // Position controller 272 | double torque = k_p * err + torque_int; 273 | // rt_printf("error=%f, torque_int=%f, tau=%f\n", err, torque_int, 274 | // torque); Send the torque command 275 | set_torque(torque); 276 | send_torque(); 277 | // check out 278 | reached_zero_pose = 279 | (std::fabs(current_angle) <= 1e-2); // nearly 0.1 degree 280 | if (reached_zero_pose) 281 | { 282 | final_angle = -err; 283 | } 284 | spinner.spin(); 285 | ++counter; 286 | if (counter > traj_time) 287 | { 288 | counter = traj_time; 289 | } 290 | } 291 | rt_printf("Final angle is=%f\n", final_angle); 292 | // reset the control to zero torque 293 | set_torque(0.0); 294 | send_torque(); 295 | spinner.spin(); 296 | 297 | // FIXME: always returns true as there is no error checking 298 | return true; 299 | } 300 | 301 | void BlmcJointModule::homing_at_current_position(double home_offset_rad) 302 | { 303 | // reset the internal zero angle. 304 | set_zero_angle(0.0); 305 | 306 | // set the zero angle 307 | set_zero_angle(get_measured_angle() + home_offset_rad); 308 | 309 | homing_state_.status = HomingReturnCode::SUCCEEDED; 310 | } 311 | 312 | void BlmcJointModule::init_homing(int joint_id, 313 | double search_distance_limit_rad, 314 | double home_offset_rad, 315 | double profile_step_size_rad) 316 | { 317 | // reset the internal zero angle. 318 | set_zero_angle(0.0); 319 | 320 | // TODO: would be nice if the joint instance had a `name` or `id` and class 321 | // level instead of storing it here (to make more useful debug prints). 322 | homing_state_.joint_id = joint_id; 323 | 324 | homing_state_.search_distance_limit_rad = search_distance_limit_rad; 325 | homing_state_.home_offset_rad = home_offset_rad; 326 | homing_state_.profile_step_size_rad = profile_step_size_rad; 327 | homing_state_.last_encoder_index_time_index = 328 | get_motor_measurement_index(mi::encoder_index); 329 | homing_state_.target_position_rad = get_measured_angle(); 330 | homing_state_.step_count = 0; 331 | homing_state_.start_position = get_measured_angle(); 332 | 333 | homing_state_.status = HomingReturnCode::RUNNING; 334 | } 335 | 336 | HomingReturnCode BlmcJointModule::update_homing() 337 | { 338 | switch (homing_state_.status) 339 | { 340 | case HomingReturnCode::NOT_INITIALIZED: 341 | set_torque(0.0); 342 | send_torque(); 343 | rt_printf("[%d] Homing is not initialized. Abort.\n", 344 | homing_state_.joint_id); 345 | break; 346 | 347 | case HomingReturnCode::FAILED: 348 | // when failed, send zero-torque commands 349 | set_torque(0.0); 350 | break; 351 | 352 | case HomingReturnCode::SUCCEEDED: 353 | { 354 | // when succeeded, keep the motor at the home position 355 | double desired_torque = 356 | execute_position_controller(homing_state_.target_position_rad); 357 | 358 | set_torque(desired_torque); 359 | break; 360 | } 361 | 362 | case HomingReturnCode::RUNNING: 363 | { 364 | // number of steps after which the distance limit is reached 365 | const uint32_t max_step_count = 366 | std::abs(homing_state_.search_distance_limit_rad / 367 | homing_state_.profile_step_size_rad); 368 | 369 | // abort if distance limit is reached 370 | if (homing_state_.step_count >= max_step_count) 371 | { 372 | set_torque(0.0); 373 | homing_state_.status = HomingReturnCode::FAILED; 374 | 375 | rt_printf( 376 | "BlmcJointModule::update_homing(): " 377 | "ERROR: Failed to find index with joint [%d].\n", 378 | homing_state_.joint_id); 379 | break; 380 | } 381 | 382 | // -- EXECUTE ONE STEP 383 | 384 | homing_state_.step_count++; 385 | homing_state_.target_position_rad += 386 | homing_state_.profile_step_size_rad; 387 | 388 | #ifdef VERBOSE 389 | const double current_position = get_measured_angle(); 390 | if (homing_state_.step_count % 100 == 0) 391 | { 392 | rt_printf("[%d] cur: %f,\t des: %f\n", 393 | homing_state_.joint_id, 394 | current_position, 395 | homing_state_.target_position_rad); 396 | } 397 | #endif 398 | 399 | // FIXME: add a safety check to stop if following error gets too 400 | // big. 401 | 402 | const double desired_torque = 403 | execute_position_controller(homing_state_.target_position_rad); 404 | set_torque(desired_torque); 405 | 406 | // Check if new encoder index was observed 407 | const long int actual_index_time = 408 | get_motor_measurement_index(mi::encoder_index); 409 | if (actual_index_time > homing_state_.last_encoder_index_time_index) 410 | { 411 | // -- FINISHED 412 | const double index_angle = get_measured_index_angle(); 413 | 414 | // Store the end position of the homing so it can be used to 415 | // determine the travelled distance. 416 | homing_state_.end_position = index_angle; 417 | 418 | // set the zero angle 419 | set_zero_angle(index_angle + homing_state_.home_offset_rad); 420 | 421 | // adjust target_position according to the new zero 422 | homing_state_.target_position_rad -= zero_angle_; 423 | 424 | #ifdef VERBOSE 425 | rt_printf("[%d] Zero angle is=%f\n", 426 | homing_state_.joint_id, 427 | zero_angle_); 428 | rt_printf("[%d] Index angle is=%f\n", 429 | homing_state_.joint_id, 430 | index_angle); 431 | #endif 432 | 433 | homing_state_.status = HomingReturnCode::SUCCEEDED; 434 | } 435 | 436 | break; 437 | } 438 | } 439 | 440 | return homing_state_.status; 441 | } 442 | 443 | double BlmcJointModule::get_distance_travelled_during_homing() const 444 | { 445 | if (homing_state_.status != HomingReturnCode::SUCCEEDED) 446 | { 447 | throw std::runtime_error( 448 | "Homing status needs to be SUCCEEDED to determine travelled " 449 | "distance."); 450 | } 451 | 452 | return homing_state_.end_position - homing_state_.start_position; 453 | } 454 | 455 | } // namespace blmc_drivers 456 | -------------------------------------------------------------------------------- /src/can_bus.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file can_bus.cpp 3 | * @author Felix Widmaier (felix.widmaier@tuebingen.mpg.de) 4 | * @author Manuel Wuthrich (manuel.wuthrich@gmail.com) 5 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 6 | * @brief This file defines classes that allow communication with a Can network. 7 | * @version 0.1 8 | * @date 2018-11-23 9 | * 10 | * @copyright Copyright (c) 2018 11 | * 12 | */ 13 | 14 | #include 15 | 16 | #include 17 | 18 | namespace blmc_drivers 19 | { 20 | CanBus::CanBus(const std::string &can_interface_name, 21 | const size_t &history_length) 22 | { 23 | input_ = std::make_shared(history_length, 0, false); 24 | sent_input_ = 25 | std::make_shared(history_length, 0, false); 26 | output_ = std::make_shared(history_length, 0, false); 27 | name_ = can_interface_name; 28 | 29 | can_connection_.set(setup_can(can_interface_name, 0)); 30 | 31 | is_loop_active_ = true; 32 | rt_thread_.create_realtime_thread(&CanBus::loop, this); 33 | } 34 | 35 | CanBus::~CanBus() 36 | { 37 | is_loop_active_ = false; 38 | rt_thread_.join(); 39 | osi::close_can_device(can_connection_.get().socket); 40 | } 41 | 42 | void CanBus::send_if_input_changed() 43 | { 44 | if (input_->has_changed_since_tag()) 45 | { 46 | time_series::Index timeindex_to_send = input_->newest_timeindex(); 47 | CanBusFrame frame_to_send = (*input_)[timeindex_to_send]; 48 | input_->tag(timeindex_to_send); 49 | sent_input_->append(frame_to_send); 50 | 51 | send_frame(frame_to_send); 52 | } 53 | } 54 | 55 | void CanBus::loop() 56 | { 57 | while (is_loop_active_) 58 | { 59 | output_->append(receive_frame()); 60 | } 61 | } 62 | 63 | void CanBus::send_frame(const CanBusFrame &unstamped_can_frame) 64 | { 65 | // get address --------------------------------------------------------- 66 | int socket = can_connection_.get().socket; 67 | struct sockaddr_can address = can_connection_.get().send_addr; 68 | 69 | // put data into can frame --------------------------------------------- 70 | // make sure to initialise the whole struct to zero to avoid issues when 71 | // using a CAN-FD-capable device. 72 | can_frame_t can_frame = {}; 73 | can_frame.can_id = unstamped_can_frame.id; 74 | can_frame.can_dlc = unstamped_can_frame.dlc; 75 | 76 | memcpy(can_frame.data, 77 | unstamped_can_frame.data.begin(), 78 | unstamped_can_frame.dlc); 79 | 80 | // send ---------------------------------------------------------------- 81 | osi::send_to_can_device(socket, 82 | (void *)&can_frame, 83 | sizeof(can_frame_t), 84 | 0, 85 | (struct sockaddr *)&address, 86 | sizeof(address)); 87 | } 88 | 89 | CanBusFrame CanBus::receive_frame() 90 | { 91 | int socket = can_connection_.get().socket; 92 | 93 | // data we want to obtain ---------------------------------------------- 94 | can_frame_t can_frame = {}; 95 | nanosecs_abs_t timestamp; 96 | struct sockaddr_can message_address; 97 | 98 | // setup message such that data can be received to variables above ----- 99 | struct iovec input_output_vector; 100 | input_output_vector.iov_base = (void *)&can_frame; 101 | input_output_vector.iov_len = sizeof(can_frame_t); 102 | 103 | struct msghdr message_header; 104 | message_header.msg_iov = &input_output_vector; 105 | message_header.msg_iovlen = 1; 106 | message_header.msg_name = (void *)&message_address; 107 | message_header.msg_namelen = sizeof(struct sockaddr_can); 108 | message_header.msg_control = (void *)×tamp; 109 | message_header.msg_controllen = sizeof(nanosecs_abs_t); 110 | 111 | // receive message from can bus ---------------------------------------- 112 | osi::receive_message_from_can_device(socket, &message_header, 0); 113 | 114 | // process received data and put into felix widmaier's format ---------- 115 | if (message_header.msg_controllen == 0) 116 | { 117 | // No timestamp for this frame available. Make sure we dont get 118 | // garbage. 119 | timestamp = 0; 120 | } 121 | 122 | CanBusFrame out_frame; 123 | out_frame.id = can_frame.can_id; 124 | out_frame.dlc = can_frame.can_dlc; 125 | for (size_t i = 0; i < can_frame.can_dlc; i++) 126 | { 127 | out_frame.data[i] = can_frame.data[i]; 128 | } 129 | 130 | return out_frame; 131 | } 132 | 133 | CanBusConnection CanBus::setup_can(std::string name, uint32_t err_mask) 134 | { 135 | int socket_number; 136 | sockaddr_can recv_addr; 137 | sockaddr_can send_addr; 138 | struct ifreq ifr; 139 | 140 | int ret; 141 | 142 | ret = rt_dev_socket(PF_CAN, SOCK_RAW, CAN_RAW); 143 | if (ret < 0) 144 | { 145 | rt_fprintf(stderr, "rt_dev_socket: %s\n", strerror(-ret)); 146 | rt_printf("Couldn't setup CAN connection. Exit."); 147 | exit(-1); 148 | } 149 | socket_number = ret; 150 | 151 | strncpy(ifr.ifr_name, name.c_str(), IFNAMSIZ); 152 | ret = rt_dev_ioctl(socket_number, SIOCGIFINDEX, &ifr); 153 | if (ret < 0) 154 | { 155 | rt_fprintf(stderr, "rt_dev_ioctl GET_IFINDEX: %s\n", strerror(-ret)); 156 | osi::close_can_device(socket_number); 157 | rt_printf("Couldn't setup CAN connection. Exit."); 158 | exit(-1); 159 | } 160 | 161 | // Set error mask 162 | if (err_mask) 163 | { 164 | ret = rt_dev_setsockopt(socket_number, 165 | SOL_CAN_RAW, 166 | CAN_RAW_ERR_FILTER, 167 | &err_mask, 168 | sizeof(err_mask)); 169 | if (ret < 0) 170 | { 171 | rt_fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret)); 172 | osi::close_can_device(socket_number); 173 | rt_printf("Couldn't setup CAN connection. Exit."); 174 | exit(-1); 175 | } 176 | } 177 | 178 | // Bind to socket 179 | recv_addr.can_family = AF_CAN; 180 | recv_addr.can_ifindex = ifr.ifr_ifindex; 181 | ret = rt_dev_bind(socket_number, 182 | (struct sockaddr *)&recv_addr, 183 | sizeof(struct sockaddr_can)); 184 | if (ret < 0) 185 | { 186 | rt_fprintf(stderr, "rt_dev_bind: %s\n", strerror(-ret)); 187 | osi::close_can_device(socket_number); 188 | rt_printf("Couldn't setup CAN connection. Exit."); 189 | exit(-1); 190 | } 191 | 192 | #ifdef __XENO__ 193 | // Enable timestamps for frames 194 | ret = 195 | rt_dev_ioctl(socket, RTCAN_RTIOC_TAKE_TIMESTAMP, RTCAN_TAKE_TIMESTAMPS); 196 | if (ret) 197 | { 198 | rt_fprintf(stderr, "rt_dev_ioctl TAKE_TIMESTAMP: %s\n", strerror(-ret)); 199 | osi::close_can_device(socket); 200 | rt_printf("Couldn't setup CAN connection. Exit."); 201 | exit(-1); 202 | } 203 | #elif defined RT_PREEMPT 204 | // TODO: Need to support timestamps. 205 | #endif 206 | 207 | // TODO why the memset? 208 | memset(&send_addr, 0, sizeof(send_addr)); 209 | send_addr.can_family = AF_CAN; 210 | send_addr.can_ifindex = ifr.ifr_ifindex; 211 | 212 | CanBusConnection can_connection; 213 | can_connection.send_addr = send_addr; 214 | can_connection.socket = socket_number; 215 | 216 | return can_connection; 217 | } 218 | 219 | } // namespace blmc_drivers 220 | -------------------------------------------------------------------------------- /src/motor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file motor.cpp 3 | * @author Manuel Wuthrich (manuel.wuthrich@gmail.com) 4 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 5 | * @brief 6 | * @version 0.1 7 | * @date 2018-11-27 8 | * 9 | * @copyright Copyright (c) 2018 10 | * 11 | */ 12 | 13 | #include 14 | 15 | namespace blmc_drivers 16 | { 17 | Motor::Motor(Ptr board, bool motor_id) 18 | : board_(board), motor_id_(motor_id) 19 | { 20 | } 21 | 22 | Motor::Ptr Motor::get_measurement( 23 | const int& index) const 24 | { 25 | if (motor_id_ == 0) 26 | { 27 | switch (index) 28 | { 29 | case current: 30 | return board_->get_measurement(MotorBoardInterface::current_0); 31 | case position: 32 | return board_->get_measurement(MotorBoardInterface::position_0); 33 | case velocity: 34 | return board_->get_measurement(MotorBoardInterface::velocity_0); 35 | case encoder_index: 36 | return board_->get_measurement( 37 | MotorBoardInterface::encoder_index_0); 38 | } 39 | } 40 | else 41 | { 42 | switch (index) 43 | { 44 | case current: 45 | return board_->get_measurement(MotorBoardInterface::current_1); 46 | case position: 47 | return board_->get_measurement(MotorBoardInterface::position_1); 48 | case velocity: 49 | return board_->get_measurement(MotorBoardInterface::velocity_1); 50 | case encoder_index: 51 | return board_->get_measurement( 52 | MotorBoardInterface::encoder_index_1); 53 | } 54 | } 55 | 56 | throw std::invalid_argument("index needs to match one of the measurements"); 57 | } 58 | 59 | Motor::Ptr Motor::get_current_target() const 60 | { 61 | if (motor_id_ == 0) 62 | { 63 | return board_->get_control(MotorBoardInterface::current_target_0); 64 | } 65 | else 66 | { 67 | return board_->get_control(MotorBoardInterface::current_target_1); 68 | } 69 | } 70 | 71 | Motor::Ptr Motor::get_sent_current_target() const 72 | { 73 | if (motor_id_ == 0) 74 | { 75 | return board_->get_sent_control(MotorBoardInterface::current_target_0); 76 | } 77 | else 78 | { 79 | return board_->get_sent_control(MotorBoardInterface::current_target_1); 80 | } 81 | } 82 | 83 | void Motor::set_current_target(const double& current_target) 84 | { 85 | if (motor_id_ == 0) 86 | { 87 | board_->set_control(current_target, 88 | MotorBoardInterface::current_target_0); 89 | } 90 | else 91 | { 92 | board_->set_control(current_target, 93 | MotorBoardInterface::current_target_1); 94 | } 95 | } 96 | 97 | void Motor::print() const 98 | { 99 | MotorBoardStatus motor_board_status; 100 | double motor_current = std::nan(""); 101 | double motor_position = std::nan(""); 102 | double motor_velocity = std::nan(""); 103 | double motor_encoder_index = std::nan(""); 104 | double motor_sent_current_target = std::nan(""); 105 | 106 | if (board_->get_status()->length() != 0) 107 | { 108 | motor_board_status = board_->get_status()->newest_element(); 109 | } 110 | 111 | if (get_measurement(current)->length() != 0) 112 | { 113 | motor_current = get_measurement(current)->newest_element(); 114 | } 115 | 116 | if (get_measurement(position)->length() != 0) 117 | { 118 | motor_position = get_measurement(position)->newest_element(); 119 | } 120 | 121 | if (get_measurement(velocity)->length() != 0) 122 | { 123 | motor_velocity = get_measurement(velocity)->newest_element(); 124 | } 125 | 126 | if (get_measurement(encoder_index)->length() != 0) 127 | { 128 | motor_encoder_index = get_measurement(encoder_index)->newest_element(); 129 | } 130 | 131 | if (get_sent_current_target()->length() != 0) 132 | { 133 | motor_sent_current_target = get_sent_current_target()->newest_element(); 134 | } 135 | 136 | rt_printf("motor board status: "); 137 | rt_printf("enabled: %d ", motor_board_status.system_enabled); 138 | rt_printf("error_code: %d ", motor_board_status.error_code); 139 | rt_printf("motor status: "); 140 | if (motor_id_ == 0) 141 | { 142 | rt_printf("enabled: %d ", motor_board_status.motor1_enabled); 143 | rt_printf("ready: %d ", motor_board_status.motor1_ready); 144 | } 145 | else 146 | { 147 | rt_printf("enabled: %d ", motor_board_status.motor2_enabled); 148 | rt_printf("ready: %d ", motor_board_status.motor2_ready); 149 | } 150 | rt_printf("motor measurements: "); 151 | rt_printf("current: %8f ", motor_current); 152 | rt_printf("position: %8f ", motor_position); 153 | rt_printf("velocity: %8f ", motor_velocity); 154 | rt_printf("encoder index: %8f ", motor_encoder_index); 155 | rt_printf("target current: %8f ", motor_sent_current_target); 156 | rt_printf("\n"); 157 | } 158 | 159 | SafeMotor::SafeMotor(Motor::Ptr board, 160 | bool motor_id, 161 | const double& max_current_target, 162 | const size_t& history_length, 163 | const double& max_velocity) 164 | : Motor(board, motor_id), 165 | max_current_target_(max_current_target), 166 | max_velocity_(max_velocity) 167 | { 168 | current_target_ = 169 | std::make_shared(history_length, 0, false); 170 | } 171 | 172 | void SafeMotor::set_current_target(const double& current_target) 173 | { 174 | current_target_->append(current_target); 175 | 176 | // limit current to avoid overheating ---------------------------------- 177 | double safe_current_target = std::min(current_target, max_current_target_); 178 | safe_current_target = std::max(safe_current_target, -max_current_target_); 179 | 180 | // limit velocity to avoid breaking the robot -------------------------- 181 | if (!std::isnan(max_velocity_) && get_measurement(velocity)->length() > 0 && 182 | std::fabs(get_measurement(velocity)->newest_element()) > max_velocity_) 183 | safe_current_target = 0; 184 | Motor::set_current_target(safe_current_target); 185 | } 186 | 187 | } // namespace blmc_drivers 188 | -------------------------------------------------------------------------------- /src/motor_board.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file motor_board.cpp 3 | * @author Felix Widmaier (felix.widmaier@tuebingen.mpg.de) 4 | * @author Manuel Wuthrich (manuel.wuthrich@gmail.com) 5 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 6 | * @brief This file implements the classes from 7 | * "blmc_drivers/devices/motor_board.hpp" 8 | * @version 0.1 9 | * @date 2018-11-26 10 | * 11 | * @copyright Copyright (c) 2018 12 | * 13 | */ 14 | 15 | #include 16 | 17 | namespace blmc_drivers 18 | { 19 | CanBusMotorBoard::CanBusMotorBoard(std::shared_ptr can_bus, 20 | const size_t& history_length, 21 | const int& control_timeout_ms) 22 | : can_bus_(can_bus), 23 | motors_are_paused_(false), 24 | control_timeout_ms_(control_timeout_ms) 25 | { 26 | measurement_ = create_vector_of_pointers( 27 | measurement_count, history_length); 28 | status_ = std::make_shared(history_length, 0, false); 29 | control_ = create_vector_of_pointers(control_count, 30 | history_length); 31 | command_ = std::make_shared(history_length, 0, false); 32 | sent_control_ = create_vector_of_pointers(control_count, 33 | history_length); 34 | sent_command_ = 35 | std::make_shared(history_length, 0, false); 36 | 37 | is_loop_active_ = true; 38 | rt_thread_.create_realtime_thread(&CanBusMotorBoard::loop, this); 39 | } 40 | 41 | CanBusMotorBoard::~CanBusMotorBoard() 42 | { 43 | is_loop_active_ = false; 44 | rt_thread_.join(); 45 | set_command(MotorBoardCommand(MotorBoardCommand::IDs::ENABLE_SYS, 46 | MotorBoardCommand::Contents::DISABLE)); 47 | send_newest_command(); 48 | } 49 | 50 | void CanBusMotorBoard::send_if_input_changed() 51 | { 52 | // send command if a new one has been set ---------------------------------- 53 | if (command_->has_changed_since_tag()) 54 | { 55 | send_newest_command(); 56 | } 57 | 58 | // send controls if a new one has been set --------------------------------- 59 | bool controls_have_changed = false; 60 | 61 | for (auto control : control_) 62 | { 63 | if (control->has_changed_since_tag()) controls_have_changed = true; 64 | } 65 | if (controls_have_changed) 66 | { 67 | send_newest_controls(); 68 | } 69 | } 70 | 71 | void CanBusMotorBoard::wait_until_ready() 72 | { 73 | rt_printf("waiting for board and motors to be ready \n"); 74 | time_series::Index time_index = status_->newest_timeindex(); 75 | bool is_ready = false; 76 | while (!is_ready) 77 | { 78 | MotorBoardStatus status = (*status_)[time_index]; 79 | time_index++; 80 | 81 | is_ready = status.is_ready(); 82 | } 83 | rt_printf("board and motors are ready \n"); 84 | } 85 | 86 | bool CanBusMotorBoard::is_ready() 87 | { 88 | if (status_->length() == 0) 89 | { 90 | return false; 91 | } 92 | else 93 | { 94 | return status_->newest_element().is_ready(); 95 | } 96 | } 97 | 98 | void CanBusMotorBoard::pause_motors() 99 | { 100 | set_control(0, current_target_0); 101 | set_control(0, current_target_1); 102 | send_newest_controls(); 103 | 104 | set_command(MotorBoardCommand(MotorBoardCommand::IDs::SET_CAN_RECV_TIMEOUT, 105 | MotorBoardCommand::Contents::DISABLE)); 106 | send_newest_command(); 107 | 108 | motors_are_paused_ = true; 109 | } 110 | 111 | void CanBusMotorBoard::disable_can_recv_timeout() 112 | { 113 | set_command(MotorBoardCommand(MotorBoardCommand::IDs::SET_CAN_RECV_TIMEOUT, 114 | MotorBoardCommand::Contents::DISABLE)); 115 | send_newest_command(); 116 | } 117 | 118 | void CanBusMotorBoard::send_newest_controls() 119 | { 120 | if (motors_are_paused_) 121 | { 122 | set_command(MotorBoardCommand( 123 | MotorBoardCommand::IDs::SET_CAN_RECV_TIMEOUT, control_timeout_ms_)); 124 | send_newest_command(); 125 | motors_are_paused_ = false; 126 | } 127 | 128 | std::array controls; 129 | for (size_t i = 0; i < control_.size(); i++) 130 | { 131 | if (control_[i]->length() == 0) 132 | { 133 | rt_printf( 134 | "you tried to send control but no control has been set\n"); 135 | exit(-1); 136 | } 137 | 138 | Index timeindex = control_[i]->newest_timeindex(); 139 | controls[i] = (*control_[i])[timeindex]; 140 | control_[i]->tag(timeindex); 141 | 142 | sent_control_[i]->append(controls[i]); 143 | } 144 | 145 | float current_mtr1 = controls[0]; 146 | float current_mtr2 = controls[1]; 147 | 148 | uint8_t data[8]; 149 | uint32_t q_current1, q_current2; 150 | 151 | // Convert floats to Q24 values 152 | q_current1 = float_to_q24(current_mtr1); 153 | q_current2 = float_to_q24(current_mtr2); 154 | 155 | // Motor 1 156 | data[0] = (q_current1 >> 24) & 0xFF; 157 | data[1] = (q_current1 >> 16) & 0xFF; 158 | data[2] = (q_current1 >> 8) & 0xFF; 159 | data[3] = q_current1 & 0xFF; 160 | 161 | // Motor 2 162 | data[4] = (q_current2 >> 24) & 0xFF; 163 | data[5] = (q_current2 >> 16) & 0xFF; 164 | data[6] = (q_current2 >> 8) & 0xFF; 165 | data[7] = q_current2 & 0xFF; 166 | 167 | CanBusFrame can_frame; 168 | can_frame.id = CanframeIDs::IqRef; 169 | for (size_t i = 0; i < 7; i++) 170 | { 171 | can_frame.data[i] = data[i]; 172 | } 173 | can_frame.dlc = 8; 174 | 175 | can_bus_->set_input_frame(can_frame); 176 | can_bus_->send_if_input_changed(); 177 | } 178 | 179 | void CanBusMotorBoard::send_newest_command() 180 | { 181 | if (command_->length() == 0) 182 | { 183 | rt_printf("you tried to send command but no command has been set\n"); 184 | exit(-1); 185 | } 186 | 187 | Index timeindex = command_->newest_timeindex(); 188 | MotorBoardCommand command = (*command_)[timeindex]; 189 | command_->tag(timeindex); 190 | sent_command_->append(command); 191 | 192 | uint32_t id = command.id_; 193 | int32_t content = command.content_; 194 | 195 | uint8_t data[8]; 196 | 197 | // content 198 | data[0] = (content >> 24) & 0xFF; 199 | data[1] = (content >> 16) & 0xFF; 200 | data[2] = (content >> 8) & 0xFF; 201 | data[3] = content & 0xFF; 202 | 203 | // command 204 | data[4] = (id >> 24) & 0xFF; 205 | data[5] = (id >> 16) & 0xFF; 206 | data[6] = (id >> 8) & 0xFF; 207 | data[7] = id & 0xFF; 208 | 209 | CanBusFrame can_frame; 210 | can_frame.id = CanframeIDs::COMMAND_ID; 211 | for (size_t i = 0; i < 8; i++) 212 | { 213 | can_frame.data[i] = data[i]; 214 | } 215 | can_frame.dlc = 8; 216 | 217 | can_bus_->set_input_frame(can_frame); 218 | can_bus_->send_if_input_changed(); 219 | } 220 | 221 | void CanBusMotorBoard::loop() 222 | { 223 | pause_motors(); 224 | 225 | // initialize board -------------------------------------------------------- 226 | set_command(MotorBoardCommand(MotorBoardCommand::IDs::ENABLE_SYS, 227 | MotorBoardCommand::Contents::ENABLE)); 228 | send_newest_command(); 229 | 230 | set_command(MotorBoardCommand(MotorBoardCommand::IDs::SEND_ALL, 231 | MotorBoardCommand::Contents::ENABLE)); 232 | send_newest_command(); 233 | 234 | set_command(MotorBoardCommand(MotorBoardCommand::IDs::ENABLE_MTR1, 235 | MotorBoardCommand::Contents::ENABLE)); 236 | send_newest_command(); 237 | 238 | set_command(MotorBoardCommand(MotorBoardCommand::IDs::ENABLE_MTR2, 239 | MotorBoardCommand::Contents::ENABLE)); 240 | send_newest_command(); 241 | 242 | // receive data from board in a loop --------------------------------------- 243 | long int timeindex = can_bus_->get_output_frame()->newest_timeindex(); 244 | while (is_loop_active_) 245 | { 246 | CanBusFrame can_frame; 247 | Index received_timeindex = timeindex; 248 | can_frame = (*can_bus_->get_output_frame())[received_timeindex]; 249 | 250 | if (received_timeindex != timeindex) 251 | { 252 | rt_printf( 253 | "did not get the timeindex we expected! " 254 | "received_timeindex: %d, " 255 | "desired_timeindex: %d\n", 256 | int(received_timeindex), 257 | int(timeindex)); 258 | exit(-1); 259 | } 260 | 261 | timeindex++; 262 | 263 | // convert to measurement ------------------------------------------ 264 | double measurement_0 = qbytes_to_float(can_frame.data.begin()); 265 | double measurement_1 = qbytes_to_float((can_frame.data.begin() + 4)); 266 | 267 | switch (can_frame.id) 268 | { 269 | case CanframeIDs::Iq: 270 | measurement_[current_0]->append(measurement_0); 271 | measurement_[current_1]->append(measurement_1); 272 | break; 273 | case CanframeIDs::POS: 274 | // Convert the position unit from the blmc card (kilo-rotations) 275 | // into rad. 276 | measurement_[position_0]->append(measurement_0 * 2 * M_PI); 277 | measurement_[position_1]->append(measurement_1 * 2 * M_PI); 278 | break; 279 | case CanframeIDs::SPEED: 280 | // Convert the speed unit from the blmc card 281 | // (kilo-rotations-per-minutes) into rad/s. 282 | measurement_[velocity_0]->append(measurement_0 * 2 * M_PI * 283 | (1000. / 60.)); 284 | measurement_[velocity_1]->append(measurement_1 * 2 * M_PI * 285 | (1000. / 60.)); 286 | break; 287 | case CanframeIDs::ADC6: 288 | measurement_[analog_0]->append(measurement_0); 289 | measurement_[analog_1]->append(measurement_1); 290 | break; 291 | case CanframeIDs::ENC_INDEX: 292 | { 293 | // here the interpretation of the message is different, 294 | // we get a motor index and a measurement 295 | uint8_t motor_index = can_frame.data[4]; 296 | if (motor_index == 0) 297 | { 298 | measurement_[encoder_index_0]->append(measurement_0 * 2 * 299 | M_PI); 300 | } 301 | else if (motor_index == 1) 302 | { 303 | measurement_[encoder_index_1]->append(measurement_0 * 2 * 304 | M_PI); 305 | } 306 | else 307 | { 308 | rt_printf( 309 | "ERROR: Invalid motor number" 310 | "for encoder index: %d\n", 311 | motor_index); 312 | exit(-1); 313 | } 314 | break; 315 | } 316 | case CanframeIDs::STATUSMSG: 317 | { 318 | MotorBoardStatus status; 319 | uint8_t data = can_frame.data[0]; 320 | status.system_enabled = data >> 0; 321 | status.motor1_enabled = data >> 1; 322 | status.motor1_ready = data >> 2; 323 | status.motor2_enabled = data >> 3; 324 | status.motor2_ready = data >> 4; 325 | status.error_code = data >> 5; 326 | 327 | status_->append(status); 328 | break; 329 | } 330 | } 331 | 332 | // static int count = 0; 333 | // if(count % 4000 == 0) 334 | // { 335 | // print_status(); 336 | // } 337 | // count++; 338 | } 339 | } 340 | 341 | void CanBusMotorBoard::print_status() 342 | { 343 | rt_printf("ouptus ======================================\n"); 344 | rt_printf("measurements: -------------------------------\n"); 345 | for (size_t i = 0; i < measurement_.size(); i++) 346 | { 347 | rt_printf("%d: ---------------------------------\n", int(i)); 348 | if (measurement_[i]->length() > 0) 349 | { 350 | double measurement = measurement_[i]->newest_element(); 351 | rt_printf("value %f:\n", measurement); 352 | } 353 | } 354 | 355 | rt_printf("status: ---------------------------------\n"); 356 | if (status_->length() > 0) status_->newest_element().print(); 357 | 358 | // rt_printf("inputs ======================================\n"); 359 | 360 | // for(size_t i = 0; i < control_names.size(); i++) 361 | // { 362 | // rt_printf("%s: ---------------------------------\n", 363 | // control_names[i].c_str()); 364 | // if(control_.at(control_names[i])->length() > 0) 365 | // { 366 | // double control = 367 | // control_.at(control_names[i])->newest_element(); 368 | // rt_printf("value %f:\n", control); 369 | // } 370 | // } 371 | 372 | // rt_printf("command: ---------------------------------\n"); 373 | // if(command_[command]->length() > 0) 374 | // command_[command]->newest_element().print(); 375 | } 376 | 377 | } // namespace blmc_drivers 378 | -------------------------------------------------------------------------------- /src/programs/can_encoder_index_test.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file 3 | * \brief Simple test tool to check if the encoder index is detected. 4 | * 5 | * Runs a motor at a constant torque and prints a line whenever the encoder 6 | * index is detected. 7 | * 8 | * \copyright Copyright (c) 2020 Max Planck Gesellschaft. 9 | */ 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | using namespace blmc_drivers; 22 | 23 | class EncoderIndexTester 24 | { 25 | public: 26 | static constexpr double TORQUE_CONSTANT_NmpA = 0.02; 27 | static constexpr double GEAR_RATIO = 9.0; 28 | static constexpr double MAX_CURRENT_A = 2.0; 29 | static constexpr double ONE_MOTOR_ROTATION_DISTANCE = 30 | 2.0 * M_PI / GEAR_RATIO; 31 | 32 | //! The max. mrev position of a joint (i.e. the point where the position 33 | //! measurement of the board rolls over. 34 | static constexpr double MAX_MOTOR_POSITION_MREV = 100; 35 | //! The max. position transformed to joint radian. 36 | static constexpr double MAX_JOINT_POSITION_RAD = 37 | MAX_MOTOR_POSITION_MREV * 2 * M_PI / GEAR_RATIO; 38 | 39 | EncoderIndexTester(const std::string &can_port, 40 | int motor_index, 41 | double torque, 42 | unsigned int number_of_revolutions) 43 | : torque_(torque), number_of_revolutions_(number_of_revolutions) 44 | { 45 | std::cout << "CAN port: " << can_port << std::endl; 46 | std::cout << "Motor Index: " << motor_index << std::endl; 47 | std::cout << "Torque: " << torque << std::endl; 48 | 49 | std::cout << "==============================" << std::endl; 50 | std::cout << "Initialising..." << std::endl; 51 | 52 | // setup can bus 53 | auto can_bus = std::make_shared(can_port); 54 | 55 | // set up motor board 56 | motor_board_ = std::make_shared(can_bus, 1000, 10); 57 | motor_board_->wait_until_ready(); 58 | 59 | std::shared_ptr motor = 60 | std::make_shared(motor_board_, motor_index); 61 | 62 | joint_module_ = std::make_unique( 63 | motor, TORQUE_CONSTANT_NmpA, GEAR_RATIO, 0, false, MAX_CURRENT_A); 64 | } 65 | 66 | void run_print_occurences() 67 | { 68 | std::cout << "Start moving with constant torque" << std::endl; 69 | real_time_tools::Spinner spinner; 70 | spinner.set_period(0.001); 71 | double last_index_position = joint_module_->get_measured_index_angle(); 72 | 73 | // Do not print, rotate 100 times, count index ticks 74 | // check after each rotation if tick was found 75 | 76 | while (true) 77 | { 78 | joint_module_->set_torque(torque_); 79 | joint_module_->send_torque(); 80 | 81 | double index_position = joint_module_->get_measured_index_angle(); 82 | 83 | if (!std::isnan(index_position) && 84 | index_position != last_index_position) 85 | { 86 | double diff = index_position - last_index_position; 87 | std::cout << "Found Encoder Index" 88 | << ".\tPosition: " << index_position 89 | << ".\tDiff to last: " << diff << std::endl; 90 | 91 | last_index_position = index_position; 92 | } 93 | spinner.spin(); 94 | } 95 | } 96 | 97 | void run_verify_occurences() 98 | { 99 | std::cout << "Start moving with constant torque" << std::endl; 100 | real_time_tools::Spinner spinner; 101 | spinner.set_period(0.001); 102 | 103 | double last_index_position = joint_module_->get_measured_index_angle(); 104 | unsigned int counter = 0; 105 | 106 | double initial_position = joint_module_->get_measured_angle(); 107 | double move_distance = 108 | number_of_revolutions_ * ONE_MOTOR_ROTATION_DISTANCE; 109 | 110 | bool has_error = false; 111 | 112 | // Do not print, rotate N times, count index ticks 113 | // check after each rotation if tick was found 114 | while (rollover_safe_distance(initial_position, 115 | joint_module_->get_measured_angle()) < 116 | move_distance) 117 | { 118 | // check for board errors 119 | auto board_status = motor_board_->get_status()->newest_element(); 120 | if (board_status.error_code != MotorBoardStatus::ErrorCodes::NONE) 121 | { 122 | std::cout << "ERROR: " << board_status.get_error_description() 123 | << std::endl; 124 | has_error = true; 125 | break; 126 | } 127 | 128 | joint_module_->set_torque(torque_); 129 | joint_module_->send_torque(); 130 | 131 | double index_position = joint_module_->get_measured_index_angle(); 132 | 133 | if (!std::isnan(index_position) && 134 | index_position != last_index_position) 135 | { 136 | counter++; 137 | last_index_position = index_position; 138 | } 139 | spinner.spin(); 140 | } 141 | 142 | if (!has_error) 143 | { 144 | std::cout << "Finished " << number_of_revolutions_ 145 | << " revolutions." << std::endl; 146 | std::cout << "Observed encoder index tick " << counter << " times." 147 | << std::endl; 148 | } 149 | } 150 | 151 | private: 152 | double torque_; 153 | unsigned int number_of_revolutions_; 154 | std::shared_ptr motor_board_; 155 | std::unique_ptr joint_module_; 156 | 157 | /** 158 | * @brief Compute distance between positions with rollover-compensation. 159 | * 160 | * The result is only correct under the following constraints: 161 | * 162 | * - The difference is positive, i.e. p1 < p2 without rollover. 163 | * - There is at most one rollover between p1 and p2. 164 | * 165 | * @param p1 Joint angle [rad] of the lower position. 166 | * @param p2 Joint angle [rad] of the higher position. 167 | * 168 | * @return Distance between p1 and p2 assuming the above constraints are 169 | * fulfilled. 170 | */ 171 | double rollover_safe_distance(double p1, double p2) 172 | { 173 | // p2 is expected to be greater than p1. If this is not the case, there 174 | // was a rollover 175 | if (p2 < p1) 176 | { 177 | p2 += 2 * MAX_JOINT_POSITION_RAD; 178 | } 179 | double diff = p2 - p1; 180 | 181 | return diff; 182 | } 183 | }; 184 | 185 | int main(int argc, char *argv[]) 186 | { 187 | if (argc != 4 && argc != 5) 188 | { 189 | std::cout << "Invalid number of arguments." << std::endl; 190 | std::cout 191 | << "Usage: " << argv[0] 192 | << " []" 193 | << std::endl; 194 | return 1; 195 | } 196 | 197 | std::string can_port = argv[1]; 198 | int motor_index = std::stoi(argv[2]); 199 | double torque = std::stod(argv[3]); 200 | int num_revolutions = (argc == 5) ? std::stoi(argv[4]) : 0; 201 | 202 | if (motor_index != 0 && motor_index != 1) 203 | { 204 | std::cout << "Invalid motor index. Only '0' and '1' are allowed." 205 | << std::endl; 206 | return 1; 207 | } 208 | 209 | constexpr int num_revolutions_limit = 210 | 2 * EncoderIndexTester::MAX_MOTOR_POSITION_MREV; 211 | if (num_revolutions < 0) 212 | { 213 | std::cout << "Invalid Input: Number of revolutions has to be positive." 214 | << std::endl; 215 | return 1; 216 | } 217 | else if (num_revolutions >= num_revolutions_limit) 218 | { 219 | std::cout << "Invalid Input: Number of revolutions has to be < " 220 | << num_revolutions_limit << std::endl; 221 | return 1; 222 | } 223 | 224 | EncoderIndexTester tester(can_port, motor_index, torque, num_revolutions); 225 | 226 | real_time_tools::RealTimeThread thread; 227 | 228 | // If no number of revolutions are specified, use the "print occurrences" 229 | // mode. Otherwise use "verify occurrences" mode. 230 | if (num_revolutions == 0) 231 | { 232 | thread.create_realtime_thread( 233 | [](void *instance_pointer) 234 | { 235 | ((EncoderIndexTester *)(instance_pointer)) 236 | ->run_print_occurences(); 237 | return (void *)nullptr; 238 | }, 239 | &tester); 240 | } 241 | else 242 | { 243 | thread.create_realtime_thread( 244 | [](void *instance_pointer) 245 | { 246 | ((EncoderIndexTester *)(instance_pointer)) 247 | ->run_verify_occurences(); 248 | return (void *)nullptr; 249 | }, 250 | &tester); 251 | } 252 | thread.join(); 253 | 254 | return 0; 255 | } 256 | -------------------------------------------------------------------------------- /src/utils/polynome.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file polynome.cpp 3 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 4 | * @brief Polynomes object for trajectories. 5 | * 6 | * @version 0.1 7 | * @date 2019-11-07 8 | * 9 | * @copyright Copyright (c) 2019 10 | * 11 | * See 12 | * https://github.com/jrl-umi3218/jrl-walkgen/blob/master/src/Mathematics/PolynomeFoot.cpp 13 | * for further enhancement. 14 | */ 15 | 16 | #include 17 | #include 18 | 19 | namespace blmc_drivers 20 | { 21 | template <> 22 | void TimePolynome<5>::set_parameters(double final_time, 23 | double init_pose, 24 | double init_speed, 25 | double final_pose) 26 | { 27 | // save the parameter in the class and do some renaming 28 | final_time_ = final_time; 29 | init_pose_ = init_pose; 30 | init_speed_ = init_speed; 31 | init_acc_ = 0.0; 32 | final_pose_ = final_pose; 33 | final_speed_ = 0.0; 34 | final_acc_ = 0.0; 35 | // shortcuts 36 | double ft = final_time_; 37 | double ip = init_pose_; 38 | double is = init_speed; 39 | double ia = final_acc_; 40 | double fp = final_pose_; 41 | // do the computation using the analytical solution 42 | double tmp; 43 | coefficients_[0] = ip; 44 | coefficients_[1] = is; 45 | coefficients_[2] = ia / 2.0; 46 | 47 | tmp = ft * ft * ft; 48 | coefficients_[3] = 49 | (-3.0 / 2.0 * ia * ft * ft - 6.0 * is * ft - 10.0 * ip + 10.0 * fp) / 50 | tmp; 51 | tmp = tmp * ft; 52 | coefficients_[4] = 53 | (3.0 / 2.0 * ia * ft * ft + 8.0 * is * ft + 15.0 * ip - 15.0 * fp) / 54 | tmp; 55 | tmp = tmp * ft; 56 | coefficients_[5] = 57 | (-1.0 / 2.0 * ia * ft * ft - 3.0 * is * ft - 6.0 * ip + 6.0 * fp) / tmp; 58 | } 59 | 60 | } // namespace blmc_drivers 61 | -------------------------------------------------------------------------------- /tests/test_polynome.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file test_polynomes.cpp 3 | * @author Maximilien Naveau (maximilien.naveau@gmail.com) 4 | * @brief Test for the polynome.hpp classes 5 | * @version 0.1 6 | * @date 2019-11-07 7 | * 8 | * @copyright Copyright (c) 2019 9 | * 10 | */ 11 | #include 12 | #include // std::sqrt 13 | #include // std::rand, std::srand 14 | #include // std::time 15 | #include 16 | #include "blmc_drivers/utils/polynome.hpp" 17 | 18 | using namespace blmc_drivers; 19 | 20 | #define RAND_MAX_NEW 1e4 21 | 22 | /**< @brief The DISABLED_TestPolynomes class is used to disable test. */ 23 | class DISABLED_TestPolynomes : public ::testing::Test 24 | { 25 | }; 26 | 27 | /**< 28 | * @brief The TestPolynomes class: test suit template for setting up 29 | * the unit tests for the Polynomes. 30 | */ 31 | class TestPolynomes : public ::testing::Test 32 | { 33 | public: 34 | TestPolynomes() : ::testing::Test() 35 | { 36 | // use current time as seed for random generator 37 | std::srand(std::time(nullptr)); 38 | } 39 | 40 | static double rand() 41 | { 42 | double sign = 1.0; 43 | if (std::rand() % 2 == 0) 44 | { 45 | sign *= -1.0; 46 | } 47 | return sign * static_cast(std::rand()) / 48 | static_cast(RAND_MAX) * 49 | static_cast(RAND_MAX_NEW); 50 | } 51 | 52 | protected: 53 | /**< @brief SetUp, is executed before the unit tests */ 54 | void SetUp() 55 | { 56 | } 57 | 58 | /**< @brief TearDown, is executed after teh unit tests */ 59 | void TearDown() 60 | { 61 | } 62 | }; 63 | 64 | /*! Test the compute function */ 65 | TEST_F(TestPolynomes, test_order_5_constructor) 66 | { 67 | // create the polynome of order 5 68 | TimePolynome<5> polynome; 69 | 70 | ASSERT_EQ(0.0, polynome.get_init_pose()); 71 | ASSERT_EQ(0.0, polynome.get_init_speed()); 72 | ASSERT_EQ(0.0, polynome.get_init_acc()); 73 | ASSERT_EQ(0.0, polynome.get_final_pose()); 74 | ASSERT_EQ(0.0, polynome.get_final_speed()); 75 | ASSERT_EQ(0.0, polynome.get_final_acc()); 76 | ASSERT_EQ(0.0, polynome.get_final_time()); 77 | } 78 | 79 | /*! Test the compute function */ 80 | TEST_F(TestPolynomes, test_order_5) 81 | { 82 | // create the polynome of order 5 83 | TimePolynome<5> polynome; 84 | 85 | // define a random polynome 86 | double duration = std::abs(TestPolynomes::rand()); 87 | double final_pose = TestPolynomes::rand(); 88 | double init_pose = TestPolynomes::rand(); 89 | double init_speed = 0.0; 90 | double eps = 1e-8; 91 | 92 | polynome.set_parameters(duration, init_pose, init_speed, final_pose); 93 | 94 | ASSERT_EQ(polynome.compute(-eps), init_pose); 95 | ASSERT_EQ(polynome.compute_derivative(-eps), init_speed); 96 | ASSERT_EQ(polynome.compute_sec_derivative(-eps), 0.0); 97 | ASSERT_EQ(polynome.compute(duration + eps), final_pose); 98 | ASSERT_EQ(polynome.compute_derivative(duration + eps), 0.0); 99 | ASSERT_EQ(polynome.compute_sec_derivative(duration + eps), 0.0); 100 | 101 | ASSERT_EQ(init_pose, polynome.get_init_pose()); 102 | ASSERT_EQ(init_speed, polynome.get_init_speed()); 103 | ASSERT_EQ(0.0, polynome.get_init_acc()); 104 | ASSERT_EQ(final_pose, polynome.get_final_pose()); 105 | ASSERT_EQ(0.0, polynome.get_final_speed()); 106 | ASSERT_EQ(0.0, polynome.get_final_acc()); 107 | ASSERT_EQ(duration, polynome.get_final_time()); 108 | 109 | double ub = std::max(init_pose, final_pose); 110 | double lb = std::min(init_pose, final_pose); 111 | 112 | double init_time = -0.2 * duration; 113 | double final_time = duration + std::abs(init_time); 114 | double time_step = std::abs(final_time - init_time) / 10000.0; 115 | 116 | double time = init_time; 117 | while (time < (final_time + 10 * time_step)) 118 | { 119 | double value = polynome.compute(time); 120 | time += time_step; 121 | 122 | // the data is always between the init and final pose 123 | ASSERT_LE(value, ub); 124 | ASSERT_LE(lb, value); 125 | } 126 | } 127 | 128 | int main(int argc, char **argv) 129 | { 130 | ::testing::InitGoogleTest(&argc, argv); 131 | return RUN_ALL_TESTS(); 132 | } 133 | --------------------------------------------------------------------------------