├── CMakeLists.txt ├── LICENSE.txt ├── README.md ├── apache20.svg ├── config └── sample.yaml ├── include └── remote_microcontroller │ ├── accessory.hpp │ ├── config.hpp │ ├── factory.hpp │ ├── gpio.hpp │ ├── gpio_switch.hpp │ ├── implementation.hpp │ ├── interface.hpp │ ├── node.hpp │ ├── proto_gpio.hpp │ ├── proto_mgmt.hpp │ ├── proto_pul.hpp │ ├── proto_pwm.hpp │ ├── proto_service.hpp │ ├── proto_uart.hpp │ ├── puldir.hpp │ ├── puldir_stepper_driver.hpp │ ├── pwm.hpp │ ├── pwm_actuator.hpp │ ├── pwm_actuator_position.hpp │ ├── pwm_actuator_velocity.hpp │ └── uart.hpp ├── microcontrollers ├── README.md ├── arduino │ ├── Makefile │ ├── main.cpp │ ├── main.hpp │ ├── rm_gpio.cpp │ ├── rm_gpio.hpp │ ├── rm_mgmt.cpp │ ├── rm_mgmt.hpp │ ├── rm_pul.cpp │ ├── rm_pul.hpp │ ├── rm_pwm.cpp │ ├── rm_pwm.hpp │ ├── rm_service.cpp │ ├── rm_service.hpp │ ├── rm_uart.cpp │ ├── rm_uart.hpp │ └── shared.mk ├── arduino_mega2560 │ ├── Makefile │ ├── config_pul.cpp │ ├── config_pul.hpp │ ├── config_pwm.cpp │ ├── config_pwm.hpp │ ├── config_uart.cpp │ └── config_uart.hpp └── arduino_uno │ ├── Makefile │ ├── config_pul.cpp │ ├── config_pul.hpp │ ├── config_pwm.cpp │ ├── config_pwm.hpp │ ├── config_uart.cpp │ └── config_uart.hpp ├── package.xml ├── src ├── accessory.cpp ├── factory.cpp ├── gpio.cpp ├── gpio_switch.cpp ├── implementation.cpp ├── interface.cpp ├── main.cpp ├── node.cpp ├── puldir.cpp ├── puldir_stepper_driver.cpp ├── pwm.cpp ├── pwm_actuator.cpp ├── pwm_actuator_position.cpp ├── pwm_actuator_velocity.cpp └── uart.cpp ├── srv └── Reset.srv └── test └── test1.py /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(remote_microcontroller) 3 | 4 | # Compiler settings 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | if($ENV{ROS_DISTRO} STREQUAL "foxy") 14 | add_compile_options(-DNO_PARAMETER_EVENT_HANDLER) 15 | endif() 16 | 17 | # Dependencies 18 | find_package(ament_cmake REQUIRED) 19 | find_package(rclcpp REQUIRED) 20 | find_package(std_msgs REQUIRED) 21 | find_package(remote_serial REQUIRED) 22 | find_package(remote_actuator REQUIRED) 23 | find_package(remote_switch REQUIRED) 24 | find_package(remote_stepper_driver REQUIRED) 25 | find_package(rosidl_default_generators REQUIRED) 26 | 27 | # Logic shared by all targets 28 | rosidl_generate_interfaces(${PROJECT_NAME} 29 | "srv/Reset.srv" 30 | ) 31 | 32 | include_directories( 33 | include 34 | ${remote_serial_INCLUDE_DIRS} 35 | ${remote_actuator_INCLUDE_DIRS} 36 | ${remote_switch_INCLUDE_DIRS} 37 | ${remote_stepper_driver_INCLUDE_DIRS} 38 | ) 39 | 40 | set(project_SOURCE_FILES 41 | src/node.cpp 42 | src/interface.cpp 43 | src/implementation.cpp 44 | src/accessory.cpp 45 | src/pwm.cpp 46 | src/pwm_actuator.cpp 47 | src/pwm_actuator_position.cpp 48 | src/pwm_actuator_velocity.cpp 49 | src/uart.cpp 50 | src/gpio.cpp 51 | src/gpio_switch.cpp 52 | src/puldir.cpp 53 | src/puldir_stepper_driver.cpp 54 | src/accessory.cpp 55 | ) 56 | 57 | set(project_LIBRARIES 58 | ${rclcpp_LIBRARIES} 59 | ${remote_serial_LIBRARIES} 60 | ${remote_serial_native_LIBRARIES} 61 | ${remote_actuator_LIBRARIES} 62 | ${remote_actuator_native_LIBRARIES} 63 | ${remote_switch_LIBRARIES} 64 | ${remote_switch_native_LIBRARIES} 65 | ${remote_stepper_driver_LIBRARIES} 66 | ${remote_stepper_driver_native_LIBRARIES} 67 | ) 68 | 69 | set(project_DEPENDENCIES 70 | rclcpp 71 | std_msgs 72 | remote_serial 73 | remote_actuator 74 | remote_switch 75 | remote_stepper_driver 76 | ) 77 | 78 | # Executable target 79 | add_executable(${PROJECT_NAME}_standalone src/main.cpp ${project_SOURCE_FILES}) 80 | ament_target_dependencies(${PROJECT_NAME}_standalone ${project_DEPENDENCIES}) 81 | install(TARGETS 82 | ${PROJECT_NAME}_standalone 83 | DESTINATION lib/${PROJECT_NAME} 84 | ) 85 | 86 | # Library target 87 | add_library(${PROJECT_NAME}_native src/factory.cpp ${project_SOURCE_FILES}) 88 | ament_target_dependencies(${PROJECT_NAME}_native ${project_DEPENDENCIES}) 89 | ament_export_targets(${PROJECT_NAME}_native_library HAS_LIBRARY_TARGET) 90 | ament_export_dependencies(${project_DEPENDENCIES} ${modbus_DEPENDENCIES}) 91 | ament_export_include_directories(include) 92 | install(TARGETS ${PROJECT_NAME}_native 93 | EXPORT ${PROJECT_NAME}_native_library 94 | DESTINATION lib 95 | ) 96 | install( 97 | DIRECTORY include/${PROJECT_NAME}/ 98 | DESTINATION include/${PROJECT_NAME} 99 | ) 100 | 101 | 102 | if($ENV{ROS_DISTRO} STREQUAL "foxy") 103 | rosidl_target_interfaces(${PROJECT_NAME}_standalone 104 | ${PROJECT_NAME} "rosidl_typesupport_cpp") 105 | rosidl_target_interfaces(${PROJECT_NAME}_native 106 | ${PROJECT_NAME} "rosidl_typesupport_cpp") 107 | else() 108 | rosidl_get_typesupport_target(cpp_typesupport_target 109 | ${PROJECT_NAME} "rosidl_typesupport_cpp") 110 | target_link_libraries(${PROJECT_NAME}_standalone ${project_LIBRARIES} "${cpp_typesupport_target}") 111 | target_link_libraries(${PROJECT_NAME}_native ${project_LIBRARIES} "${cpp_typesupport_target}") 112 | endif() 113 | 114 | if(BUILD_TESTING) 115 | find_package(ament_lint_auto REQUIRED) 116 | # the following line skips the linter which checks for copyrights 117 | # comment the line when a copyright and license is added to all source files 118 | set(ament_cmake_copyright_FOUND TRUE) 119 | # the following line skips cpplint (only works in a git repo) 120 | # comment the line when this package is in a git repo and when 121 | # a copyright and license is added to all source files 122 | set(ament_cmake_cpplint_FOUND TRUE) 123 | ament_lint_auto_find_test_dependencies() 124 | endif() 125 | 126 | ament_package() 127 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # OpenVMP 2 | 3 | [![License](./apache20.svg)](./LICENSE.txt) 4 | 5 | This package is a part of [the OpenVMP project](https://github.com/openvmp/openvmp). 6 | But it's designed to be universal and usable independently from the rest of OpenVMP or in a combination with select OpenVMP packages. 7 | 8 | ## remote\_microcontroller 9 | 10 | This package implements a minimalistic protocol to relay input and output to 11 | and from compatible microcontrollers to leverage their I/O connectivity. 12 | 13 | ### Features 14 | 15 | The high level features: 16 | 17 | - I/O connectivity of microcontrollers is shared with other nodes 18 | - All ROS2 messaging overhead resides on the computer running this package 19 | - Microcontroller is exclusively responsible for I/O operations with 20 | the least overhead possible 21 | - Microcontrollers I/O contacts are mapped to ROS2 services and topics using 22 | YAML config files 23 | 24 | The currently supported I/O channels: 25 | 26 | - PWM 27 | - Arbitrary position and velocity control using the ROS2 built-in std\_msgs::msg::UInt16 28 | - [remote_actuator](https://github.com/openvmp/actuator) 29 | - both position and velocity variants are supported 30 | - see [remote_hardware_interface](https://github.com/openvmp/remote_hardware_interface) for info on how to consume this interface 31 | - PUL and PUL/DIR pulse train generation 32 | - Arbitrary velocity control using the ROS2 built-in std\_msgs::msg::Int32 (pulses per second, sign is the direction) 33 | - [remote_actuator](https://github.com/openvmp/actuator) for velocity control 34 | - see [remote_hardware_interface](https://github.com/openvmp/remote_hardware_interface) for info on how to consume this interface 35 | - [remote_stepper_driver](https://github.com/openvmp/stepper_driver) 36 | - pulses per revolution and other parameter tuning 37 | - provides the [remote_actuator](https://github.com/openvmp/actuator) interface too 38 | - GPIO 39 | - Arbitrary state control using the ROS2 built-in std\_msgs::msg::Boolean 40 | - [remote_switch](https://github.com/openvmp/switch) 41 | - UART ports 42 | - Arbitrary serial input and output using the ROS2 built-in std\_msgs::msg::String 43 | - [remote_serial](https://github.com/openvmp/serial) 44 | 45 | ### Supported microcontrollers 46 | 47 | It currently supports Arduino Mega2560 and Arduino Uno. 48 | But it was only tested on Arduino Mega2560. 49 | 50 | Support for other Arduino boards can be added with trivial changes. 51 | Support for other microcontroller boards would require a little more effort. 52 | Contributors are very welcome! 53 | 54 | See the details of mapping I/O channels to the pins of the microcontroller 55 | [here](./microcontrollers/README.md). 56 | 57 | ### Getting started 58 | 59 | Since this package and some of its dependencies are not yet added to ROS2 60 | distribution packages, you will have to clone all of them into the `src` 61 | folder of your ROS2 workspace. 62 | 63 | ```bash 64 | mkdir src 65 | git clone https://github.com/openvmp/actuator.git src/remote_actuator 66 | git clone https://github.com/openvmp/encoder.git src/remote_encoder 67 | git clone https://github.com/openvmp/switch.git src/remote_switch 68 | git clone https://github.com/openvmp/serial.git src/remote_serial 69 | git clone https://github.com/openvmp/microcontroller.git src/remote_microcontroller 70 | ``` 71 | 72 | After building the workspace, you can link against this package as a library 73 | 74 | ```c++ 75 | // In the declaration of your node class 76 | std::shared_ptr mcu_; 77 | 78 | /// In the definition of your node class 79 | #include 80 | ... 81 | mcu_ = remote_microcontroller::Factory::New(this, exec); // where exec is the multithreaded executor your node is running in 82 | ``` 83 | 84 | or run it as a standalone process: 85 | 86 | ```bash 87 | ros2 run remote_microcontroller remote_microcontroller_standalone ... 88 | ``` 89 | 90 | In both cases the following parameters need to be provided: 91 | 92 | - microcontroller_config: path to the configuration file 93 | - serial_is_remote: false 94 | - serial_dev_name: i.e. /dev/ttyACM0 95 | - serial_baud_rate: i.e. 115200 96 | 97 | See [remote_serial](https://github.com/openvmp/serial/blob/main/README.md) for more serial port parameters if needed. 98 | 99 | Here is an example of the configuration file for controlling a single servo: 100 | 101 | ```yaml 102 | pwm: 103 | - channel: 0 # maps to pin 2 on Arduino Mega2560 and pin 3 on Arduino Uno 104 | type: simple_pwm # fixed value, one of several supported types 105 | name: actuator0 # this will be used to produce the node name, no need to match with any other values 106 | prefix: /pwm0 # where to expose ROS2 interfaces 107 | pwm_min: 0 108 | pwm_max: 255 109 | ``` 110 | 111 | If this configuration file is used, then the following command can be used to control the servo motor: 112 | 113 | ```bash 114 | ros2 topic pub /pwm0/pwm std_msgs/msg/UInt16 '{"data":150}' 115 | ``` 116 | 117 | ### Integration with `ros2_control` 118 | 119 | The following packages are needed to integrate with `ros2_control`: 120 | 121 | ```bash 122 | git clone https://github.com/openvmp/remote_hardware_interface.git src/remote_hardware_interface 123 | ``` 124 | 125 | Add the following section to the URDF file: 126 | 127 | ```xml 128 | 129 | 130 | remote_hardware_interface/RemoteSystemInterface 131 | /robot 132 | 133 | 134 | 135 | 136 | 137 | ``` 138 | 139 | Add the following into `ros2_controllers.yaml`: 140 | 141 | ```yaml 142 | controller_manager: 143 | ros__parameters: 144 | update_rate: 20 # match this value with your performance expectations 145 | 146 | velocity_controller: 147 | type: velocity_controllers/JointGroupVelocityController 148 | 149 | velocity_controller: 150 | ros__parameters: 151 | joints: 152 | - joint0 153 | ``` 154 | 155 | Use the following configuration file (note that `simple_pwm` is replaced with `actuator_position` which provides the interface consumed by `remote_hardware_interface`) 156 | 157 | ```yaml 158 | pwm: 159 | - channel: 0 # maps to the pin 2 on Mega2560 and the pin 3 on Uno 160 | type: actuator_velocity # can be "actuator_position" depending on the type of servo 161 | name: joint0 # this will be used to produce the node name, no need to match with any other values 162 | prefix: /robot/actuator/joint0 # namespace + "/actuator/" + joint name 163 | actuator_velocity_min: -3.14 # or "actuator_position_min" 164 | actuator_velocity_max: 3.14 # or "actuator_position_max" 165 | pwm_min: 0 166 | pwm_max: 255 167 | ``` 168 | 169 | Please, note, if the channel is not specified explicitly 170 | then the natural order of channels is assumed 171 | (the first YAML entry is the channel 0, 172 | the second one is the channel 1 etc). 173 | 174 | Now launch your robot and let `ros2_control` do the work. 175 | 176 | ```bash 177 | ros2 topic pub /velocity_controller/commands std_msgs/msg/Float64MultiArray '{"data":[1.0]}' 178 | ``` 179 | 180 | ### Why not micro-ROS? 181 | 182 | Unlike this package, micro-ROS implements DDS messaging middleware on the 183 | microcontroller itself, allowing it to communicate with other ROS2 nodes over 184 | intermittent and lossy connections. 185 | 186 | In some designs where the microcontroller is permanently connected to 187 | a more powerful computer (even a Raspberry Pi), there is no benefit in using 188 | the constrained resources of a microcontroller to run DDS and to handle all 189 | the ROS2 messaging overhead. 190 | If the system is designed in a way so that the worst case scenario 191 | traffic does not saturate the permanent channel between the computer and the 192 | microcontroller, then it's better to delegate messaging functionality to the 193 | computer. 194 | 195 | If there is no permanent connection to the computer or if the worst case 196 | scenario may saturate the channel (and it is believed that QoS features will 197 | save the day), then it's better to use micro-ROS instead of this package. 198 | -------------------------------------------------------------------------------- /apache20.svg: -------------------------------------------------------------------------------- 1 | License: Apache 2.0LicenseApache 2.0 -------------------------------------------------------------------------------- /config/sample.yaml: -------------------------------------------------------------------------------- 1 | # GPIO is the only type of I/O accessories here which may cover more than 2 | # one MCU pin with a single record in this file 3 | gpio: 4 | - prefix: /gpio 5 | name: driver_gpio0 6 | channel: 13 7 | # This switch module will cover 2 channels starting with 13 8 | switch_channels: 2 9 | 10 | pul: 11 | - channel: 10 12 | dir_channel: 11 13 | prefix: /puldir0 14 | name: driver_puldir0 15 | actuator_velocity_min: -3.14 16 | actuator_velocity_max: 3.14 17 | ppr: 1200 18 | 19 | pwm: 20 | - type: actuator_position 21 | name: driver_actuator0 22 | prefix: /actuator0 23 | actuator_velocity_min: -3.14 24 | actuator_velocity_max: 3.14 25 | pwm_min: 0 26 | pwm_max: 255 27 | - type: actuator_position 28 | name: driver_actuator1 29 | prefix: /actuator1 30 | actuator_velocity_min: -3.14 31 | actuator_velocity_max: 3.14 32 | pwm_min: 0 33 | pwm_max: 255 34 | - type: simple_pwm 35 | name: driver_actuator2 36 | prefix: /pwm0 37 | pwm_min: 0 38 | pwm_max: 255 39 | 40 | uart: 41 | - prefix: /serial 42 | name: driver_serial0 43 | -------------------------------------------------------------------------------- /include/remote_microcontroller/accessory.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ACCESSORY_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ACCESSORY_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_microcontroller/implementation.hpp" 18 | 19 | namespace remote_microcontroller { 20 | 21 | class Accessory { 22 | public: 23 | Accessory(rclcpp::Node* node, Implementation* microcontroller, uint16_t addr, 24 | const std::string& prefix); 25 | virtual ~Accessory(){}; 26 | 27 | uint16_t get_addr() const { return addr_; } 28 | 29 | virtual void init(); // must set 'initialized_' 30 | virtual void read_cb(uint16_t value) = 0; 31 | virtual void stream_cb(const std::string& value) = 0; 32 | 33 | protected: 34 | rclcpp::Node* node_; 35 | Implementation* microcontroller_; 36 | uint16_t addr_; 37 | bool initialized_; 38 | 39 | const std::string get_prefix() const; 40 | 41 | void write(uint16_t value, bool force = false); 42 | void read(); 43 | void stream(const std::string& value); 44 | 45 | private: 46 | const std::string prefix_; 47 | }; 48 | 49 | } // namespace remote_microcontroller 50 | 51 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ACCESSORY_H 52 | -------------------------------------------------------------------------------- /include/remote_microcontroller/config.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2022 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2022-09-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_MICROCONTROLLER_CONFIG_H_INCLUDED 11 | #define OPENVMP_MICROCONTROLLER_CONFIG_H_INCLUDED 12 | 13 | #define MICROCONTROLLER_CONFIG_CHAPTER_GPIO "gpio" 14 | #define MICROCONTROLLER_CONFIG_CHAPTER_PUL "pul" 15 | #define MICROCONTROLLER_CONFIG_CHAPTER_PWM "pwm" 16 | #define MICROCONTROLLER_CONFIG_CHAPTER_UART "uart" 17 | 18 | namespace remote_microcontroller {} 19 | 20 | #endif // OPENVMP_MICROCONTROLLER_CONFIG_H_INCLUDED 21 | -------------------------------------------------------------------------------- /include/remote_microcontroller/factory.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_FACTORY_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_FACTORY_H 12 | 13 | #include 14 | 15 | #include "rclcpp/rclcpp.hpp" 16 | #include "remote_microcontroller/interface.hpp" 17 | 18 | namespace remote_microcontroller { 19 | 20 | class Factory { 21 | public: 22 | static std::shared_ptr New( 23 | rclcpp::Node *node, 24 | std::shared_ptr exec); 25 | }; 26 | 27 | } // namespace remote_microcontroller 28 | 29 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_FACTORY_H 30 | -------------------------------------------------------------------------------- /include/remote_microcontroller/gpio.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_GPIO_H 10 | #define OPENVMP_REMOTE_MICROCONTROLLER_GPIO_H 11 | 12 | #include 13 | #include 14 | 15 | #include "rclcpp/rclcpp.hpp" 16 | #include "remote_microcontroller/accessory.hpp" 17 | #include "remote_microcontroller/proto_gpio.hpp" 18 | 19 | namespace remote_microcontroller { 20 | 21 | class GPIO : public Accessory { 22 | public: 23 | GPIO(rclcpp::Node *node, 24 | remote_microcontroller::Implementation *microcontroller, int channel, 25 | const std::string &prefix); 26 | 27 | protected: 28 | // Set pwm using values from min to max (e.g. from 0 to 180) 29 | void gpio_set(int switch_channel, bool on); 30 | 31 | // implementation of Accessory 32 | virtual void read_cb(uint16_t value) override; 33 | virtual void stream_cb(const std::string &value) override; 34 | }; 35 | 36 | } // namespace remote_microcontroller 37 | 38 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_GPIO_H 39 | -------------------------------------------------------------------------------- /include/remote_microcontroller/gpio_switch.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_GPIO_SWITCH_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_GPIO_SWITCH_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_microcontroller/gpio.hpp" 18 | #include "remote_switch/implementation.hpp" 19 | 20 | namespace remote_microcontroller { 21 | 22 | class GPIOSwitch : public GPIO, public remote_switch::Implementation { 23 | public: 24 | GPIOSwitch(rclcpp::Node *node, 25 | remote_microcontroller::Implementation *microcontroller, 26 | int channel, const std::string &prefix); 27 | 28 | virtual void switch_handler_real_( 29 | const std::shared_ptr request, 30 | std::shared_ptr response) override; 31 | }; 32 | 33 | } // namespace remote_microcontroller 34 | 35 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_GPIO_SWITCH_H 36 | -------------------------------------------------------------------------------- /include/remote_microcontroller/implementation.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_IMPLEMENTATION_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_IMPLEMENTATION_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_microcontroller/interface.hpp" 18 | #include "remote_microcontroller/srv/reset.hpp" 19 | #include "remote_serial/implementation.hpp" 20 | 21 | #define MICROCONTROLLER_SERVICE_RESET "/reset" 22 | 23 | namespace remote_microcontroller { 24 | 25 | class Accessory; 26 | 27 | class Implementation final : public Interface { 28 | public: 29 | Implementation( 30 | rclcpp::Node *node, 31 | std::shared_ptr exec); 32 | virtual ~Implementation() {} 33 | 34 | void write(uint16_t addr, uint16_t value); 35 | void read(uint16_t addr); 36 | void stream(uint16_t addr, const std::string &value); 37 | 38 | private: 39 | bool initialized_; 40 | std::shared_ptr exec_; 41 | std::shared_ptr prov_; 42 | std::string input_queue_; 43 | std::mutex input_queue_mutex_; 44 | 45 | rclcpp::Parameter param_config_; 46 | 47 | std::map> accessories_; 48 | 49 | rclcpp::Service::SharedPtr srv_reset_; 50 | rclcpp::FutureReturnCode reset_handler_( 51 | const std::shared_ptr request, 52 | std::shared_ptr response); 53 | virtual bool reset_(bool hard, bool reflash) override; 54 | 55 | // input_cb is a static method to receive callbacks from the serial module. 56 | static void input_cb_(const std::string &msg, void *user_data); 57 | // input_cb_real_ is the real handler of the callbacks from the serial module. 58 | void input_cb_real_(const std::string &msg); 59 | }; 60 | 61 | } // namespace remote_microcontroller 62 | 63 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_IMPLEMENTATION_H 64 | -------------------------------------------------------------------------------- /include/remote_microcontroller/interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_INTERFACE_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_INTERFACE_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_actuator/interface.hpp" 18 | #include "remote_microcontroller/srv/reset.hpp" 19 | #include "remote_serial/interface.hpp" 20 | 21 | namespace remote_microcontroller { 22 | 23 | class Interface { 24 | public: 25 | Interface(rclcpp::Node *node); 26 | virtual ~Interface() {} 27 | 28 | protected: 29 | rclcpp::Node *node_; 30 | rclcpp::CallbackGroup::SharedPtr callback_group_; 31 | rclcpp::Parameter interface_prefix_; 32 | 33 | std::string get_prefix_(); 34 | 35 | virtual bool reset_(bool hard, bool reflash) = 0; 36 | 37 | private: 38 | rclcpp::Service::SharedPtr srv_reset_; 39 | 40 | rclcpp::FutureReturnCode reset_handler_( 41 | const std::shared_ptr request, 42 | std::shared_ptr response); 43 | }; 44 | 45 | } // namespace remote_microcontroller 46 | 47 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_INTERFACE_H 48 | -------------------------------------------------------------------------------- /include/remote_microcontroller/node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_NODE_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_NODE_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_microcontroller/interface.hpp" 18 | 19 | namespace remote_microcontroller { 20 | 21 | class Node : public rclcpp::Node { 22 | public: 23 | Node(std::shared_ptr exec); 24 | 25 | private: 26 | std::shared_ptr intf_; 27 | }; 28 | 29 | } // namespace remote_microcontroller 30 | 31 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_NODE_H 32 | -------------------------------------------------------------------------------- /include/remote_microcontroller/proto_gpio.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_GPIO_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_GPIO_HPP_INCLUDED 12 | 13 | #define ADDR_GPIO_MIN 200 14 | #define ADDR_GPIO_MAX 599 15 | 16 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_GPIO_HPP_INCLUDED -------------------------------------------------------------------------------- /include/remote_microcontroller/proto_mgmt.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_MGMT_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_MGMT_HPP_INCLUDED 12 | 13 | #define HEADER_1 0x97 14 | #define HEADER_2 0x39 15 | 16 | #define COMMAND_WRITE 0 // write 2 bytes 17 | #define COMMAND_READ 1 // read 2 bytes 18 | #define COMMAND_STREAM 2 // write bytes 19 | 20 | #define LENGTH_CMD_MIN 5 21 | #define LENGTH_READ_REQ 5 22 | #define LENGTH_READ_RESP 7 23 | #define LENGTH_WRITE_REQ 7 24 | #define LENGTH_STREAM_MIN 6 25 | 26 | static inline void rm_mgmt_pack5_read(uint16_t addr, uint8_t response[5]) { 27 | response[0] = HEADER_1; 28 | response[1] = HEADER_2; 29 | response[2] = COMMAND_READ; 30 | response[3] = addr >> 8; 31 | response[4] = addr; 32 | } 33 | 34 | static inline void rm_mgmt_pack7_write(uint16_t addr, uint16_t value, 35 | uint8_t response[7]) { 36 | response[0] = HEADER_1; 37 | response[1] = HEADER_2; 38 | response[2] = COMMAND_WRITE; 39 | response[3] = addr >> 8; 40 | response[4] = addr; 41 | response[5] = value >> 8; 42 | response[6] = value; 43 | } 44 | 45 | static inline void rm_mgmt_pack7_value(uint8_t command, uint16_t addr, 46 | uint16_t value, uint8_t *response) { 47 | response[0] = HEADER_1; 48 | response[1] = HEADER_2; 49 | response[2] = command; 50 | response[3] = addr >> 8; 51 | response[4] = addr; 52 | response[5] = value >> 8; 53 | response[6] = value; 54 | } 55 | 56 | static inline void rm_mgmt_pack6_stream(uint16_t addr, uint8_t len, 57 | uint8_t *response) { 58 | response[0] = HEADER_1; 59 | response[1] = HEADER_2; 60 | response[2] = COMMAND_STREAM; 61 | response[3] = addr >> 8; 62 | response[4] = addr; 63 | response[5] = len; 64 | } 65 | 66 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_MGMT_HPP_INCLUDED 67 | -------------------------------------------------------------------------------- /include/remote_microcontroller/proto_pul.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_PUL_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_PUL_HPP_INCLUDED 12 | 13 | #define ADDR_PUL_MIN 250 14 | #define ADDR_PUL_MAX 349 15 | 16 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_PUL_HPP_INCLUDED -------------------------------------------------------------------------------- /include/remote_microcontroller/proto_pwm.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_PWM_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_PWM_HPP_INCLUDED 12 | 13 | #define ADDR_PWM_MIN 0 14 | #define ADDR_PWM_MAX 99 15 | 16 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_PWM_HPP_INCLUDED -------------------------------------------------------------------------------- /include/remote_microcontroller/proto_service.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-28 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_SERVICE_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_SERVICE_HPP_INCLUDED 12 | 13 | #define ADDR_SERVICE_MIN 200 14 | #define ADDR_SERVICE_PULSE 200 15 | #define ADDR_SERVICE_MAX 249 16 | 17 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_SERVICE_HPP_INCLUDED 18 | -------------------------------------------------------------------------------- /include/remote_microcontroller/proto_uart.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_UART_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_UART_HPP_INCLUDED 12 | 13 | #define ADDR_UART_MIN 100 14 | #define ADDR_UART_MAX 199 15 | 16 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_UART_HPP_INCLUDED -------------------------------------------------------------------------------- /include/remote_microcontroller/puldir.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_PULDIR_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_PULDIR_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_microcontroller/accessory.hpp" 18 | #include "remote_microcontroller/proto_pul.hpp" 19 | #include "std_msgs/msg/int32.hpp" 20 | 21 | namespace remote_microcontroller { 22 | 23 | class PulDir : public Accessory { 24 | public: 25 | static const int PULDIR_CW = 0, PULDIR_CCW = 1; 26 | 27 | PulDir(rclcpp::Node *node, 28 | remote_microcontroller::Implementation *microcontroller, 29 | int pul_channel, int dir_channel, const std::string &prefix); 30 | 31 | protected: 32 | void puldir_set(int dir, uint16_t ppr); 33 | 34 | // implementation of Accessory 35 | virtual void read_cb(uint16_t value) override; 36 | virtual void stream_cb(const std::string &value) override; 37 | 38 | private: 39 | rclcpp::Subscription::SharedPtr topic_pps_; 40 | int dir_channel_; 41 | int last_dir_; 42 | uint16_t last_pps_; 43 | 44 | void puldir_set_callback_(const std_msgs::msg::Int32::SharedPtr); 45 | }; 46 | 47 | } // namespace remote_microcontroller 48 | 49 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_PULDIR_H 50 | -------------------------------------------------------------------------------- /include/remote_microcontroller/puldir_stepper_driver.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_PULDIR_STEPPER_DRIVER_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_PULDIR_STEPPER_DRIVER_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_microcontroller/puldir.hpp" 18 | #include "remote_stepper_driver/implementation.hpp" 19 | #include "remote_stepper_driver/srv/param_ppr_get.hpp" 20 | #include "remote_stepper_driver/srv/param_ppr_set.hpp" 21 | 22 | namespace remote_microcontroller { 23 | 24 | class PulDirStepperDriver : public PulDir, 25 | public remote_stepper_driver::Implementation { 26 | public: 27 | PulDirStepperDriver(rclcpp::Node *node, 28 | remote_microcontroller::Implementation *microcontroller, 29 | int pul_channel, int dir_channel, 30 | const std::string &prefix); 31 | rclcpp::Parameter param_ppr_; 32 | 33 | virtual bool has_position() override { return false; } 34 | virtual bool has_velocity() override { return true; } 35 | 36 | protected: 37 | virtual void position_set_real_(double) override {} 38 | virtual void velocity_set_real_(double) override; 39 | 40 | virtual rclcpp::FutureReturnCode param_ppr_get_handler_( 41 | const std::shared_ptr 42 | request, 43 | std::shared_ptr 44 | response) override; 45 | virtual rclcpp::FutureReturnCode param_ppr_set_handler_( 46 | const std::shared_ptr 47 | request, 48 | std::shared_ptr 49 | response) override; 50 | 51 | private: 52 | uint32_t ppr_; 53 | }; 54 | 55 | } // namespace remote_microcontroller 56 | 57 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_PULDIR_STEPPER_DRIVER_H 58 | -------------------------------------------------------------------------------- /include/remote_microcontroller/pwm.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_PWM_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_PWM_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_microcontroller/accessory.hpp" 18 | #include "remote_microcontroller/proto_pwm.hpp" 19 | #include "std_msgs/msg/u_int16.hpp" 20 | 21 | namespace remote_microcontroller { 22 | 23 | class PWM : public Accessory { 24 | public: 25 | PWM(rclcpp::Node *node, 26 | remote_microcontroller::Implementation *microcontroller, int index, 27 | const std::string &prefix); 28 | 29 | protected: 30 | uint16_t pwm_min_, pwm_max_; 31 | 32 | // Set pwm using values from min to max (e.g. from 0 to 180) 33 | void pwm_set(uint16_t value); 34 | 35 | // implementation of Accessory 36 | virtual void read_cb(uint16_t value) override; 37 | virtual void stream_cb(const std::string &value) override; 38 | 39 | private: 40 | rclcpp::Parameter param_min_, param_max_; 41 | rclcpp::Publisher::SharedPtr topic_pwm_min_; 42 | rclcpp::Publisher::SharedPtr topic_pwm_max_; 43 | rclcpp::Subscription::SharedPtr topic_pwm_; 44 | 45 | void pwm_set_callback_(const std_msgs::msg::UInt16::SharedPtr); 46 | }; 47 | 48 | } // namespace remote_microcontroller 49 | 50 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_PWM_H 51 | -------------------------------------------------------------------------------- /include/remote_microcontroller/pwm_actuator.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_PWM_ACTUATOR_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_PWM_ACTUATOR_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_actuator/implementation.hpp" 18 | #include "remote_microcontroller/pwm.hpp" 19 | 20 | namespace remote_microcontroller { 21 | 22 | class PWMActuator : public PWM, public remote_actuator::Implementation { 23 | public: 24 | PWMActuator(rclcpp::Node *node, 25 | remote_microcontroller::Implementation *microcontroller, 26 | int index, const std::string &prefix); 27 | }; 28 | 29 | } // namespace remote_microcontroller 30 | 31 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_PWM_ACTUATOR_H 32 | -------------------------------------------------------------------------------- /include/remote_microcontroller/pwm_actuator_position.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_PWM_ACTUATOR_POSITION_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_PWM_ACTUATOR_POSITION_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_microcontroller/pwm_actuator.hpp" 18 | 19 | namespace remote_microcontroller { 20 | 21 | class PWMActuatorPosition : public PWMActuator { 22 | public: 23 | PWMActuatorPosition(rclcpp::Node *node, 24 | remote_microcontroller::Implementation *microcontroller, 25 | int index, const std::string &prefix); 26 | 27 | virtual bool has_position() override { return true; } 28 | 29 | protected: 30 | virtual void position_set_real_(double) override; 31 | virtual void velocity_set_real_(double) override {} 32 | }; 33 | 34 | } // namespace remote_microcontroller 35 | 36 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_PWM_ACTUATOR_POSITION_H 37 | -------------------------------------------------------------------------------- /include/remote_microcontroller/pwm_actuator_velocity.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_PWM_ACTUATOR_VELOCITY_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_PWM_ACTUATOR_VELOCITY_H 12 | 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "remote_microcontroller/pwm_actuator.hpp" 18 | 19 | namespace remote_microcontroller { 20 | 21 | class PWMActuatorVelocity : public PWMActuator { 22 | public: 23 | PWMActuatorVelocity(rclcpp::Node *node, 24 | remote_microcontroller::Implementation *microcontroller, 25 | int index, const std::string &prefix); 26 | 27 | virtual bool has_velocity() override { return true; } 28 | 29 | protected: 30 | virtual void position_set_real_(double) override {} 31 | virtual void velocity_set_real_(double) override; 32 | }; 33 | 34 | } // namespace remote_microcontroller 35 | 36 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_PWM_ACTUATOR_VELOCITY_H 37 | -------------------------------------------------------------------------------- /include/remote_microcontroller/uart.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_UART_H 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_UART_H 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | #include "remote_microcontroller/accessory.hpp" 19 | #include "remote_microcontroller/implementation.hpp" 20 | #include "remote_microcontroller/proto_uart.hpp" 21 | #include "remote_serial/implementation.hpp" 22 | 23 | namespace remote_microcontroller { 24 | 25 | class UART : public Accessory, public remote_serial::Implementation { 26 | public: 27 | UART(rclcpp::Node *node, 28 | remote_microcontroller::Implementation *microcontroller, int index, 29 | const std::string &prefix); 30 | rclcpp::Parameter baud_rate; 31 | 32 | protected: 33 | // implementation of Accessory 34 | virtual void init() override; 35 | virtual void read_cb(uint16_t value) override; 36 | virtual void stream_cb(const std::string &value) override; 37 | 38 | // implementation of remote_serial::Implementation 39 | virtual void output(const std::string &msg) override; 40 | virtual void register_input_cb(void (*input_cb)(const std::string &msg, 41 | void *user_data), 42 | void *user_data) override; 43 | virtual void inject_input(const std::string &msg) override; 44 | 45 | private: 46 | // callbacks 47 | void (*volatile input_cb_)(const std::string &msg, void *user_data); 48 | void *volatile input_cb_user_data_; 49 | std::mutex input_cb_mutex_; 50 | }; 51 | 52 | } // namespace remote_microcontroller 53 | 54 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_UART_H 55 | -------------------------------------------------------------------------------- /microcontrollers/README.md: -------------------------------------------------------------------------------- 1 | # remote_microcontroller 2 | 3 | This document enumerates the supported microcontrollers, how to connect it to the computer running this package and what I/O pins are used for what purpose. 4 | 5 | ## Arduino 6 | 7 | All arduino boards have to be connected to the computer using the USB port. 8 | Don't forget to supply additional power (whether through the power port or 9 | the Vin port). 10 | 11 | Please, note, all pins are listed in the order of channels (starting with the channel 0). 12 | 13 | ### Arduino Mega2560 14 | 15 | There are 15 PWM channels: 16 | 17 | - 2-13 18 | - 44-46 19 | 20 | There are 5 UART channels: 21 | 22 | - RX: D19, TX: D18 23 | - RX: D17, TX: D16 24 | - RX: D15, TX: D14 25 | - RX: D50, TX: D51 26 | - RX: D52, TX: D53 27 | 28 | ### Arduino Uno 29 | 30 | There are 6 PWM channels: 31 | 32 | - 3, 5, 6, 9, 10, 11 33 | 34 | There is 1 UART channel: 35 | 36 | - RX: D2, TX: D3 -------------------------------------------------------------------------------- /microcontrollers/arduino/Makefile: -------------------------------------------------------------------------------- 1 | all: 2 | @echo "This is a library shared by all Arduino targets." 3 | @echo "Build the directory for the specific Arduino board instead." 4 | @exit 1 -------------------------------------------------------------------------------- /microcontrollers/arduino/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "main.hpp" 11 | 12 | #include 13 | 14 | #include "rm_gpio.hpp" 15 | #include "rm_mgmt.hpp" 16 | #include "rm_pul.hpp" 17 | #include "rm_pwm.hpp" 18 | #include "rm_service.hpp" 19 | #include "rm_uart.hpp" 20 | 21 | unsigned long now; 22 | 23 | void setup() { 24 | pinMode(13, OUTPUT); 25 | 26 | rm_gpio_setup(); 27 | rm_pul_setup(); 28 | rm_pwm_setup(); 29 | rm_service_setup(); 30 | rm_uart_setup(); 31 | 32 | rm_mgmt_setup(); 33 | } 34 | 35 | void loop() { 36 | now = micros(); 37 | 38 | while (Serial.available() > 0) { 39 | rm_mgmt_loop(); 40 | digitalWrite(13, HIGH - digitalRead(13)); // toggle led 41 | } 42 | rm_pul_loop(); 43 | rm_uart_loop(); 44 | rm_service_loop(); 45 | 46 | // TODO(clairbee): consider making delays board-specific 47 | // delay(1); 48 | } 49 | -------------------------------------------------------------------------------- /microcontrollers/arduino/main.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MAIN_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MAIN_HPP_INCLUDED 12 | 13 | extern unsigned long now; 14 | 15 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MAIN_HPP_INCLUDED -------------------------------------------------------------------------------- /microcontrollers/arduino/rm_gpio.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "rm_gpio.hpp" 11 | 12 | #include "rm_mgmt.hpp" 13 | 14 | #define GPIO_CHANNELS_NUM 54 15 | 16 | uint8_t gpio_modes[GPIO_CHANNELS_NUM]; 17 | #define GPIO_MODE_INPUT 1 18 | #define GPIO_MODE_INPUT_PULLUP 2 19 | #define GPIO_MODE_OUTPUT 3 20 | 21 | void rm_gpio_setup() { 22 | // TODO(clairbee): can we rely on .bss here? 23 | memset(&gpio_modes, 0, sizeof(gpio_modes)); 24 | } 25 | 26 | void rm_gpio_read(uint8_t addr) { 27 | if (addr < ADDR_GPIO_MIN || addr > ADDR_GPIO_MAX) { 28 | rm_mgmt_report_read(addr, 0xDEAD); 29 | return; 30 | } 31 | uint16_t channel = addr - ADDR_GPIO_MIN; 32 | 33 | // Let's agree to use channel >200 as the pull up version of the first 200 34 | // channels 35 | bool pullup = false; 36 | if (channel >= 200) { 37 | pullup = true; 38 | channel -= 100; 39 | } 40 | if (channel >= GPIO_CHANNELS_NUM) { 41 | rm_mgmt_report_read(addr, 0xDEAD); 42 | return; 43 | } 44 | 45 | if (!pullup && gpio_modes[channel] != GPIO_MODE_INPUT) { 46 | pinMode(channel, INPUT); 47 | gpio_modes[channel] = GPIO_MODE_INPUT; 48 | } else if (pullup && gpio_modes[channel] != GPIO_MODE_INPUT_PULLUP) { 49 | pinMode(channel, INPUT_PULLUP); 50 | gpio_modes[channel] = GPIO_MODE_INPUT_PULLUP; 51 | } 52 | 53 | bool result = digitalRead(channel); 54 | rm_mgmt_report_read(addr, result); 55 | } 56 | 57 | void rm_gpio_write(uint8_t addr, uint16_t value) { 58 | if (addr < ADDR_GPIO_MIN || addr > ADDR_GPIO_MAX) { 59 | // TODO(clairbee): report the error 60 | return; 61 | } 62 | uint16_t channel = addr - ADDR_GPIO_MIN; 63 | 64 | if (channel >= GPIO_CHANNELS_NUM) { 65 | // TODO(clairbee): report the error 66 | return; 67 | } 68 | 69 | if (gpio_modes[channel] != GPIO_MODE_OUTPUT) { 70 | pinMode(channel, OUTPUT); 71 | gpio_modes[channel] = GPIO_MODE_OUTPUT; 72 | } 73 | 74 | digitalWrite(channel, value ? HIGH : LOW); 75 | } 76 | -------------------------------------------------------------------------------- /microcontrollers/arduino/rm_gpio.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_GPIO_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_GPIO_HPP_INCLUDED 12 | 13 | #include 14 | 15 | #include "../../include/remote_microcontroller/proto_gpio.hpp" 16 | 17 | extern void rm_gpio_setup(); 18 | extern void rm_gpio_read(uint8_t addr); 19 | extern void rm_gpio_write(uint8_t addr, uint16_t value); 20 | 21 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_GPIO_HPP_INCLUDED -------------------------------------------------------------------------------- /microcontrollers/arduino/rm_mgmt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "rm_mgmt.hpp" 11 | 12 | #include "rm_gpio.hpp" 13 | #include "rm_pwm.hpp" 14 | #include "rm_uart.hpp" 15 | 16 | #define MGMT_INPUT_HEADER_1 0 17 | #define MGMT_INPUT_HEADER_2 1 18 | #define MGMT_INPUT_COMMAND 2 19 | #define MGMT_INPUT_ADDR_1 3 20 | #define MGMT_INPUT_ADDR_2 4 21 | #define MGMT_INPUT_VALUE_1 5 22 | #define MGMT_INPUT_VALUE_2 6 23 | #define MGMT_INPUT_LENGTH 7 24 | #define MGMT_INPUT_STRING 8 25 | int mgmt_input_state = MGMT_INPUT_HEADER_1; 26 | 27 | uint8_t command; 28 | 29 | uint16_t addr; 30 | uint16_t value; 31 | 32 | uint8_t length; 33 | uint8_t bytes_left; 34 | uint8_t *stream_data; 35 | uint16_t stream_data_size; 36 | 37 | void rm_mgmt_setup() { 38 | stream_data_size = 256; 39 | stream_data = (uint8_t *)malloc(stream_data_size); 40 | 41 | Serial.begin(115200); 42 | } 43 | 44 | void rm_mgmt_loop() { 45 | uint8_t byte = Serial.read(); 46 | 47 | switch (mgmt_input_state) { 48 | case MGMT_INPUT_HEADER_1: 49 | if (byte == HEADER_1) { 50 | mgmt_input_state = MGMT_INPUT_HEADER_2; 51 | } 52 | break; 53 | 54 | case MGMT_INPUT_HEADER_2: 55 | if (byte == HEADER_2) { 56 | mgmt_input_state = MGMT_INPUT_COMMAND; 57 | } else { 58 | // FIXME(clairbee): report the error 59 | mgmt_input_state = MGMT_INPUT_HEADER_1; 60 | } 61 | break; 62 | 63 | case MGMT_INPUT_COMMAND: 64 | command = byte; 65 | switch (command) { 66 | case COMMAND_WRITE: 67 | case COMMAND_READ: 68 | case COMMAND_STREAM: 69 | mgmt_input_state = MGMT_INPUT_ADDR_1; 70 | break; 71 | default: 72 | mgmt_input_state = MGMT_INPUT_HEADER_1; 73 | } 74 | break; 75 | 76 | case MGMT_INPUT_ADDR_1: 77 | addr = byte << 8; 78 | mgmt_input_state = MGMT_INPUT_ADDR_2; 79 | break; 80 | 81 | case MGMT_INPUT_ADDR_2: 82 | addr += byte; 83 | switch (command) { 84 | case COMMAND_WRITE: 85 | mgmt_input_state = MGMT_INPUT_VALUE_1; 86 | break; 87 | case COMMAND_READ: 88 | if (addr >= ADDR_GPIO_MIN && addr <= ADDR_GPIO_MAX) { 89 | rm_gpio_read(addr); 90 | } else { 91 | rm_mgmt_report_read(addr, 0xDEAD); 92 | } 93 | mgmt_input_state = MGMT_INPUT_HEADER_1; 94 | break; 95 | case COMMAND_STREAM: 96 | mgmt_input_state = MGMT_INPUT_LENGTH; 97 | break; 98 | } 99 | break; 100 | 101 | case MGMT_INPUT_VALUE_1: 102 | value = byte << 8; 103 | mgmt_input_state = MGMT_INPUT_VALUE_2; 104 | break; 105 | 106 | case MGMT_INPUT_VALUE_2: 107 | value += byte; 108 | 109 | if (addr >= ADDR_PWM_MIN && addr <= ADDR_PWM_MAX) { 110 | rm_pwm(addr, value); 111 | } else if (addr >= ADDR_GPIO_MIN && addr <= ADDR_GPIO_MAX) { 112 | rm_gpio_write(addr, value); 113 | } else if (addr >= ADDR_UART_MIN && addr <= ADDR_UART_MAX) { 114 | rm_uart_write(addr, value); 115 | } else { 116 | // TODO(clairbee): report the error 117 | } 118 | mgmt_input_state = MGMT_INPUT_HEADER_1; 119 | break; 120 | 121 | case MGMT_INPUT_LENGTH: 122 | length = byte; 123 | bytes_left = length; 124 | if (length == 0) { 125 | // TODO(clairbee): subscribe for inbound data 126 | mgmt_input_state = MGMT_INPUT_HEADER_1; 127 | } else { 128 | if (length > stream_data_size) { 129 | while (length > stream_data_size) { 130 | stream_data_size *= 1.5; 131 | } 132 | stream_data = (uint8_t *)realloc(stream_data, stream_data_size); 133 | } 134 | mgmt_input_state = MGMT_INPUT_STRING; 135 | } 136 | break; 137 | 138 | case MGMT_INPUT_STRING: 139 | stream_data[length - bytes_left] = byte; 140 | bytes_left--; 141 | 142 | if (bytes_left == 0) { 143 | rm_uart_stream(addr, &stream_data[0], length); 144 | mgmt_input_state = MGMT_INPUT_HEADER_1; 145 | } 146 | break; 147 | 148 | default: 149 | // FIXME(clairbee): report the error 150 | mgmt_input_state = MGMT_INPUT_HEADER_1; 151 | } 152 | } 153 | 154 | void rm_mgmt_report_read(uint16_t addr, uint16_t value) { 155 | uint8_t response[7]; 156 | rm_mgmt_pack7_value(COMMAND_READ, addr, value, response); 157 | Serial.write(&response[0], sizeof(response)); 158 | } 159 | 160 | void rm_mgmt_report_stream(uint16_t addr, uint8_t *ptr, uint8_t len) { 161 | uint8_t response[6]; 162 | rm_mgmt_pack6_stream(addr, len, response); 163 | Serial.write(&response[0], sizeof(response)); 164 | Serial.write(ptr, len); 165 | } 166 | -------------------------------------------------------------------------------- /microcontrollers/arduino/rm_mgmt.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MGMT_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MGMT_HPP_INCLUDED 12 | 13 | #include 14 | 15 | #include "../../include/remote_microcontroller/proto_mgmt.hpp" 16 | 17 | void rm_mgmt_setup(); 18 | void rm_mgmt_loop(); 19 | void rm_mgmt_report_read(uint16_t addr, uint16_t value); 20 | void rm_mgmt_report_stream(uint16_t addr, uint8_t *ptr, uint8_t len); 21 | 22 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MGMT_HPP_INCLUDED -------------------------------------------------------------------------------- /microcontrollers/arduino/rm_pul.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "rm_pul.hpp" 11 | 12 | #include 13 | 14 | #include "config_pul.hpp" 15 | #include "main.hpp" 16 | 17 | struct Pul { 18 | unsigned long micros_between_states; // half of time between pulses 19 | unsigned long micros_state_next; // next time the state must change 20 | #define PUL_STATE_UNINITIALIZED 0 21 | #define PUL_STATE_RUNNING 1 22 | #define PUL_STATE_STOPPED 2 23 | uint8_t state; 24 | uint8_t on; // last output state 25 | uint8_t stopped; // last output state 26 | int16_t next; // the index of the next one or -1 27 | }; 28 | 29 | Pul pul[PUL_CHANNELS_NUM]; 30 | 31 | // Linked list 32 | int head = -1; 33 | int *tail = nullptr; // The pointer to the "next" field of the tail of queue 34 | 35 | extern uint8_t pul_channel_to_pin(uint8_t channel); 36 | 37 | void rm_pul_setup() { 38 | // TODO(clairbee): can we rely on .bss here? 39 | memset(&pul, 0, sizeof(pul)); 40 | } 41 | 42 | void rm_pul(uint8_t addr, uint16_t value) { 43 | uint8_t channel = addr - ADDR_PUL_MIN; 44 | if (channel < 0 || channel >= PUL_CHANNELS_NUM) { 45 | return; 46 | } 47 | 48 | uint16_t pin = pul_channel_to_pin(channel); 49 | 50 | if (value == 0) { 51 | pul[channel].state = PUL_STATE_STOPPED; 52 | return; 53 | } 54 | pul[channel].stopped = 0; 55 | 56 | unsigned long delta = 500000 / value; 57 | 58 | if (pul[channel].state == PUL_STATE_UNINITIALIZED) { 59 | pinMode(pin, OUTPUT); 60 | digitalWrite(pin, HIGH); 61 | 62 | pul[channel].micros_between_states = delta; 63 | pul[channel].micros_state_next = now + pul[channel].micros_between_states; 64 | pul[channel].on = 1; 65 | pul[channel].next = -1; 66 | 67 | if (tail == nullptr) { 68 | head = channel; 69 | } else { 70 | *tail = channel; 71 | } 72 | tail = &pul[channel].next; 73 | } else { 74 | // when did we change the state last time 75 | pul[channel].micros_state_next -= pul[channel].micros_between_states; 76 | // how often do we need to change states now 77 | pul[channel].micros_between_states = delta; 78 | // when will we change the state next time 79 | pul[channel].micros_state_next += pul[channel].micros_between_states; 80 | 81 | if (pul[channel].micros_state_next < now) { 82 | pul[channel].micros_state_next = now; 83 | } 84 | } 85 | 86 | pul[channel].state = PUL_STATE_RUNNING; 87 | } 88 | 89 | void rm_pul_loop() { 90 | int next = head; 91 | while (next != -1) { 92 | if (pul[next].state == PUL_STATE_RUNNING && 93 | now >= pul[next].micros_state_next) { 94 | pul[next].micros_state_next += pul[next].micros_between_states; 95 | pul[next].on = !pul[next].on; 96 | digitalWrite(next, pul[next].on ? HIGH : LOW); 97 | } 98 | next = pul[next].next; 99 | } 100 | } -------------------------------------------------------------------------------- /microcontrollers/arduino/rm_pul.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_PUL_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_PUL_HPP_INCLUDED 12 | 13 | #include 14 | 15 | #include "../../include/remote_microcontroller/proto_pul.hpp" 16 | 17 | extern void rm_pul_setup(); 18 | extern void rm_pul(uint8_t addr, uint16_t value); 19 | extern void rm_pul_loop(); 20 | 21 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_PUL_HPP_INCLUDED -------------------------------------------------------------------------------- /microcontrollers/arduino/rm_pwm.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "rm_pwm.hpp" 11 | 12 | #include 13 | #include 14 | 15 | #include "config_pwm.hpp" 16 | 17 | Servo servo[PWM_CHANNELS_NUM]; 18 | uint8_t pwm_initialized[PWM_CHANNELS_NUM]; 19 | uint8_t servo_library_initialized; 20 | uint8_t pins_9_10_initialized; 21 | 22 | extern uint8_t pwm_channel_to_pin(uint8_t channel); 23 | 24 | void rm_pwm_setup() { 25 | // We can only use the Servo library if the PWM pins 9 and 10 are not used 26 | servo_library_initialized = 0; 27 | pins_9_10_initialized = 0; 28 | 29 | // TODO(clairbee): can we rely on .bss here? 30 | memset(&pwm_initialized, 0, sizeof(pwm_initialized)); 31 | } 32 | 33 | void rm_pwm(uint8_t addr, uint16_t value) { 34 | uint8_t channel = addr - ADDR_PWM_MIN; 35 | 36 | if (channel < 0 || channel >= PWM_CHANNELS_NUM) { 37 | return; 38 | } 39 | 40 | if (channel < PWM_CHANNELS_PWM_NUM) { 41 | // Hardware control of a PWM pin 42 | uint16_t pin = pwm_channel_to_pin(channel); 43 | 44 | if (pwm_initialized[channel] == 0) { 45 | if (pin == 9 || pin == 10) { 46 | if (servo_library_initialized != 0) { 47 | // TODO(clairbee): report an error 48 | return; 49 | } 50 | pins_9_10_initialized = 1; 51 | } 52 | 53 | pinMode(pin, OUTPUT); 54 | 55 | pwm_initialized[channel] = 1; 56 | } 57 | 58 | analogWrite(pin, value); // should be from 0 to 255 59 | } else { 60 | // Software control of an arbitrary digital pin using the Servo library 61 | uint16_t pin = channel - PWM_CHANNELS_PWM_NUM; 62 | 63 | if (pwm_initialized[channel] == 0) { 64 | if (pins_9_10_initialized != 0) { 65 | // TODO(clairbee): report an error 66 | return; 67 | } 68 | 69 | servo[channel].attach(pin); 70 | servo_library_initialized = 1; 71 | 72 | pwm_initialized[channel] = 1; 73 | } 74 | 75 | servo[channel].write(value); // should be from 0 to 180 76 | } 77 | } -------------------------------------------------------------------------------- /microcontrollers/arduino/rm_pwm.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_PWM_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_PWM_HPP_INCLUDED 12 | 13 | #include 14 | 15 | #include "../../include/remote_microcontroller/proto_pwm.hpp" 16 | 17 | extern void rm_pwm_setup(); 18 | extern void rm_pwm(uint8_t addr, uint16_t value); 19 | 20 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_PWM_HPP_INCLUDED -------------------------------------------------------------------------------- /microcontrollers/arduino/rm_service.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-28 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "rm_service.hpp" 11 | 12 | #include "../../include/remote_microcontroller/proto_service.hpp" 13 | #include "rm_mgmt.hpp" 14 | 15 | uint32_t service_cycle; 16 | 17 | void rm_service_setup() { 18 | pinMode(13, OUTPUT); 19 | digitalWrite(13, LOW); 20 | } 21 | 22 | void rm_service_loop() { 23 | service_cycle++; 24 | if (service_cycle % 100000 == 0) { 25 | digitalWrite(13, HIGH - digitalRead(13)); // toggle led 26 | rm_mgmt_report_read(ADDR_SERVICE_PULSE, service_cycle / 100000); 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /microcontrollers/arduino/rm_service.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-28 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_SERVICE_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_SERVICE_HPP_INCLUDED 12 | 13 | #include 14 | 15 | #include "../../include/remote_microcontroller/proto_uart.hpp" 16 | 17 | extern void rm_service_setup(); 18 | extern void rm_service_loop(); 19 | 20 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_SERVICE_HPP_INCLUDED 21 | -------------------------------------------------------------------------------- /microcontrollers/arduino/rm_uart.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "rm_uart.hpp" 11 | 12 | #include 13 | 14 | #include "config_uart.hpp" 15 | #include "rm_mgmt.hpp" 16 | 17 | #define CHANNELS_INVERSE_OFFSET 50 18 | 19 | extern HardwareSerial *uart_hw[UART_HW_CHANNELS_NUM]; 20 | extern SoftwareSerial *uart_sw[UART_SW_CHANNELS_NUM]; 21 | extern Stream **uart_ptr[UART_CHANNELS_NUM]; 22 | extern uint8_t uart_rx[UART_CHANNELS_NUM]; 23 | extern uint8_t uart_tx[UART_CHANNELS_NUM]; 24 | uint8_t uart_inverse[UART_CHANNELS_NUM]; 25 | 26 | #define INPUT_MAX 32 /* max characters to read per loop */ 27 | uint8_t *input_buffer; 28 | 29 | void rm_uart_setup() { 30 | // TODO(clairbee): can we rely on .bss here? 31 | memset(&uart_inverse, 0, sizeof(uart_inverse)); 32 | input_buffer = (uint8_t *)malloc(INPUT_MAX); 33 | } 34 | 35 | void rm_uart_write(uint8_t addr, uint16_t value) { 36 | if (addr < ADDR_UART_MIN || addr > ADDR_UART_MAX) { 37 | // TODO(clairbee): report the error 38 | return; 39 | } 40 | 41 | uint8_t channel = addr - ADDR_UART_MIN; 42 | bool inverse = channel >= CHANNELS_INVERSE_OFFSET; 43 | channel %= CHANNELS_INVERSE_OFFSET; 44 | if (channel >= UART_CHANNELS_NUM) { 45 | // TODO(clairbee): report the error 46 | return; 47 | } 48 | 49 | if ((*uart_ptr[channel]) == nullptr) { 50 | (*uart_ptr[channel]) = 51 | new SoftwareSerial(uart_rx[channel], uart_tx[channel], inverse); 52 | uart_inverse[channel] = inverse; 53 | } else { 54 | if (uart_inverse[channel] != inverse) { 55 | // inverse configuration mismatch 56 | // TODO(clairbee): report the error 57 | return; 58 | } 59 | } 60 | 61 | unsigned long baud_rate = 0; 62 | switch (value) { 63 | case 1: 64 | baud_rate = 300; 65 | break; 66 | case 2: 67 | baud_rate = 600; 68 | break; 69 | case 3: 70 | baud_rate = 1200; 71 | break; 72 | case 4: 73 | baud_rate = 2400; 74 | break; 75 | case 5: 76 | baud_rate = 4800; 77 | break; 78 | case 6: 79 | baud_rate = 9600; 80 | break; 81 | case 7: 82 | baud_rate = 14400; 83 | break; 84 | case 8: 85 | baud_rate = 19200; 86 | break; 87 | case 9: 88 | baud_rate = 28800; 89 | break; 90 | case 10: 91 | baud_rate = 31250; 92 | break; 93 | case 11: 94 | baud_rate = 38400; 95 | break; 96 | case 12: 97 | baud_rate = 57600; 98 | break; 99 | case 13: 100 | baud_rate = 76800; 101 | break; 102 | case 14: 103 | baud_rate = 115200; 104 | break; 105 | } 106 | 107 | if (baud_rate == 0) { 108 | return; 109 | } 110 | 111 | if (channel < UART_HW_CHANNELS_NUM) { 112 | uart_hw[channel]->begin(baud_rate); 113 | } else { 114 | uart_sw[channel - UART_HW_CHANNELS_NUM]->begin(baud_rate); 115 | } 116 | } 117 | 118 | void rm_uart_stream(uint16_t addr, uint8_t *ptr, uint8_t len) { 119 | if (addr < ADDR_UART_MIN || addr > ADDR_UART_MAX) { 120 | // TODO(clairbee): report the error 121 | return; 122 | } 123 | 124 | uint8_t channel = addr - ADDR_UART_MIN; 125 | bool inverse = channel >= CHANNELS_INVERSE_OFFSET; 126 | channel %= CHANNELS_INVERSE_OFFSET; 127 | if (channel >= UART_CHANNELS_NUM) { 128 | // TODO(clairbee): report the error 129 | return; 130 | } 131 | 132 | if ((*uart_ptr[channel]) == nullptr) { 133 | (*uart_ptr[channel]) = 134 | new SoftwareSerial(uart_rx[channel], uart_tx[channel], inverse); 135 | uart_inverse[channel] = inverse; 136 | } else { 137 | if (uart_inverse[channel] != inverse) { 138 | // inverse configuration mismatch 139 | // TODO(clairbee): report the error 140 | return; 141 | } 142 | } 143 | 144 | (*uart_ptr[channel])->write(ptr, len); 145 | } 146 | 147 | void rm_uart_loop() { 148 | for (int i = 0; i < UART_CHANNELS_NUM; i++) { 149 | if ((*uart_ptr[i]) == nullptr) { 150 | continue; 151 | } 152 | 153 | if (!(*uart_ptr[i])->available()) { 154 | continue; 155 | } 156 | 157 | int8_t input_offset = 0; 158 | do { 159 | input_buffer[input_offset++] = (*uart_ptr[i])->read(); 160 | } while ((*uart_ptr[i])->available() && input_offset < INPUT_MAX); 161 | 162 | uint16_t addr = ADDR_UART_MIN + i; 163 | if (uart_inverse[i]) { 164 | addr += CHANNELS_INVERSE_OFFSET; 165 | } 166 | rm_mgmt_report_stream(addr, input_buffer, input_offset); 167 | } 168 | } 169 | -------------------------------------------------------------------------------- /microcontrollers/arduino/rm_uart.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_UART_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_UART_HPP_INCLUDED 12 | 13 | #include 14 | 15 | #include "../../include/remote_microcontroller/proto_uart.hpp" 16 | 17 | extern void rm_uart_setup(); 18 | extern void rm_uart_write(uint8_t addr, uint16_t value); 19 | extern void rm_uart_stream(uint16_t addr, uint8_t *ptr, uint8_t len); 20 | extern void rm_uart_loop(); 21 | 22 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_UART_HPP_INCLUDED 23 | -------------------------------------------------------------------------------- /microcontrollers/arduino/shared.mk: -------------------------------------------------------------------------------- 1 | OBJDIR = ../../../../build/microcontrollers/arduino 2 | OBJDIR_CREATE = $(mkdir -p $OBJDIR) 3 | 4 | USER_LIB_PATH := ../ 5 | ARDUINO_LIBS = arduino 6 | ARDUINO_LIBS += Servo SoftwareSerial 7 | 8 | ifneq ("$(wildcard ${HOME}/Arduino/libraries/Servo)","") 9 | ARDUINO_LIB_PATH = ${HOME}/Arduino/libraries 10 | endif 11 | 12 | ifneq ("$(wildcard /etc/avrdude.conf)","") 13 | AVRDUDE_CONF = /etc/avrdude.conf 14 | endif 15 | 16 | CFLAGS = -I$(realpath .) -I../../include 17 | CXXFLAGS = -I$(realpath .) -I../../include 18 | LDFLAGS = # Ignore LDFLAGS supplied by OS or the dev environment 19 | 20 | default: clean all 21 | 22 | -include /usr/share/arduino/Arduino.mk # Linux 23 | -include /usr/local/opt/arduino-mk/Arduino.mk # MacOS 24 | -------------------------------------------------------------------------------- /microcontrollers/arduino_mega2560/Makefile: -------------------------------------------------------------------------------- 1 | BOARD_TAG = mega 2 | BOARD_SUB = atmega2560 3 | MONITOR_PORT = /dev/ttyACM0 4 | 5 | include ../arduino/shared.mk 6 | -------------------------------------------------------------------------------- /microcontrollers/arduino_mega2560/config_pul.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "config_pul.hpp" 11 | 12 | #include 13 | 14 | uint8_t pul_channel_to_pin(uint8_t channel) { return channel; } -------------------------------------------------------------------------------- /microcontrollers/arduino_mega2560/config_pul.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MEGA2560_PUL_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MEGA2560_PUL_HPP_INCLUDED 12 | 13 | #define PUL_CHANNELS_NUM 54 14 | 15 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MEGA2560_PUL_HPP_INCLUDED -------------------------------------------------------------------------------- /microcontrollers/arduino_mega2560/config_pwm.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "config_pwm.hpp" 11 | 12 | #include 13 | 14 | uint8_t pwm_channel_to_pin(uint8_t channel) { 15 | if (channel >= 12) { 16 | return 44 + channel - 12; 17 | } else { 18 | return 2 + channel; 19 | } 20 | } -------------------------------------------------------------------------------- /microcontrollers/arduino_mega2560/config_pwm.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MEGA2560_PWM_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MEGA2560_PWM_HPP_INCLUDED 12 | 13 | #define PWM_CHANNELS_PWM_NUM 15 14 | #define PWM_CHANNELS_NUM (15 + 54) 15 | 16 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MEGA2560_PWM_HPP_INCLUDED -------------------------------------------------------------------------------- /microcontrollers/arduino_mega2560/config_uart.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "config_uart.hpp" 11 | 12 | #include 13 | #include 14 | 15 | HardwareSerial *uart_hw[UART_HW_CHANNELS_NUM] = { 16 | &Serial1, 17 | &Serial2, 18 | &Serial3, 19 | }; 20 | SoftwareSerial *uart_sw[UART_SW_CHANNELS_NUM] = { 21 | nullptr, 22 | nullptr, 23 | }; 24 | Stream **uart_ptr[UART_CHANNELS_NUM] = { 25 | &uart_hw[0], &uart_hw[1], &uart_hw[2], &uart_sw[0], &uart_sw[1], 26 | }; 27 | uint8_t uart_rx[UART_CHANNELS_NUM] = {19, 17, 15, 50, 52}; 28 | uint8_t uart_tx[UART_CHANNELS_NUM] = {18, 16, 14, 51, 53}; 29 | -------------------------------------------------------------------------------- /microcontrollers/arduino_mega2560/config_uart.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MEGA2560_UART_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MEGA2560_UART_HPP_INCLUDED 12 | 13 | #define UART_CHANNELS_NUM 5 14 | #define UART_HW_CHANNELS_NUM 3 15 | #define UART_SW_CHANNELS_NUM 2 16 | 17 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_MEGA2560_UART_HPP_INCLUDED 18 | -------------------------------------------------------------------------------- /microcontrollers/arduino_uno/Makefile: -------------------------------------------------------------------------------- 1 | BOARD_TAG = uno 2 | MONITOR_PORT = /dev/ttyACM0 3 | 4 | include ../arduino/shared.mk -------------------------------------------------------------------------------- /microcontrollers/arduino_uno/config_pul.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "config_pul.hpp" 11 | 12 | #include 13 | 14 | uint8_t pul_channel_to_pin(uint8_t channel) { return channel; } -------------------------------------------------------------------------------- /microcontrollers/arduino_uno/config_pul.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_UNO_PUL_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_UNO_PUL_HPP_INCLUDED 12 | 13 | #define PUL_CHANNELS_NUM 14 14 | 15 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_UNO_PUL_HPP_INCLUDED -------------------------------------------------------------------------------- /microcontrollers/arduino_uno/config_pwm.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "config_pwm.hpp" 11 | 12 | #include 13 | 14 | static uint8_t pwm_channel_to_pin_map[6] = {3, 5, 6, 9, 10, 11}; 15 | 16 | uint8_t pwm_channel_to_pin(uint8_t channel) { 17 | return pwm_channel_to_pin_map[channel]; 18 | } -------------------------------------------------------------------------------- /microcontrollers/arduino_uno/config_pwm.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_UNO_PWM_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_UNO_PWM_HPP_INCLUDED 12 | 13 | #define PWM_CHANNELS_PWM_NUM 6 14 | #define PWM_CHANNELS_NUM (6 + 14) 15 | 16 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_UNO_PWM_HPP_INCLUDED 17 | -------------------------------------------------------------------------------- /microcontrollers/arduino_uno/config_uart.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "config_uart.hpp" 11 | 12 | #include 13 | #include 14 | 15 | HardwareSerial *uart_hw[UART_HW_CHANNELS_NUM] = {}; 16 | SoftwareSerial *uart_sw[UART_SF_CHANNELS_NUM] = {nullptr}; 17 | Stream **uart_ptr[UART_CHANNELS_NUM] = {&uart_sw[0]}; 18 | uint8_t uart_rx[UART_CHANNELS_NUM] = {2}; 19 | uint8_t uart_tx[UART_CHANNELS_NUM] = {3}; 20 | -------------------------------------------------------------------------------- /microcontrollers/arduino_uno/config_uart.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #ifndef OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_UNO_UART_HPP_INCLUDED 11 | #define OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_UNO_UART_HPP_INCLUDED 12 | 13 | #define UART_CHANNELS_NUM 1 14 | #define UART_HW_CHANNELS_NUM 0 15 | #define UART_SW_CHANNELS_NUM 1 16 | 17 | #endif // OPENVMP_REMOTE_MICROCONTROLLER_ARDUINO_UNO_UART_HPP_INCLUDED 18 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | remote_microcontroller 5 | 0.0.1 6 | Proxy input and output to and from compatible microcontrollers to leverage their I/O 7 | connectivity 8 | Roman Kuzmenko 9 | Apache License 2.0 10 | 11 | ament_cmake 12 | rclcpp 13 | std_msgs 14 | remote_serial 15 | remote_actuator 16 | remote_switch 17 | remote_stepper_driver 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | rosidl_default_generators 23 | rosidl_default_runtime 24 | rosidl_interface_packages 25 | 26 | 27 | ament_cmake 28 | 29 | -------------------------------------------------------------------------------- /src/accessory.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/accessory.hpp" 11 | 12 | namespace remote_microcontroller { 13 | 14 | Accessory::Accessory(rclcpp::Node* node, Implementation* microcontroller, 15 | uint16_t addr, const std::string& prefix) 16 | : node_{node}, 17 | microcontroller_{microcontroller}, 18 | addr_{addr}, 19 | initialized_{false}, 20 | prefix_{prefix} {} 21 | 22 | const std::string Accessory::get_prefix() const { 23 | std::string prefix = node_->get_namespace(); 24 | 25 | if (prefix == "/") prefix = ""; 26 | prefix += prefix_; 27 | 28 | return prefix; 29 | } 30 | 31 | void Accessory::init() { initialized_ = true; } 32 | 33 | void Accessory::write(uint16_t value, bool force) { 34 | if (initialized_ || force) { 35 | microcontroller_->write(addr_, value); 36 | } 37 | } 38 | 39 | void Accessory::read() { 40 | if (initialized_) { 41 | microcontroller_->read(addr_); 42 | } 43 | } 44 | 45 | void Accessory::stream(const std::string& value) { 46 | if (initialized_) { 47 | microcontroller_->stream(addr_, value); 48 | } 49 | } 50 | 51 | } // namespace remote_microcontroller 52 | -------------------------------------------------------------------------------- /src/factory.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/factory.hpp" 11 | 12 | #include "remote_microcontroller/implementation.hpp" 13 | 14 | namespace remote_microcontroller { 15 | 16 | std::shared_ptr Factory::New( 17 | rclcpp::Node* node, 18 | std::shared_ptr exec) { 19 | return std::make_shared(node, exec); 20 | } 21 | 22 | } // namespace remote_microcontroller 23 | -------------------------------------------------------------------------------- /src/gpio.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/gpio.hpp" 11 | 12 | namespace remote_microcontroller { 13 | 14 | GPIO::GPIO(rclcpp::Node *node, 15 | remote_microcontroller::Implementation *microcontroller, int channel, 16 | const std::string &prefix) 17 | : Accessory(node, microcontroller, ((uint16_t)(ADDR_GPIO_MIN + channel)), 18 | prefix) {} 19 | 20 | void GPIO::gpio_set(int switch_channel, bool on) { 21 | // bypass Accessory::write() to control the address value 22 | microcontroller_->write(addr_ + switch_channel, on); 23 | } 24 | 25 | // GPIO does not use register read ops 26 | void GPIO::read_cb(uint16_t value) { (void)value; } 27 | // GPIO does not use stream ops 28 | void GPIO::stream_cb(const std::string &value) { (void)value; } 29 | 30 | } // namespace remote_microcontroller 31 | -------------------------------------------------------------------------------- /src/gpio_switch.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/gpio_switch.hpp" 11 | 12 | namespace remote_microcontroller { 13 | 14 | GPIOSwitch::GPIOSwitch(rclcpp::Node *node, 15 | remote_microcontroller::Implementation *microcontroller, 16 | int index, const std::string &prefix) 17 | : GPIO(node, microcontroller, index, prefix), 18 | remote_switch::Implementation(node) {} 19 | 20 | void GPIOSwitch::switch_handler_real_( 21 | const std::shared_ptr request, 22 | std::shared_ptr response) { 23 | (void)response; 24 | gpio_set(request->channel, request->on); 25 | } 26 | 27 | } // namespace remote_microcontroller -------------------------------------------------------------------------------- /src/implementation.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/implementation.hpp" 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include "remote_microcontroller/config.hpp" 19 | #include "remote_microcontroller/gpio_switch.hpp" 20 | #include "remote_microcontroller/interface.hpp" 21 | #include "remote_microcontroller/proto_mgmt.hpp" 22 | #include "remote_microcontroller/proto_service.hpp" 23 | #include "remote_microcontroller/puldir_stepper_driver.hpp" 24 | #include "remote_microcontroller/pwm_actuator_position.hpp" 25 | #include "remote_microcontroller/pwm_actuator_velocity.hpp" 26 | #include "remote_microcontroller/uart.hpp" 27 | #include "remote_serial/factory.hpp" 28 | #include "remote_serial/utils.hpp" 29 | 30 | namespace remote_microcontroller { 31 | 32 | Implementation::Implementation( 33 | rclcpp::Node *node, 34 | std::shared_ptr exec) 35 | : Interface{node}, initialized_{false}, exec_{exec} { 36 | auto prefix = get_prefix_(); 37 | std::string ns = node_->get_namespace(); 38 | 39 | prov_ = remote_serial::Factory::New(node); 40 | prov_->register_input_cb(&Implementation::input_cb_, this); 41 | 42 | srv_reset_ = node_->create_service( 43 | prefix + MICROCONTROLLER_SERVICE_RESET, 44 | std::bind(&Implementation::reset_handler_, this, std::placeholders::_1, 45 | std::placeholders::_2), 46 | ::rmw_qos_profile_default, callback_group_); 47 | 48 | node->declare_parameter("microcontroller_config", 49 | "config/microcontroller.yaml"); 50 | node->get_parameter("microcontroller_config", param_config_); 51 | 52 | // Static part is done, now dynamic behavior driven by the config 53 | auto config_filename = param_config_.as_string(); 54 | auto config_file = fopen(config_filename.c_str(), "r"); 55 | yaml_parser_t parser; 56 | yaml_document_t doc; 57 | yaml_parser_initialize(&parser); 58 | yaml_parser_set_input_file(&parser, config_file); 59 | if (yaml_parser_load(&parser, &doc)) { 60 | RCLCPP_DEBUG(node->get_logger(), "YAML file is loaded"); 61 | yaml_node_t *root = yaml_document_get_root_node(&doc); 62 | 63 | if (root && root->type == YAML_MAPPING_NODE) { 64 | for (auto root_item = root->data.mapping.pairs.start; 65 | root_item < root->data.mapping.pairs.end; root_item++) { 66 | RCLCPP_DEBUG(node->get_logger(), "root item is found"); 67 | auto root_key = yaml_document_get_node(&doc, root_item->key); 68 | if (!root_key || root_key->type != YAML_SCALAR_NODE) continue; 69 | 70 | auto accessories = yaml_document_get_node(&doc, root_item->value); 71 | if (!accessories || accessories->type != YAML_SEQUENCE_NODE) continue; 72 | RCLCPP_DEBUG(node->get_logger(), "root item is a sequence"); 73 | 74 | std::string chapter((char *)root_key->data.scalar.value); 75 | 76 | int i; 77 | yaml_node_item_t *accessory_item; 78 | for (accessory_item = accessories->data.sequence.items.start, i = 0; 79 | accessory_item < accessories->data.sequence.items.top; 80 | ++accessory_item, ++i) { 81 | RCLCPP_DEBUG(node->get_logger(), "accessory item is found"); 82 | auto accessory = yaml_document_get_node(&doc, *accessory_item); 83 | if (!accessory || accessory->type != YAML_MAPPING_NODE) continue; 84 | RCLCPP_DEBUG(node->get_logger(), "accessory item is a map"); 85 | 86 | std::string node_name; 87 | std::string node_prefix; 88 | int channel = i; 89 | int dir_channel = -1; // used by puldir only 90 | std::string type; // used by pwm only 91 | auto node_options = rclcpp::NodeOptions{}; 92 | 93 | for (auto param_item = accessory->data.mapping.pairs.start; 94 | param_item < accessory->data.mapping.pairs.end; param_item++) { 95 | RCLCPP_DEBUG(node->get_logger(), "param item is found: %d", 96 | param_item->key); 97 | auto param_key = yaml_document_get_node(&doc, param_item->key); 98 | if (!param_key || param_key->type != YAML_SCALAR_NODE) continue; 99 | RCLCPP_DEBUG(node->get_logger(), "param item is a scalar"); 100 | auto key = std::string((char *)param_key->data.scalar.value); 101 | RCLCPP_DEBUG(node->get_logger(), "param item is %s", 102 | (char *)param_key->data.scalar.value); 103 | 104 | auto param_value = yaml_document_get_node(&doc, param_item->value); 105 | if (!param_value || param_value->type != YAML_SCALAR_NODE) continue; 106 | RCLCPP_DEBUG(node->get_logger(), "param value is a scalar"); 107 | 108 | if (key == "type") { 109 | type = std::string((char *)param_value->data.scalar.value); 110 | } else if (key == "name") { 111 | node_name = "driver_microcontroller_" + 112 | std::string((char *)param_value->data.scalar.value); 113 | } else if (key == "prefix") { 114 | node_prefix = std::string((char *)param_value->data.scalar.value); 115 | } else if (key == "channel") { 116 | channel = std::atoi((char *)param_value->data.scalar.value); 117 | } else if (key == "dir_channel") { 118 | dir_channel = std::atoi((char *)param_value->data.scalar.value); 119 | } else { 120 | std::string string_value((char *)param_value->data.scalar.value); 121 | const char *start = string_value.c_str(); 122 | const char *end = start + strlen(start); 123 | 124 | int int_value; 125 | auto res = std::from_chars(start, end, int_value); 126 | if (res.ec == std::errc{} && !*res.ptr) { 127 | node_options.parameter_overrides().push_back({key, int_value}); 128 | } else { 129 | double double_value; 130 | #ifdef __clang__ 131 | if (sscanf(start, "%lf", &double_value) == 1) 132 | #else 133 | res = std::from_chars(start, end, double_value); 134 | if (res.ec == std::errc{} && !*res.ptr) 135 | #endif 136 | { 137 | node_options.parameter_overrides().push_back( 138 | {key, double_value}); 139 | } else { 140 | node_options.parameter_overrides().push_back( 141 | {key, string_value}); 142 | } 143 | } 144 | } 145 | } 146 | 147 | if (node_name == "") { 148 | node_name = 149 | "driver_microcontroller_" + chapter + std::to_string(channel); 150 | } 151 | 152 | // Create the node 153 | RCLCPP_DEBUG(node->get_logger(), "creating the node"); 154 | node_options.use_intra_process_comms(true); 155 | auto accessory_node = 156 | std::make_shared(node_name, ns, node_options); 157 | exec->add_node(accessory_node); 158 | 159 | // Instantiate the accessory 160 | RCLCPP_DEBUG(node->get_logger(), "instantiating the accessory"); 161 | std::shared_ptr ptr; 162 | if (chapter == MICROCONTROLLER_CONFIG_CHAPTER_GPIO) { 163 | ptr = std::make_shared(accessory_node.get(), this, 164 | channel, node_prefix); 165 | RCLCPP_DEBUG(node->get_logger(), "instantiated GPIO"); 166 | } else if (chapter == MICROCONTROLLER_CONFIG_CHAPTER_PUL) { 167 | ptr = std::make_shared( 168 | accessory_node.get(), this, channel, dir_channel, node_prefix); 169 | RCLCPP_DEBUG(node->get_logger(), 170 | "instantiated PulDirStepperDriver"); 171 | } else if (chapter == MICROCONTROLLER_CONFIG_CHAPTER_PWM) { 172 | if (type == "actuator_position") { 173 | RCLCPP_DEBUG(node_->get_logger(), 174 | "Found a position actuator entry"); 175 | ptr = std::make_shared( 176 | accessory_node.get(), this, channel, node_prefix); 177 | RCLCPP_DEBUG(node->get_logger(), 178 | "instantiated PWMActuatorVelocity"); 179 | } else if (type == "actuator_position") { 180 | RCLCPP_DEBUG(node_->get_logger(), 181 | "Found a velocity actuator entry"); 182 | ptr = std::make_shared( 183 | accessory_node.get(), this, channel, node_prefix); 184 | RCLCPP_DEBUG(node->get_logger(), 185 | "instantiated PWMActuatorPosition"); 186 | } else { 187 | RCLCPP_DEBUG(node_->get_logger(), "Found a simple pwm entry"); 188 | ptr = std::make_shared(accessory_node.get(), this, channel, 189 | node_prefix); 190 | RCLCPP_DEBUG(node->get_logger(), "instantiated PWM"); 191 | } 192 | } else if (chapter == MICROCONTROLLER_CONFIG_CHAPTER_UART) { 193 | ptr = std::make_shared(accessory_node.get(), this, channel, 194 | node_prefix); 195 | RCLCPP_DEBUG(node->get_logger(), "instantiated UART"); 196 | } 197 | 198 | // Store the accessory in the collection 199 | accessories_.insert({ptr->get_addr(), ptr}); 200 | } 201 | } 202 | } 203 | RCLCPP_DEBUG(node->get_logger(), "Destroying the document"); 204 | yaml_document_delete(&doc); 205 | } 206 | RCLCPP_DEBUG(node->get_logger(), "Destroying the parser"); 207 | yaml_parser_delete(&parser); 208 | } 209 | 210 | bool Implementation::reset_(bool hard, bool reflash) { 211 | (void)hard; 212 | (void)reflash; 213 | // TODO(clairbee): implement reset 214 | return false; 215 | } 216 | 217 | void Implementation::write(uint16_t addr, uint16_t value) { 218 | uint8_t cmd[7]; 219 | rm_mgmt_pack7_write(addr, value, cmd); 220 | prov_->output(std::string((char *)&cmd[0], sizeof(cmd))); 221 | } 222 | 223 | void Implementation::read(uint16_t addr) { 224 | uint8_t cmd[5]; 225 | rm_mgmt_pack5_read(addr, cmd); 226 | prov_->output(std::string((char *)&cmd[0], sizeof(cmd))); 227 | } 228 | 229 | void Implementation::stream(uint16_t addr, const std::string &value) { 230 | uint8_t cmd[6]; 231 | rm_mgmt_pack6_stream(addr, value.length(), cmd); 232 | prov_->output(std::string((char *)&cmd[0], sizeof(cmd)) + value); 233 | } 234 | 235 | /* static */ void Implementation::input_cb_(const std::string &msg, 236 | void *user_data) { 237 | (void)msg; 238 | (void)user_data; 239 | 240 | Implementation *that = (Implementation *)user_data; 241 | that->input_cb_real_(msg); 242 | } 243 | 244 | void Implementation::input_cb_real_(const std::string &msg) { 245 | RCLCPP_DEBUG(node_->get_logger(), "Received data: %s", 246 | (remote_serial::utils::bin2hex(msg)).c_str()); 247 | 248 | input_queue_mutex_.lock(); 249 | input_queue_ += msg; // TODO(clairbee): optimize it to reduce extra copying 250 | RCLCPP_DEBUG(node_->get_logger(), "Queued data: %s", 251 | (remote_serial::utils::bin2hex(input_queue_)).c_str()); 252 | 253 | while (input_queue_.length() >= LENGTH_CMD_MIN) { 254 | if ((uint8_t)input_queue_[0] != HEADER_1) { 255 | input_queue_ = input_queue_.substr(1); 256 | continue; 257 | } 258 | if ((uint8_t)input_queue_[1] != HEADER_2) { 259 | input_queue_ = input_queue_.substr(2); 260 | continue; 261 | } 262 | 263 | // With some level of confidence we can assume that we are receiving data 264 | // from a properly programmed controller. 265 | // Now it's time to initialize all accessories if it has not been done yet. 266 | if (!initialized_) { 267 | for (auto &acc_it : accessories_) { 268 | acc_it.second->init(); 269 | } 270 | initialized_ = true; 271 | } 272 | 273 | if (input_queue_[2] == COMMAND_READ) { 274 | if (input_queue_.length() < LENGTH_READ_RESP) { 275 | // Not enough data yet 276 | break; 277 | } 278 | 279 | uint16_t addr = ((uint8_t)input_queue_[3]) << 8; 280 | addr += ((uint8_t)input_queue_[4]); 281 | 282 | if (addr >= ADDR_SERVICE_MIN && addr <= ADDR_SERVICE_MAX) { 283 | // Handle service traffic within this module. 284 | // TODO(clairbee): handle ping messages 285 | } else { 286 | // Treat everything else as traffic related to one of the 287 | // accessories. 288 | auto acc_it = accessories_.find(addr); 289 | if (acc_it == accessories_.end()) { 290 | RCLCPP_ERROR(node_->get_logger(), 291 | "Received READ from an unknown accessory: %d", addr); 292 | } else { 293 | uint16_t value = ((uint8_t)input_queue_[5]) << 8; 294 | value += ((uint8_t)input_queue_[6]); 295 | 296 | acc_it->second->read_cb(value); 297 | } 298 | } 299 | 300 | input_queue_ = input_queue_.substr(LENGTH_READ_RESP); 301 | } else if (input_queue_[2] == COMMAND_STREAM) { 302 | if (input_queue_.length() < LENGTH_STREAM_MIN) { 303 | // Not enough data yet 304 | break; 305 | } 306 | 307 | uint8_t len = ((uint8_t)input_queue_[5]); 308 | if ((int)input_queue_.length() < LENGTH_STREAM_MIN + len) { 309 | // Not enough data yet 310 | break; 311 | } 312 | 313 | uint16_t addr = ((uint8_t)input_queue_[3]) << 8; 314 | addr += ((uint8_t)input_queue_[4]); 315 | 316 | auto acc_it = accessories_.find(addr); 317 | if (acc_it == accessories_.end()) { 318 | RCLCPP_ERROR(node_->get_logger(), 319 | "Received STREAM from an unknown accessory: %d", addr); 320 | } else { 321 | acc_it->second->stream_cb( 322 | std::string((char *)&input_queue_[LENGTH_STREAM_MIN], len)); 323 | } 324 | 325 | input_queue_ = input_queue_.substr(LENGTH_STREAM_MIN + len); 326 | } else { 327 | RCLCPP_ERROR(node_->get_logger(), 328 | "Received unknown command from an unknown accessory: %d", 329 | input_queue_[2]); 330 | input_queue_ = input_queue_.substr(2); // remove the header only 331 | } 332 | } 333 | input_queue_mutex_.unlock(); 334 | } 335 | 336 | rclcpp::FutureReturnCode Implementation::reset_handler_( 337 | const std::shared_ptr request, 338 | std::shared_ptr response) { 339 | response->success = reset_(request->hard, request->reflash); 340 | return rclcpp::FutureReturnCode::SUCCESS; 341 | } 342 | 343 | } // namespace remote_microcontroller 344 | -------------------------------------------------------------------------------- /src/interface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/interface.hpp" 11 | 12 | #include "remote_actuator/factory.hpp" 13 | #include "remote_serial/factory.hpp" 14 | 15 | namespace remote_microcontroller { 16 | 17 | Interface::Interface(rclcpp::Node *node) : node_{node} { 18 | callback_group_ = 19 | node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); 20 | 21 | node->declare_parameter("microcontroller_prefix", 22 | "/microcontroller/" + std::string(node_->get_name())); 23 | node->get_parameter("microcontroller_prefix", interface_prefix_); 24 | 25 | auto prefix = get_prefix_(); 26 | } 27 | 28 | std::string Interface::get_prefix_() { 29 | std::string prefix = std::string(node_->get_namespace()); 30 | if (prefix.length() > 0 && prefix[prefix.length() - 1] == '/') { 31 | prefix = prefix.substr(0, prefix.length() - 1); 32 | } 33 | prefix += interface_prefix_.as_string(); 34 | return prefix; 35 | } 36 | 37 | } // namespace remote_microcontroller 38 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "rclcpp/rclcpp.hpp" 11 | #include "remote_microcontroller/node.hpp" 12 | 13 | int main(int argc, char **argv) { 14 | rclcpp::init(argc, argv); 15 | auto exec = std::make_shared(); 16 | std::shared_ptr node; 17 | 18 | auto thread = std::thread([exec, &node]() { 19 | node = std::make_shared(exec); 20 | exec->add_node(node); 21 | }); 22 | 23 | exec->spin(); 24 | exec.reset(); 25 | 26 | rclcpp::shutdown(); 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /src/node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-26 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/node.hpp" 11 | 12 | #include "remote_microcontroller/implementation.hpp" 13 | 14 | namespace remote_microcontroller { 15 | 16 | Node::Node(std::shared_ptr exec) 17 | : rclcpp::Node::Node("remote_microcontroller") { 18 | ; 19 | intf_ = std::make_shared(this, exec); 20 | } 21 | 22 | } // namespace remote_microcontroller 23 | -------------------------------------------------------------------------------- /src/puldir.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/puldir.hpp" 11 | 12 | #include "remote_microcontroller/proto_gpio.hpp" 13 | 14 | namespace remote_microcontroller { 15 | 16 | PulDir::PulDir(rclcpp::Node *node, 17 | remote_microcontroller::Implementation *microcontroller, 18 | int pul_channel, int dir_channel, const std::string &prefix) 19 | : Accessory(node, microcontroller, ((uint16_t)(ADDR_PUL_MIN + pul_channel)), 20 | prefix), 21 | dir_channel_{dir_channel} { 22 | topic_pps_ = node->create_subscription( 23 | get_prefix() + "/pps", 1, 24 | std::bind(&PulDir::puldir_set_callback_, this, std::placeholders::_1)); 25 | } 26 | 27 | void PulDir::puldir_set(int dir, uint16_t pps) { 28 | if (last_dir_ != dir && dir_channel_ != -1) { 29 | if (last_pps_ != 0) { 30 | // set velocity to 0 before switching the direction 31 | write(0); 32 | last_pps_ = 0; 33 | } 34 | 35 | microcontroller_->write(ADDR_GPIO_MIN + dir_channel_, dir); 36 | last_dir_ = dir; 37 | } 38 | 39 | if (last_pps_ != pps) { 40 | write(pps); 41 | last_pps_ = pps; 42 | } 43 | } 44 | 45 | void PulDir::puldir_set_callback_(const std_msgs::msg::Int32::SharedPtr value) { 46 | int dir = value->data >= 0 ? PULDIR_CW : PULDIR_CCW; 47 | uint32_t pps = ::abs(value->data); 48 | puldir_set(dir, pps); 49 | } 50 | 51 | // PulDir does not use register read ops 52 | void PulDir::read_cb(uint16_t value) { (void)value; } 53 | // PulDir does not use stream ops 54 | void PulDir::stream_cb(const std::string &value) { (void)value; } 55 | 56 | } // namespace remote_microcontroller 57 | -------------------------------------------------------------------------------- /src/puldir_stepper_driver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-04-09 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/puldir_stepper_driver.hpp" 11 | 12 | namespace remote_microcontroller { 13 | 14 | PulDirStepperDriver::PulDirStepperDriver( 15 | rclcpp::Node *node, remote_microcontroller::Implementation *microcontroller, 16 | int pul_channel, int dir_channel, const std::string &prefix) 17 | : PulDir(node, microcontroller, pul_channel, dir_channel, prefix), 18 | remote_stepper_driver::Implementation(node) { 19 | node->declare_parameter("ppr", 0); 20 | node->get_parameter("ppr", param_ppr_); 21 | ppr_ = param_ppr_.as_int(); 22 | } 23 | 24 | void PulDirStepperDriver::velocity_set_real_(double velocity) { 25 | std::lock_guard guard(param_maxmin_lock_); 26 | 27 | int dir; 28 | if (velocity >= 0) { 29 | dir = PULDIR_CW; 30 | } else { 31 | dir = PULDIR_CCW; 32 | } 33 | 34 | uint32_t pps = ppr_ * velocity / M_PI; 35 | if (pps > 65535) { 36 | pps = 65535; 37 | } 38 | 39 | puldir_set(dir, pps); 40 | } 41 | 42 | rclcpp::FutureReturnCode PulDirStepperDriver::param_ppr_get_handler_( 43 | const std::shared_ptr 44 | request, 45 | std::shared_ptr 46 | response) { 47 | (void)request; 48 | response->ppr = ppr_; 49 | return rclcpp::FutureReturnCode::SUCCESS; 50 | } 51 | rclcpp::FutureReturnCode PulDirStepperDriver::param_ppr_set_handler_( 52 | const std::shared_ptr 53 | request, 54 | std::shared_ptr 55 | response) { 56 | (void)response; 57 | ppr_ = request->ppr; 58 | return rclcpp::FutureReturnCode::SUCCESS; 59 | } 60 | 61 | } // namespace remote_microcontroller 62 | -------------------------------------------------------------------------------- /src/pwm.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/pwm.hpp" 11 | 12 | namespace remote_microcontroller { 13 | 14 | PWM::PWM(rclcpp::Node *node, 15 | remote_microcontroller::Implementation *microcontroller, int index, 16 | const std::string &prefix) 17 | : Accessory(node, microcontroller, ((uint16_t)(ADDR_PWM_MIN + index)), 18 | prefix) { 19 | node->declare_parameter("pwm_min", 0); 20 | node->get_parameter("pwm_min", param_min_); 21 | node->declare_parameter("pwm_max", 255); 22 | node->get_parameter("pwm_max", param_max_); 23 | 24 | // TODO(clairbee): create a parameter event handler to track parameter changes 25 | pwm_min_ = param_min_.as_int(); 26 | pwm_max_ = param_max_.as_int(); 27 | 28 | // Publish the min/max values so that the consumers now the boundaries 29 | topic_pwm_min_ = node->create_publisher( 30 | get_prefix() + "/pwm_min", 1); 31 | topic_pwm_min_->publish(std_msgs::msg::UInt16().set__data(pwm_min_)); 32 | topic_pwm_max_ = node->create_publisher( 33 | get_prefix() + "/pwm_max", 1); 34 | topic_pwm_max_->publish(std_msgs::msg::UInt16().set__data(pwm_max_)); 35 | 36 | topic_pwm_ = node->create_subscription( 37 | get_prefix() + "/pwm", 1, 38 | std::bind(&PWM::pwm_set_callback_, this, std::placeholders::_1)); 39 | } 40 | 41 | void PWM::pwm_set(uint16_t value) { 42 | if (value > pwm_max_) value = pwm_max_; 43 | if (value < pwm_min_) value = pwm_min_; 44 | write(value); 45 | } 46 | 47 | void PWM::pwm_set_callback_(const std_msgs::msg::UInt16::SharedPtr value) { 48 | pwm_set(value->data); 49 | } 50 | 51 | // PWM does not use register read ops 52 | void PWM::read_cb(uint16_t value) { (void)value; } 53 | // PWM does not use stream ops 54 | void PWM::stream_cb(const std::string &value) { (void)value; } 55 | 56 | } // namespace remote_microcontroller 57 | -------------------------------------------------------------------------------- /src/pwm_actuator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/pwm_actuator.hpp" 11 | 12 | #include 13 | 14 | namespace remote_microcontroller { 15 | 16 | PWMActuator::PWMActuator( 17 | rclcpp::Node *node, remote_microcontroller::Implementation *microcontroller, 18 | int index, const std::string &prefix) 19 | : PWM(node, microcontroller, index, prefix), 20 | remote_actuator::Implementation(node, prefix) {} 21 | 22 | } // namespace remote_microcontroller 23 | -------------------------------------------------------------------------------- /src/pwm_actuator_position.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/pwm_actuator_position.hpp" 11 | 12 | namespace remote_microcontroller { 13 | 14 | PWMActuatorPosition::PWMActuatorPosition( 15 | rclcpp::Node *node, remote_microcontroller::Implementation *microcontroller, 16 | int index, const std::string &prefix) 17 | : PWMActuator(node, microcontroller, index, prefix) {} 18 | 19 | void PWMActuatorPosition::position_set_real_(double position) { 20 | std::lock_guard guard(param_maxmin_lock_); 21 | 22 | position -= position_min_; 23 | position /= position_max_ - position_min_; 24 | // position is now remaped from [min;max] to [0;1] 25 | 26 | pwm_set(pwm_min_ + position * (pwm_max_ - pwm_min_)); 27 | } 28 | 29 | } // namespace remote_microcontroller 30 | -------------------------------------------------------------------------------- /src/pwm_actuator_velocity.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/pwm_actuator_velocity.hpp" 11 | 12 | namespace remote_microcontroller { 13 | 14 | PWMActuatorVelocity::PWMActuatorVelocity( 15 | rclcpp::Node *node, remote_microcontroller::Implementation *microcontroller, 16 | int index, const std::string &prefix) 17 | : PWMActuator(node, microcontroller, index, prefix) {} 18 | 19 | void PWMActuatorVelocity::velocity_set_real_(double velocity) { 20 | std::lock_guard guard(param_maxmin_lock_); 21 | 22 | velocity -= velocity_min_; 23 | velocity /= velocity_max_ - velocity_min_; 24 | // velocity is now remaped from [min;max] to [0;1] 25 | 26 | pwm_set(pwm_min_ + velocity * (pwm_max_ - pwm_min_)); 27 | } 28 | 29 | } // namespace remote_microcontroller 30 | -------------------------------------------------------------------------------- /src/uart.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVMP, 2023 3 | * 4 | * Author: Roman Kuzmenko 5 | * Created: 2023-03-27 6 | * 7 | * Licensed under Apache License, Version 2.0. 8 | */ 9 | 10 | #include "remote_microcontroller/uart.hpp" 11 | 12 | namespace remote_microcontroller { 13 | 14 | UART::UART(rclcpp::Node *node, 15 | remote_microcontroller::Implementation *microcontroller, int index, 16 | const std::string &prefix) 17 | : Accessory(node, microcontroller, (uint16_t)(ADDR_UART_MIN + index), 18 | prefix), 19 | remote_serial::Implementation(node, prefix), 20 | input_cb_{nullptr}, 21 | input_cb_user_data_{nullptr} { 22 | node->declare_parameter("serial_baud_rate", 115200); 23 | node->get_parameter("serial_baud_rate", baud_rate); 24 | } 25 | 26 | void UART::init() { 27 | uint16_t value = 0; 28 | switch (baud_rate.as_int()) { 29 | case 300: 30 | value = 1; 31 | break; 32 | case 600: 33 | value = 2; 34 | break; 35 | case 1200: 36 | value = 3; 37 | break; 38 | case 2400: 39 | value = 4; 40 | break; 41 | case 4800: 42 | value = 5; 43 | break; 44 | case 9600: 45 | value = 6; 46 | break; 47 | case 14400: 48 | value = 7; 49 | break; 50 | case 19200: 51 | value = 8; 52 | break; 53 | case 28800: 54 | value = 9; 55 | break; 56 | case 31250: 57 | value = 10; 58 | break; 59 | case 28400: 60 | value = 11; 61 | break; 62 | case 57600: 63 | value = 12; 64 | break; 65 | case 76800: 66 | value = 13; 67 | break; 68 | case 115200: 69 | value = 14; 70 | break; 71 | } 72 | 73 | write(value, true); 74 | 75 | initialized_ = true; 76 | 77 | init_serial_(); 78 | } 79 | 80 | void UART::read_cb(uint16_t value) { 81 | (void)value; 82 | // UART does not use register read/write ops 83 | } 84 | 85 | void UART::stream_cb(const std::string &msg) { 86 | std::lock_guard guard(input_cb_mutex_); 87 | if (input_cb_) { 88 | input_cb_(msg, input_cb_user_data_); 89 | } 90 | 91 | auto message = std_msgs::msg::UInt8MultiArray(); 92 | message.data = std::vector(msg.begin(), msg.end()); 93 | inspect_input->publish(message); 94 | } 95 | 96 | void UART::output(const std::string &msg) { 97 | stream(msg); 98 | 99 | auto message = std_msgs::msg::UInt8MultiArray(); 100 | message.data = std::vector(msg.begin(), msg.end()); 101 | inspect_output->publish(message); 102 | } 103 | 104 | void UART::register_input_cb(void (*input_cb)(const std::string &msg, 105 | void *user_data), 106 | void *user_data) { 107 | std::lock_guard guard(input_cb_mutex_); 108 | input_cb_ = input_cb; 109 | input_cb_user_data_ = user_data; 110 | } 111 | 112 | void UART::inject_input(const std::string &msg) { stream_cb(msg); } 113 | 114 | } // namespace remote_microcontroller 115 | -------------------------------------------------------------------------------- /srv/Reset.srv: -------------------------------------------------------------------------------- 1 | bool hard 2 | bool reflash 3 | --- 4 | 5 | bool success -------------------------------------------------------------------------------- /test/test1.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import serial 4 | 5 | if __name__ == '__main__': 6 | ser = serial.Serial('/dev/ttyACM0', 9600, timeout=1) 7 | ser.reset_input_buffer() 8 | while True: 9 | if ser.in_waiting > 0: 10 | line = ser.readline().decode('utf-8').rstrip() 11 | print(line) --------------------------------------------------------------------------------