From 117a262b01a3ce09c5adee1bf3d9d73b428b2562 Mon Sep 17 00:00:00 2001 From: Phoebe Bridgeback Date: Wed, 19 Aug 2026 10:55:04 -0500 Subject: [PATCH 1/9] Update ros2_robotiq_gripper to run on hardware with jazzy --- src/ros2_robotiq_gripper | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros2_robotiq_gripper b/src/ros2_robotiq_gripper index dc7c884..8f2eb05 160000 --- a/src/ros2_robotiq_gripper +++ b/src/ros2_robotiq_gripper @@ -1 +1 @@ -Subproject commit dc7c8848c235ea1250061e366969e12433a33e9a +Subproject commit 8f2eb05c1172fb040c96dd11db06dd99214d8675 From 1e794ad40e6c1419d9b5a6b928505b642f56e109 Mon Sep 17 00:00:00 2001 From: Mark Paterson Date: Wed, 26 Aug 2026 12:07:20 -0500 Subject: [PATCH 2/9] Fold in gripper fix from humble --- src/ros2_robotiq_gripper | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros2_robotiq_gripper b/src/ros2_robotiq_gripper index 8f2eb05..fb2d353 160000 --- a/src/ros2_robotiq_gripper +++ b/src/ros2_robotiq_gripper @@ -1 +1 @@ -Subproject commit 8f2eb05c1172fb040c96dd11db06dd99214d8675 +Subproject commit fb2d3536456938db2d22631d20eeb17e5b20e9d8 From c5d5c1ed7adaa5e2689b2189cda5fdd426a66d94 Mon Sep 17 00:00:00 2001 From: Mark Paterson Date: Wed, 26 Aug 2026 12:07:50 -0500 Subject: [PATCH 3/9] Working transport mode in jazzy --- src/phoebe_bridgeback | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/phoebe_bridgeback b/src/phoebe_bridgeback index 5f10254..eae332d 160000 --- a/src/phoebe_bridgeback +++ b/src/phoebe_bridgeback @@ -1 +1 @@ -Subproject commit 5f1025420d3200606c14399a74fb71d13412b66f +Subproject commit eae332d846a00da343b2297d9ea8b7260619fc89 From ef0c2e4273f01375382637cba521be5d9302d39a Mon Sep 17 00:00:00 2001 From: Phoebe Bridgeback Date: Mon, 31 Aug 2026 11:32:48 -0500 Subject: [PATCH 4/9] Update to current phoebe_bridgeback --- src/phoebe_bridgeback | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/phoebe_bridgeback b/src/phoebe_bridgeback index eae332d..eb7950d 160000 --- a/src/phoebe_bridgeback +++ b/src/phoebe_bridgeback @@ -1 +1 @@ -Subproject commit eae332d846a00da343b2297d9ea8b7260619fc89 +Subproject commit eb7950de0332adf1aa484d0b548a5f5e013863ab From b0e600bb3005b6875e029372946ab23617ad9ade Mon Sep 17 00:00:00 2001 From: Mark Paterson Date: Mon, 31 Aug 2026 16:55:42 -0500 Subject: [PATCH 5/9] Add temporary modified puma driver to avoid max wheel speed on controller disable Ros2control current sends NaNs to the motor if there is no controller. This causes the wheels to spin at max speed. We have submitted a ros2control MR for this https://github.com/ros-controls/ros2_controllers/pull/2578 but in the meantime, intercept the NaNs and change them to zeroes. --- src/puma_motor_driver/CHANGELOG.rst | 245 ++++ src/puma_motor_driver/CMakeLists.txt | 55 + src/puma_motor_driver/LICENSE | 29 + .../README.clearpath_robot.md | 107 ++ src/puma_motor_driver/README.md | 11 + .../include/puma_motor_driver/can_proto.hpp | 399 +++++++ .../include/puma_motor_driver/driver.hpp | 539 +++++++++ .../puma_motor_driver/multi_puma_node.hpp | 202 ++++ src/puma_motor_driver/package.xml | 26 + src/puma_motor_driver/src/driver.cpp | 1057 +++++++++++++++++ src/puma_motor_driver/src/multi_puma_node.cpp | 368 ++++++ 11 files changed, 3038 insertions(+) create mode 100644 src/puma_motor_driver/CHANGELOG.rst create mode 100644 src/puma_motor_driver/CMakeLists.txt create mode 100644 src/puma_motor_driver/LICENSE create mode 100644 src/puma_motor_driver/README.clearpath_robot.md create mode 100644 src/puma_motor_driver/README.md create mode 100644 src/puma_motor_driver/include/puma_motor_driver/can_proto.hpp create mode 100644 src/puma_motor_driver/include/puma_motor_driver/driver.hpp create mode 100644 src/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp create mode 100644 src/puma_motor_driver/package.xml create mode 100644 src/puma_motor_driver/src/driver.cpp create mode 100644 src/puma_motor_driver/src/multi_puma_node.cpp diff --git a/src/puma_motor_driver/CHANGELOG.rst b/src/puma_motor_driver/CHANGELOG.rst new file mode 100644 index 0000000..f092871 --- /dev/null +++ b/src/puma_motor_driver/CHANGELOG.rst @@ -0,0 +1,245 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package puma_motor_driver +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.0 (2025-01-15) +------------------ + +2.9.6 (2026-08-13) +------------------ + +2.9.5 (2026-08-07) +------------------ + +2.9.4 (2026-07-17) +------------------ + +2.9.3 (2026-05-21) +------------------ + +2.9.2 (2026-04-14) +------------------ + +2.9.1 (2026-02-11) +------------------ + +2.9.0 (2026-02-11) +------------------ +* Fix: CMake Verision and CI (`#299 `_) + * Updated cmake version to 3.20 + * Switch action to run on container + * Update repos update action version + * Increase test timeout +* Contributors: luis-camero + +2.8.6 (2026-01-20) +------------------ + +2.8.5 (2026-01-19) +------------------ +* CORE-36182: [puma_motor_driver] Port to can_hardware socketcan driver (`#286 `_) + * CORE-36182: [puma_motor_driver] Port to can_hardware socketcan driver + * misc clean up +* Contributors: Natesh Narain + +2.8.4 (2025-11-13) +------------------ + +2.8.3 (2025-11-10) +------------------ + +2.8.2 (2025-10-28) +------------------ + +2.8.1 (2025-10-23) +------------------ + +2.8.0 (2025-10-23) +------------------ +* Add additional Puma diagnostics (`#270 `_) +* Contributors: Chris Iverach-Brereton + +2.7.2 (2025-09-18) +------------------ + +2.7.1 (2025-09-16) +------------------ + +2.7.0 (2025-08-25) +------------------ + +2.6.3 (2025-08-18) +------------------ + +2.6.2 (2025-07-15) +------------------ + +2.6.1 (2025-07-04) +------------------ + +2.6.0 (2025-07-04) +------------------ + +2.5.1 (2025-06-17) +------------------ + +2.5.0 (2025-05-29) +------------------ + +2.4.1 (2025-05-20) +------------------ + +2.4.0 (2025-04-30) +------------------ + +2.3.3 (2025-04-17) +------------------ + +2.3.2 (2025-04-16) +------------------ +* Fixed depend version. +* Contributors: Tony Baltovski + +2.3.1 (2025-04-14) +------------------ + +2.3.0 (2025-04-11) +------------------ +* Reduced feedback frequency to 20hz (`#195 `_) +* Jazzy dingo and ridgeback fixes (`#169 `_) +* Contributors: Roni Kreinin + +2.2.4 (2025-04-07) +------------------ + +2.2.3 (2025-03-20) +------------------ + +2.2.2 (2025-03-17) +------------------ + +2.2.1 (2025-03-12) +------------------ + +2.2.0 (2025-03-11) +------------------ +* [clearpath_motor_drivers/puma_motor_driver] Linting fix. +* [clearpath_motor_drivers/puma_motor_driver] Fixed copy size. +* Contributors: Tony Baltovski + +2.1.2 (2025-02-28) +------------------ + +2.1.1 (2025-02-06) +------------------ + +2.1.0 (2025-01-31) +------------------ + +2.0.4 (2025-01-22) +------------------ + +2.0.3 (2025-01-17) +------------------ + +2.0.2 (2025-01-17) +------------------ + +2.0.1 (2025-01-17) +------------------ + +2.0.0 (2025-01-17) +------------------ +* 1.1.0 +* Changes. +* Add clearpath_motor_drivers, puma_motor_driver +* Contributors: Luis Camero, Tony Baltovski + +1.0.1 (2024-11-28) +------------------ + +1.0.0 (2024-11-26) +------------------ +* Added minimum version. +* Add clearpath_motor_drivers, puma_motor_driver +* Contributors: Luis Camero, Tony Baltovski + +* Added minimum version. +* Merge pull request `#103 `_ from clearpathrobotics/humble-1.0RC + Humble 1.0 RC +* Merge pull request `#104 `_ from clearpathrobotics/feature/reorganization + Package Re-organization +* Add clearpath_motor_drivers, puma_motor_driver +* Contributors: Luis Camero, Tony Baltovski, luis-camero + +0.3.2 (2024-10-04) +------------------ + +0.2.15 (2024-08-12) +------------------- + +0.2.14 (2024-08-08) +------------------- + +0.2.13 (2024-07-30) +------------------- + +0.2.12 (2024-07-22) +------------------- + +0.2.11 (2024-05-28) +------------------- + +0.2.10 (2024-05-16 17:09) +------------------------- + +0.2.9 (2024-05-16 12:19) +------------------------ + +0.2.8 (2024-05-14) +------------------ + +0.2.7 (2024-04-10) +------------------ + +0.2.6 (2024-04-08) +------------------ + +0.2.5 (2024-03-07) +------------------ + +0.2.4 (2024-01-19) +------------------ + +0.2.3 (2024-01-18) +------------------ + +0.2.2 (2024-01-10) +------------------ + +0.2.1 (2023-12-18) +------------------ + +0.2.0 (2023-12-13) +------------------ + +0.1.3 (2023-10-04) +------------------ + +0.1.2 (2023-09-27) +------------------ + +0.1.1 (2023-09-11) +------------------ + +0.1.0 (2023-08-31) +------------------ + +0.0.3 (2023-08-15) +------------------ + +0.0.2 (2023-07-25) +------------------ + +0.0.1 (2023-07-20) +------------------ diff --git a/src/puma_motor_driver/CMakeLists.txt b/src/puma_motor_driver/CMakeLists.txt new file mode 100644 index 0000000..d987fff --- /dev/null +++ b/src/puma_motor_driver/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.20) +project(puma_motor_driver) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(can_hardware REQUIRED) +find_package(clearpath_motor_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + + +set(DEPENDENCIES + ament_cmake + can_hardware + clearpath_motor_msgs + diagnostic_updater + rclcpp + std_msgs + sensor_msgs +) + +add_executable(multi_puma_node + src/multi_puma_node.cpp + src/driver.cpp +) +target_include_directories(multi_puma_node PUBLIC + $ + $) +target_compile_features(multi_puma_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +ament_target_dependencies(multi_puma_node ${DEPENDENCIES}) +target_link_libraries(multi_puma_node) + +install(TARGETS multi_puma_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/puma_motor_driver/LICENSE b/src/puma_motor_driver/LICENSE new file mode 100644 index 0000000..e9b530e --- /dev/null +++ b/src/puma_motor_driver/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2023, clearpathrobotics + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + diff --git a/src/puma_motor_driver/README.clearpath_robot.md b/src/puma_motor_driver/README.clearpath_robot.md new file mode 100644 index 0000000..d5c3958 --- /dev/null +++ b/src/puma_motor_driver/README.clearpath_robot.md @@ -0,0 +1,107 @@ +# Contributing to clearpath_robot + +Thanks for your interest in improving `clearpath_robot`! This is the **on-robot** half of the +stack: it runs on the physical platform's onboard computer, talks to the motor controllers and +sensors, and brings the robot up as a set of `systemd` services. Because it drives real hardware, +please read the notes below before opening a pull request. + +## Getting started + +1. Fork the repository and clone your fork. +2. Create a feature branch off `jazzy`: + + ```bash + git checkout -b my-feature jazzy + ``` + +3. Build the workspace and source it: + + ```bash + rosdep install --from-paths src --ignore-src -r -y + colcon build --symlink-install + source install/setup.bash + ``` + +4. Install the pre-commit hooks (one-time setup): + + ```bash + pip install pre-commit + pre-commit install + ``` + +## Linting + +This repository uses [pre-commit](https://pre-commit.com/) to run linting and formatting checks +(trailing whitespace, end-of-file, YAML/JSON checks, `markdownlint`, `flake8`, and `cspell` +spell-checking) before each commit. Run them against the whole tree before pushing: + +```bash +pre-commit run --all-files +``` + +## Where things live + +See the [Packages section of the README](README.md#packages) for the full map: + +- [`clearpath_robot`](clearpath_robot) — bringup scripts and the `systemd` services that start the + robot on boot. +- [`clearpath_generator_robot`](clearpath_generator_robot) — generates the robot-side launch and + parameter files (builds on `clearpath_generator_common`). +- [`clearpath_hardware_interfaces`](clearpath_hardware_interfaces) and + [`clearpath_motor_drivers`](clearpath_motor_drivers) — hardware and motor controller drivers. +- [`clearpath_sensors`](clearpath_sensors) — default sensor launch/param files. +- [`clearpath_tests`](clearpath_tests) — on-robot hardware/functional test suite. + +## Testing your changes + +Most of this repository interacts with real hardware and the `systemd` bringup, so validate changes +on a robot where possible: + +```bash +sudo systemctl restart clearpath-robot # regenerate + restart everything +sudo systemctl status clearpath-robot # overall bringup status +journalctl -u clearpath-platform -f # follow a single service's logs +ros2 run clearpath_robot generate # regenerate launch/params from robot.yaml +``` + +The [`clearpath_tests`](clearpath_tests) package provides an on-robot functional test suite — see +its [README](clearpath_tests/README.md). Build and run any package unit tests through `colcon`: + +```bash +colcon test --packages-select +colcon test-result --verbose +``` + +Be careful with changes that move the platform — ensure the area is clear and an e-stop is within +reach. + +## Generator tests + +Changes to the generators in this repository (`clearpath_generator_robot`) may affect the generated +output for launch and parameter files. The +[clearpath_generator_tests](https://github.com/clearpathrobotics/clearpath_generator_tests) +repository versions the expected output and validates it through CI. + +Before merging, ensure a corresponding branch with the **same name** exists in +`clearpath_generator_tests` with regenerated samples. See the +[Development Workflow](https://github.com/clearpathrobotics/clearpath_generator_tests#development-workflow) +section of that repository for the full process. + +## Submitting a pull request + +1. Make sure the workspace builds and any tests pass. +2. Push your branch and open a pull request against `jazzy`. +3. Write a clear description of what the change does and the platform/hardware you tested against. +4. If your change touches generator output, note the matching `clearpath_generator_tests` branch. + +## Reporting issues + +Please open issues on the +[GitHub issue tracker](https://github.com/clearpathrobotics/clearpath_robot/issues) and fill out +the bug report template, which walks you through the details we need to reproduce the problem. + +## License + +By contributing, you agree that your contributions will be licensed under the +[BSD-3-Clause license](LICENSE) that covers this project. + diff --git a/src/puma_motor_driver/README.md b/src/puma_motor_driver/README.md new file mode 100644 index 0000000..bc8b898 --- /dev/null +++ b/src/puma_motor_driver/README.md @@ -0,0 +1,11 @@ +This package is excerpted from https://github.com/clearpathrobotics/clearpath_robot/tree/jazzy +to address one temporary specific issue as follows. + +When the platform velocity controller is deactivated, ros2 control sends NaNs to the +driver. The Puma driver forwards these to the motor controller, which spins the wheels at +maximum velocity. We have made a ros2control MR to send zeroes instead +(https://github.com/ros-controls/ros2_controllers/pull/2578). +In the meantime, we intercept the NaNs in the driver and subsitute zeroes. + +The original README.md from the Clearpath parent project is included here as +README.clearpath_robot.md along with the original LICENSE. diff --git a/src/puma_motor_driver/include/puma_motor_driver/can_proto.hpp b/src/puma_motor_driver/include/puma_motor_driver/can_proto.hpp new file mode 100644 index 0000000..9a14fe2 --- /dev/null +++ b/src/puma_motor_driver/include/puma_motor_driver/can_proto.hpp @@ -0,0 +1,399 @@ +//***************************************************************************** +// +// can_proto.h - Definitions for the CAN protocol used to communicate with the +// BDC motor controller. +// +// Copyright (c) 2008-2013 Texas Instruments Incorporated. All rights reserved. +// Software License Agreement +// +// Texas Instruments (TI) is supplying this software for use solely and +// exclusively on TI's microcontroller products. The software is owned by +// TI and/or its suppliers, and is protected under applicable copyright +// laws. You may not combine this software with "viral" open-source +// software in order to form a larger program. +// +// THIS SOFTWARE IS PROVIDED "AS IS" AND WITH ALL FAULTS. +// NO WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT +// NOT LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. TI SHALL NOT, UNDER ANY +// CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR CONSEQUENTIAL +// DAMAGES, FOR ANY REASON WHATSOEVER. +// +// This is part of revision 10636 of the RDK-BDC24 Firmware Package. +// +//***************************************************************************** + +#ifndef __CAN_PROTO_H__ +#define __CAN_PROTO_H__ + +//***************************************************************************** +// +// The masks of the fields that are used in the message identifier. +// +//***************************************************************************** +#define CAN_MSGID_FULL_M 0x1fffffff +#define CAN_MSGID_DEVNO_M 0x0000003f +#define CAN_MSGID_API_M 0x0000ffc0 +#define CAN_MSGID_MFR_M 0x00ff0000 +#define CAN_MSGID_DTYPE_M 0x1f000000 +#define CAN_MSGID_DEVNO_S 0 +#define CAN_MSGID_API_S 6 +#define CAN_MSGID_MFR_S 16 +#define CAN_MSGID_DTYPE_S 24 + +//***************************************************************************** +// +// The Reserved device number values in the Message Id. +// +//***************************************************************************** +#define CAN_MSGID_DEVNO_BCAST 0x00000000 + +//***************************************************************************** +// +// The Reserved system control API numbers in the Message Id. +// +//***************************************************************************** +#define CAN_MSGID_API_SYSHALT 0x00000000 +#define CAN_MSGID_API_SYSRST 0x00000040 +#define CAN_MSGID_API_DEVASSIGN 0x00000080 +#define CAN_MSGID_API_DEVQUERY 0x000000c0 +#define CAN_MSGID_API_HEARTBEAT 0x00000140 +#define CAN_MSGID_API_SYNC 0x00000180 +#define CAN_MSGID_API_UPDATE 0x000001c0 +#define CAN_MSGID_API_FIRMVER 0x00000200 +#define CAN_MSGID_API_ENUMERATE 0x00000240 +#define CAN_MSGID_API_SYSRESUME 0x00000280 + +//***************************************************************************** +// +// The 32 bit values associated with the CAN_MSGID_API_STATUS request. +// +//***************************************************************************** +#define CAN_STATUS_CODE_M 0x0000ffff +#define CAN_STATUS_MFG_M 0x00ff0000 +#define CAN_STATUS_DTYPE_M 0x1f000000 +#define CAN_STATUS_CODE_S 0 +#define CAN_STATUS_MFG_S 16 +#define CAN_STATUS_DTYPE_S 24 + +//***************************************************************************** +// +// The Reserved manufacturer identifiers in the Message Id. +// +//***************************************************************************** +#define CAN_MSGID_MFR_NI 0x00010000 +#define CAN_MSGID_MFR_LM 0x00020000 +#define CAN_MSGID_MFR_DEKA 0x00030000 + +//***************************************************************************** +// +// The Reserved device type identifiers in the Message Id. +// +//***************************************************************************** +#define CAN_MSGID_DTYPE_BCAST 0x00000000 +#define CAN_MSGID_DTYPE_ROBOT 0x01000000 +#define CAN_MSGID_DTYPE_MOTOR 0x02000000 +#define CAN_MSGID_DTYPE_RELAY 0x03000000 +#define CAN_MSGID_DTYPE_GYRO 0x04000000 +#define CAN_MSGID_DTYPE_ACCEL 0x05000000 +#define CAN_MSGID_DTYPE_USONIC 0x06000000 +#define CAN_MSGID_DTYPE_GEART 0x07000000 +#define CAN_MSGID_DTYPE_UPDATE 0x1f000000 + +//***************************************************************************** +// +// LM Motor Control API Classes API Class and ID masks. +// +//***************************************************************************** +#define CAN_MSGID_API_CLASS_M 0x0000fc00 +#define CAN_MSGID_API_ID_M 0x000003c0 + +//***************************************************************************** +// +// LM Motor Control API Classes in the Message Id for non-broadcast. +// These are the upper 6 bits of the API field, the lower 4 bits determine +// the APIId. +// +//***************************************************************************** +#define CAN_API_MC_VOLTAGE 0x00000000 +#define CAN_API_MC_SPD 0x00000400 +#define CAN_API_MC_VCOMP 0x00000800 +#define CAN_API_MC_POS 0x00000c00 +#define CAN_API_MC_ICTRL 0x00001000 +#define CAN_API_MC_STATUS 0x00001400 +#define CAN_API_MC_PSTAT 0x00001800 +#define CAN_API_MC_CFG 0x00001c00 +#define CAN_API_MC_ACK 0x00002000 + +//***************************************************************************** +// +// The Stellaris Motor Class Control Voltage API definitions. +// +//***************************************************************************** +#define LM_API_VOLT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VOLTAGE) +#define LM_API_VOLT_EN (LM_API_VOLT | (0 << CAN_MSGID_API_S)) +#define LM_API_VOLT_DIS (LM_API_VOLT | (1 << CAN_MSGID_API_S)) +#define LM_API_VOLT_SET (LM_API_VOLT | (2 << CAN_MSGID_API_S)) +#define LM_API_VOLT_SET_RAMP (LM_API_VOLT | (3 << CAN_MSGID_API_S)) +#define LM_API_VOLT_SET_NO_ACK (LM_API_VOLT | (8 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The Stellaris Motor Class Control API definitions for LM_API_VOLT_SET_RAMP. +// +//***************************************************************************** +#define LM_API_VOLT_RAMP_DIS 0 + +//***************************************************************************** +// +// The Stellaris Motor Class Control API definitions for CAN_MSGID_API_SYNC. +// +//***************************************************************************** +#define LM_API_SYNC_PEND_NOW 0 + +//***************************************************************************** +// +// The Stellaris Motor Class Speed Control API definitions. +// +//***************************************************************************** +#define LM_API_SPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_SPD) +#define LM_API_SPD_EN (LM_API_SPD | (0 << CAN_MSGID_API_S)) +#define LM_API_SPD_DIS (LM_API_SPD | (1 << CAN_MSGID_API_S)) +#define LM_API_SPD_SET (LM_API_SPD | (2 << CAN_MSGID_API_S)) +#define LM_API_SPD_PC (LM_API_SPD | (3 << CAN_MSGID_API_S)) +#define LM_API_SPD_IC (LM_API_SPD | (4 << CAN_MSGID_API_S)) +#define LM_API_SPD_DC (LM_API_SPD | (5 << CAN_MSGID_API_S)) +#define LM_API_SPD_REF (LM_API_SPD | (6 << CAN_MSGID_API_S)) +#define LM_API_SPD_SET_NO_ACK (LM_API_SPD | (11 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The Stellaris Motor Control Voltage Compensation Control API definitions. +// +//***************************************************************************** +#define LM_API_VCOMP (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VCOMP) +#define LM_API_VCOMP_EN (LM_API_VCOMP | (0 << CAN_MSGID_API_S)) +#define LM_API_VCOMP_DIS (LM_API_VCOMP | (1 << CAN_MSGID_API_S)) +#define LM_API_VCOMP_SET (LM_API_VCOMP | (2 << CAN_MSGID_API_S)) +#define LM_API_VCOMP_IN_RAMP (LM_API_VCOMP | (3 << CAN_MSGID_API_S)) +#define LM_API_VCOMP_COMP_RAMP (LM_API_VCOMP | (4 << CAN_MSGID_API_S)) +#define LM_API_VCOMP_SET_NO_ACK (LM_API_VCOMP | (9 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The Stellaris Motor Class Position Control API definitions. +// +//***************************************************************************** +#define LM_API_POS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_POS) +#define LM_API_POS_EN (LM_API_POS | (0 << CAN_MSGID_API_S)) +#define LM_API_POS_DIS (LM_API_POS | (1 << CAN_MSGID_API_S)) +#define LM_API_POS_SET (LM_API_POS | (2 << CAN_MSGID_API_S)) +#define LM_API_POS_PC (LM_API_POS | (3 << CAN_MSGID_API_S)) +#define LM_API_POS_IC (LM_API_POS | (4 << CAN_MSGID_API_S)) +#define LM_API_POS_DC (LM_API_POS | (5 << CAN_MSGID_API_S)) +#define LM_API_POS_REF (LM_API_POS | (6 << CAN_MSGID_API_S)) +#define LM_API_POS_SET_NO_ACK (LM_API_POS | (11 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The Stellaris Motor Class Current Control API definitions. +// +//***************************************************************************** +#define LM_API_ICTRL (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ICTRL) +#define LM_API_ICTRL_EN (LM_API_ICTRL | (0 << CAN_MSGID_API_S)) +#define LM_API_ICTRL_DIS (LM_API_ICTRL | (1 << CAN_MSGID_API_S)) +#define LM_API_ICTRL_SET (LM_API_ICTRL | (2 << CAN_MSGID_API_S)) +#define LM_API_ICTRL_PC (LM_API_ICTRL | (3 << CAN_MSGID_API_S)) +#define LM_API_ICTRL_IC (LM_API_ICTRL | (4 << CAN_MSGID_API_S)) +#define LM_API_ICTRL_DC (LM_API_ICTRL | (5 << CAN_MSGID_API_S)) +#define LM_API_ICTRL_SET_NO_ACK (LM_API_ICTRL | (10 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The Stellaris Motor Class Firmware Update API definitions. +// +//***************************************************************************** +#define LM_API_UPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_UPDATE) +#define LM_API_UPD_PING (LM_API_UPD | (0 << CAN_MSGID_API_S)) +#define LM_API_UPD_DOWNLOAD (LM_API_UPD | (1 << CAN_MSGID_API_S)) +#define LM_API_UPD_SEND_DATA (LM_API_UPD | (2 << CAN_MSGID_API_S)) +#define LM_API_UPD_RESET (LM_API_UPD | (3 << CAN_MSGID_API_S)) +#define LM_API_UPD_ACK (LM_API_UPD | (4 << CAN_MSGID_API_S)) +#define LM_API_HWVER (LM_API_UPD | (5 << CAN_MSGID_API_S)) +#define LM_API_UPD_REQUEST (LM_API_UPD | (6 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The Stellaris Motor Class Status API definitions. +// +//***************************************************************************** +#define LM_API_STATUS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_STATUS) +#define LM_API_STATUS_VOLTOUT (LM_API_STATUS | (0 << CAN_MSGID_API_S)) +#define LM_API_STATUS_VOLTBUS (LM_API_STATUS | (1 << CAN_MSGID_API_S)) +#define LM_API_STATUS_CURRENT (LM_API_STATUS | (2 << CAN_MSGID_API_S)) +#define LM_API_STATUS_TEMP (LM_API_STATUS | (3 << CAN_MSGID_API_S)) +#define LM_API_STATUS_POS (LM_API_STATUS | (4 << CAN_MSGID_API_S)) +#define LM_API_STATUS_SPD (LM_API_STATUS | (5 << CAN_MSGID_API_S)) +#define LM_API_STATUS_LIMIT (LM_API_STATUS | (6 << CAN_MSGID_API_S)) +#define LM_API_STATUS_FAULT (LM_API_STATUS | (7 << CAN_MSGID_API_S)) +#define LM_API_STATUS_POWER (LM_API_STATUS | (8 << CAN_MSGID_API_S)) +#define LM_API_STATUS_CMODE (LM_API_STATUS | (9 << CAN_MSGID_API_S)) +#define LM_API_STATUS_VOUT (LM_API_STATUS | (10 << CAN_MSGID_API_S)) + +#define CPR_API_STATUS_ANALOG (LM_API_STATUS | (15 << CAN_MSGID_API_S)) +#define LM_API_STATUS_STKY_FLT (LM_API_STATUS | (11 << CAN_MSGID_API_S)) +#define LM_API_STATUS_FLT_COUNT (LM_API_STATUS | (12 << CAN_MSGID_API_S)) + + +//***************************************************************************** +// +// These definitions are used with the byte that is returned from +// the status request for LM_API_STATUS_LIMIT. +// +//***************************************************************************** +#define LM_STATUS_LIMIT_FWD 0x01 +#define LM_STATUS_LIMIT_REV 0x02 + +//***************************************************************************** +// +// LM Motor Control status codes returned due to the CAN_STATUS_CODE_M field. +// +//***************************************************************************** +#define LM_STATUS_FAULT_ILIMIT 0x01 +#define LM_STATUS_FAULT_TLIMIT 0x02 +#define LM_STATUS_FAULT_VLIMIT 0x04 + +//***************************************************************************** +// +// The Stellaris Motor Class Configuration API definitions. +// +//***************************************************************************** +#define LM_API_CFG (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_CFG) +#define LM_API_CFG_NUM_BRUSHES (LM_API_CFG | (0 << CAN_MSGID_API_S)) +#define LM_API_CFG_ENC_LINES (LM_API_CFG | (1 << CAN_MSGID_API_S)) +#define LM_API_CFG_POT_TURNS (LM_API_CFG | (2 << CAN_MSGID_API_S)) +#define LM_API_CFG_BRAKE_COAST (LM_API_CFG | (3 << CAN_MSGID_API_S)) +#define LM_API_CFG_LIMIT_MODE (LM_API_CFG | (4 << CAN_MSGID_API_S)) +#define LM_API_CFG_LIMIT_FWD (LM_API_CFG | (5 << CAN_MSGID_API_S)) +#define LM_API_CFG_LIMIT_REV (LM_API_CFG | (6 << CAN_MSGID_API_S)) +#define LM_API_CFG_MAX_VOUT (LM_API_CFG | (7 << CAN_MSGID_API_S)) +#define LM_API_CFG_FAULT_TIME (LM_API_CFG | (8 << CAN_MSGID_API_S)) + +#define CPR_API_CFG_SHUTDOWN_TEMP (LM_API_CFG | (11 << CAN_MSGID_API_S)) +#define CPR_API_CFG_MINIMUM_LEVEL (LM_API_CFG | (12 << CAN_MSGID_API_S)) +#define CPR_API_CFG_NOMINAL_LEVEL (LM_API_CFG | (13 << CAN_MSGID_API_S)) +#define CPR_API_CFG_SHUTOFF_LEVEL (LM_API_CFG | (14 << CAN_MSGID_API_S)) +#define CPR_API_CFG_SHUTOFF_TIME (LM_API_CFG | (15 << CAN_MSGID_API_S)) + + +//***************************************************************************** +// +// The Stellaris ACK API definition. +// +//***************************************************************************** +#define LM_API_ACK (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ACK) + +//***************************************************************************** +// +// The 8 bit values that can be returned by a call to LM_API_STATUS_HWVER. +// +//***************************************************************************** +#define LM_HWVER_UNKNOWN 0x00 +#define LM_HWVER_JAG_1_0 0x01 +#define LM_HWVER_JAG_2_0 0x02 + +//***************************************************************************** +// +// The 8 bit values that can be returned by a call to LM_API_STATUS_CMODE. +// +//***************************************************************************** +#define LM_STATUS_CMODE_VOLT 0x00 +#define LM_STATUS_CMODE_CURRENT 0x01 +#define LM_STATUS_CMODE_SPEED 0x02 +#define LM_STATUS_CMODE_POS 0x03 +#define LM_STATUS_CMODE_VCOMP 0x04 + +//***************************************************************************** +// +// The values that can specified as the position or speed reference. Not all +// values are valid for each reference; if an invalid reference is set, then +// none will be selected. +// +//***************************************************************************** +#define LM_REF_ENCODER 0x00 +#define LM_REF_POT 0x01 +#define LM_REF_INV_ENCODER 0x02 +#define LM_REF_QUAD_ENCODER 0x03 +#define LM_REF_NONE 0xff + +//***************************************************************************** +// +// The flags that are used to indicate the currently active fault sources. +// +//***************************************************************************** +#define LM_FAULT_CURRENT 0x01 +#define LM_FAULT_TEMP 0x02 +#define LM_FAULT_VBUS 0x04 +#define LM_FAULT_GATE_DRIVE 0x08 +#define LM_FAULT_COMM 0x10 + +//***************************************************************************** +// +// The Stellaris Motor Class Periodic Status API definitions. +// +//***************************************************************************** +#define LM_API_PSTAT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_PSTAT) +#define LM_API_PSTAT_PER_EN_S0 (LM_API_PSTAT | (0 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_PER_EN_S1 (LM_API_PSTAT | (1 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_PER_EN_S2 (LM_API_PSTAT | (2 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_PER_EN_S3 (LM_API_PSTAT | (3 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_CFG_S0 (LM_API_PSTAT | (4 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_CFG_S1 (LM_API_PSTAT | (5 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_CFG_S2 (LM_API_PSTAT | (6 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_CFG_S3 (LM_API_PSTAT | (7 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_DATA_S0 (LM_API_PSTAT | (8 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_DATA_S1 (LM_API_PSTAT | (9 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_DATA_S2 (LM_API_PSTAT | (10 << CAN_MSGID_API_S)) +#define LM_API_PSTAT_DATA_S3 (LM_API_PSTAT | (11 << CAN_MSGID_API_S)) + +//***************************************************************************** +// +// The values that can be used to configure the data the Periodic Status +// Message bytes. Bytes of a multi-byte data values are encoded as +// little-endian, therefore B0 is the least significant byte. +// +//***************************************************************************** +#define LM_PSTAT_END 0 +#define LM_PSTAT_VOLTOUT_B0 1 +#define LM_PSTAT_VOLTOUT_B1 2 +#define LM_PSTAT_VOLTBUS_B0 3 +#define LM_PSTAT_VOLTBUS_B1 4 +#define LM_PSTAT_CURRENT_B0 5 +#define LM_PSTAT_CURRENT_B1 6 +#define LM_PSTAT_TEMP_B0 7 +#define LM_PSTAT_TEMP_B1 8 +#define LM_PSTAT_POS_B0 9 +#define LM_PSTAT_POS_B1 10 +#define LM_PSTAT_POS_B2 11 +#define LM_PSTAT_POS_B3 12 +#define LM_PSTAT_SPD_B0 13 +#define LM_PSTAT_SPD_B1 14 +#define LM_PSTAT_SPD_B2 15 +#define LM_PSTAT_SPD_B3 16 +#define LM_PSTAT_LIMIT_NCLR 17 +#define LM_PSTAT_LIMIT_CLR 18 +#define LM_PSTAT_FAULT 19 +#define LM_PSTAT_STKY_FLT_NCLR 20 +#define LM_PSTAT_STKY_FLT_CLR 21 +#define LM_PSTAT_VOUT_B0 22 +#define LM_PSTAT_VOUT_B1 23 +#define LM_PSTAT_FLT_COUNT_CURRENT 24 +#define LM_PSTAT_FLT_COUNT_TEMP 25 +#define LM_PSTAT_FLT_COUNT_VOLTBUS 26 +#define LM_PSTAT_FLT_COUNT_GATE 27 +#define LM_PSTAT_FLT_COUNT_COMM 28 +#define LM_PSTAT_CANSTS 29 +#define LM_PSTAT_CANERR_B0 30 +#define LM_PSTAT_CANERR_B1 31 + +#endif // __CAN_PROTO_H__ diff --git a/src/puma_motor_driver/include/puma_motor_driver/driver.hpp b/src/puma_motor_driver/include/puma_motor_driver/driver.hpp new file mode 100644 index 0000000..23055fd --- /dev/null +++ b/src/puma_motor_driver/include/puma_motor_driver/driver.hpp @@ -0,0 +1,539 @@ +/** +Software License Agreement (BSD) + +\authors Mike Purvis +\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that +the following conditions are met: + * Redistributions of source code must retain the above copyright notice, this list of conditions and the + following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- +RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- +DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +#ifndef PUMA_MOTOR_DRIVER_DRIVER_H +#define PUMA_MOTOR_DRIVER_DRIVER_H + +#include +#include + +#include + +#include "can_hardware/common/types.hpp" +#include "can_hardware/drivers/socketcan_driver.hpp" + +#include "clearpath_motor_msgs/msg/puma_status.hpp" + +#include "diagnostic_updater/update_functions.hpp" + +#include "puma_motor_driver/can_proto.hpp" + +namespace puma_motor_driver +{ + +class Driver +{ +public: + Driver( + const std::shared_ptr interface, + std::shared_ptr nh, + const uint8_t & device_number, + const std::string & device_name); + + void processMessage(const can_hardware::Frame & frame); + + double radPerSecToRpm() const; + + /** + * Sends messages to the motor controller requesting all missing elements to + * populate the cache of status data. Returns true if any messages were sent, + * false if the cache is already complete. + */ + void requestStatusMessages(); + + /** + * Sends messages to the motor controller requesting all missing elements to + * populate the cache of feedback data. Returns true if any messages were sent, + * false if the cache is already complete. + */ + void requestFeedbackMessages(); + /** + * Sends a message to the motor controller requesting the instantaneous duty cycle to + * populate the cache of feedback data. + */ + void requestFeedbackDutyCycle(); + /** + * Sends a message to the motor controller requesting the instantaneous current consumption to + * populate the cache of feedback data. + */ + void requestFeedbackCurrent(); + /** + * Sends a message to the motor controller requesting the instantaneous angular distance to + * populate the cache of feedback data. + */ + void requestFeedbackPosition(); + /** + * Sends a message to the motor controller requesting the instantaneous angular speed to + * populate the cache of feedback data. + */ + void requestFeedbackSpeed(); + /** + * Sends a message to the motor controller requesting the state of the power flag. + */ + void requestFeedbackPowerState(); + /** + * Sends a message to the motor controller requesting the instantaneous set point of the + * current control mode to populate the cache of feedback data. + */ + void requestFeedbackSetpoint(); + /** + * Clear the received flags from the status cache, in preparation for the next + * request batch to go out. + */ + void clearMsgCache(); + /** + * Command the supplied value in open-loop voltage control. + * + * @param[in] cmd Value to command, ranging from -1.0 to 1.0, where zero is neutral. + */ + void commandDutyCycle(const float cmd); + /** + * Command the desired speed set-point in close-loop speed control. + * + * @param[in] cmd Value to command in rad/s. + */ + void commandSpeed(const double cmd); + // void currentSet(float cmd); + // void positionSet(float cmd); + // void neutralSet(); + + /** + * Set the encoders resolution in counts per rev. + * + * @param[in] encoder_cpr Value to set. + */ + void setEncoderCPR(const uint16_t encoder_cpr); + /** + * Set the gear ratio of the motors. + * + * @param[in] gear_ratio Value to set. + */ + void setGearRatio(const float gear_ratio); + /** + * Set the control mode of the motor drivers. + * + * @param[in] mode Value to set. + */ + void setMode(const uint8_t mode); + /** + * Set the control mode of the motor drivers + * with PID gains for close loop control. + * + * @param[in] mode Value to set. + * @param[in] p Value to set. + * @param[in] i Value to set. + * @param[in] d Value to set. + */ + void setMode(const uint8_t mode, const double p, const double i, const double d); + /** + * Set the control mode's PID gains for close loop control. + * + * @param[in] p Value to set. + * @param[in] i Value to set. + * @param[in] d Value to set. + */ + void setGains(const double p, const double i, const double d); + + /** + * Check fault response field was received. + * + * @return received flag + */ + bool receivedFault(); + /** + * Check power field was received. + * + * @return received flag + */ + bool receivedPower(); + /** + * Check mode field was received. + * + * @return received flag + */ + bool receivedMode(); + /** + * Check duty cycle field was received. + * + * @return received flag + */ + bool receivedDutyCycle(); + /** + * Check bus voltage field was received. + * + * @return received flag + */ + bool receivedBusVoltage(); + /** + * Check current field was received. + * + * @return received flag + */ + bool receivedCurrent(); + /** + * Check out voltage field was received. + * + * @return received flag + */ + bool receivedOutVoltage(); + /** + * Check teperature field was received. + * + * @return received flag + */ + bool receivedTemperature(); + /** + * Check analog input field was received. + * + * @return received flag + */ + bool receivedAnalogInput(); + /** + * Check position field was received. + * + * @return received flag + */ + bool receivedPosition(); + /** + * Check speed field was received. + * + * @return received flag + */ + bool receivedSpeed(); + /** + * Check setpoint field was received. + * + * @return received flag + */ + bool receivedSetpoint(); + /** + * Check the set-point response in voltage + * open-loop control was received. + * + * @return received flag + */ + bool receivedDutyCycleSetpoint(); + /** + * Check the set-point response in speed + * closed-loop control was received. + * + * @return received flag + */ + bool receivedSpeedSetpoint(); + /** + * Check the set-point response in currrent + * closed-loop control was received. + * + * @return received flag + */ + bool receivedCurrentSetpoint(); + /** + * Check the set-point response in position + * closed-loop control was received. + * + * @return received flag + */ + bool receivedPositionSetpoint(); + /** + * Process the last received fault response. + * + * @return state of fault status. + */ + uint8_t lastFault(); + /** + * Process the last received power response. + * + * @return state of power status. + */ + uint8_t lastPower(); + /** + * Process the last received mode response. + * + * @return current mode of motor driver. + */ + uint8_t lastMode(); + /** + * Process the last received duty cycle response. + * + * @return value of the instantaneous duty cycle. + */ + float lastDutyCycle(); + /** + * Process the last received bus voltage response. + * + * @return value of the instantaneous bus voltage. + */ + float lastBusVoltage(); + /** + * Process the last received current response. + * + * @return value of the instantaneous current. + */ + float lastCurrent(); + /** + * Process the last received out voltage response. + * + * @return value of the instantaneous out voltage. + */ + float lastOutVoltage(); + /** + * Process the last received temperature response. + * + * @return value of the instantaneous temperature. + */ + float lastTemperature(); + /** + * Process the last received analog_input response. + * + * @return value of the instantaneous analog_input. + */ + float lastAnalogInput(); + /** + * Process the last received travel response. + * + * @return value of the instantaneous angular position. + */ + double lastPosition(); + /** + * Process the last received speed response. + * + * @return value of the instantaneous angular speed. + */ + double lastSpeed(); + /** + * Process the last received set-point response + * for the current control mode. + * + * @return value of the set-point response. + */ + double lastSetpoint(); + + /** + * The requesting part of the state machine that sends a message to the + * motor controller requesting a parameter be set. + */ + void configureParams(); + /** + * The verifying part of the state machine that checks the response of + * the motor controller to ensure the value was set. + */ + void verifyParams(); + /** + * Gets if the driver has been configured. + * + * @return bool if driver is configured. + */ + bool isConfigured() const; + void resetConfiguration(); + /** + * Reset the configured flag to restart the verification process. + */ + void updateGains(); + /** + * Updates the PID gains. + */ + + /** + * Process the last received position encoder reference response + * + * @return value of the reference response. + */ + uint8_t posEncoderRef(); + /** + * Process the last received speed encoder reference response + * + * @return value of the reference response. + */ + uint8_t spdEncoderRef(); + /** + * Process the last received encoder counts response + * + * @return value of the encoder counts. + */ + uint16_t encoderCounts(); + + /** + * Process the last received P gain + * for the current control mode. + * + * @return value of the P gain response. + */ + double getP(); + /** + * Process the last received I gain + * for the current control mode. + * + * @return value of the I gain response. + */ + double getI(); + /** + * Process the last received D gain + * for the current control mode. + * + * @return value of the D gain response. + */ + double getD(); + /** + * Process the last received P gain + * for the current control mode. + * + * @return pointer to raw 4 bytes of the P gain response. + */ + uint8_t * getRawP(); + /** + * Process the last received I gain + * for the current control mode. + * + * @return pointer to raw 4 bytes of the I gain response. + */ + uint8_t * getRawI(); + /** + * Process the last received I gain + * for the current control mode. + * + * @return pointer to raw 4 bytes of the I gain response. + */ + uint8_t * getRawD(); + /** + * Process the last received set-point response + * in voltage open-loop control. + * + * @return value of the set-point response. + */ + float statusDutyCycleGet(); + /** + * Process the last received set-point response + * in speed closed-loop control. + * + * @return value of the set-point response. + */ + double statusSpeedGet(); + /** + * Process the last received set-point response + * in currrent closed-loop control. + * + * @return value of the set-point response. + */ + float statusCurrentGet(); + /** + * Process the last received set-point response + * in position closed-loop control. + * + * @return value of the set-point response. + */ + double statusPositionGet(); + + std::string deviceName() const {return device_name_;} + + uint8_t deviceNumber() const {return device_number_;} + + struct Field + { + static constexpr int FIELD_STRUCT_DATA_SIZE = 4; + uint8_t data[FIELD_STRUCT_DATA_SIZE]; + bool received; + + float interpretFixed8x8() + { + return *(reinterpret_cast(data)) / static_cast(1 << 8); + } + + double interpretFixed16x16() + { + return *(reinterpret_cast(data)) / static_cast(1 << 16); + } + }; + + // Diagnostics + void runFreqStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); + void driverUpdateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat, bool updating); + +private: + std::shared_ptr interface_; + std::shared_ptr nh_; + uint8_t device_number_; + std::string device_name_; + + bool configured_; + uint8_t state_; + double last_power_clear_ts_; + + uint8_t control_mode_; + double gain_p_; + double gain_i_; + double gain_d_; + uint16_t encoder_cpr_; + float gear_ratio_; + + /** + * Helpers to generate data for CAN messages. + */ + void sendId(const uint32_t id); + void sendUint8(const uint32_t id, const uint8_t value); + void sendUint16(const uint32_t id, const uint16_t value); + void sendFixed8x8(const uint32_t id, const float value); + void sendFixed16x16(const uint32_t id, const double value); + can_hardware::Frame getMsg(const uint32_t id); + uint32_t getApi(const can_hardware::Frame & msg); + uint32_t getDeviceNumber(const can_hardware::Frame & msg); + + /** + * Comparing the raw bytes of the 16x16 fixed-point numbers + * to avoid comparing the floating point values. + * + * @return boolean if received is equal to expected. + */ + bool verifyRaw16x16(const uint8_t * received, const double expected); + + /** + * Comparing the raw bytes of the 8x8 fixed-point numbers + * to avoid comparing the floating point values. + * + * @return boolean if received is equal to expected. + */ + bool verifyRaw8x8(const uint8_t * received, const float expected); + + Field voltage_fields_[4]; + Field spd_fields_[7]; + Field vcomp_fields_[5]; + Field pos_fields_[7]; + Field ictrl_fields_[6]; + Field status_fields_[15]; + Field cfg_fields_[15]; + + Field * voltageFieldForMessage(uint32_t api); + Field * spdFieldForMessage(uint32_t api); + Field * vcompFieldForMessage(uint32_t api); + Field * posFieldForMessage(uint32_t api); + Field * ictrlFieldForMessage(uint32_t api); + Field * statusFieldForMessage(uint32_t api); + Field * cfgFieldForMessage(uint32_t api); + + // Frequency Status for diagnostics + std::shared_ptr can_feedback_rate_; // Shared ptr prevents copy errors of FrequencyStatus + std::shared_ptr can_feedback_freq_status_; +}; + +} // namespace puma_motor_driver + +#endif // PUMA_MOTOR_DRIVER_DRIVER_H diff --git a/src/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp b/src/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp new file mode 100644 index 0000000..6bbd172 --- /dev/null +++ b/src/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp @@ -0,0 +1,202 @@ +/** +Software License Agreement (BSD) + +\authors Luis Camero +\copyright Copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that +the following conditions are met: + * Redistributions of source code must retain the above copyright notice, this list of conditions and the + following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- +RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- +DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +#ifndef PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H +#define PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64.hpp" +#include "sensor_msgs/msg/joint_state.hpp" + +#include "clearpath_motor_msgs/msg/puma_multi_status.hpp" +#include "clearpath_motor_msgs/msg/puma_status.hpp" +#include "clearpath_motor_msgs/msg/puma_multi_feedback.hpp" +#include "clearpath_motor_msgs/msg/puma_feedback.hpp" + +#include "can_hardware/common/types.hpp" +#include "can_hardware/drivers/socketcan_driver.hpp" + +#include "diagnostic_updater/diagnostic_updater.hpp" + +#include "puma_motor_driver/driver.hpp" +// #include "puma_motor_driver/diagnostic_updater.hpp" + +namespace FeedbackBit +{ +enum +{ + DutyCycle, + Current, + Position, + Speed, + Setpoint, + Count, +}; +} + +namespace StatusBit +{ +enum +{ + BusVoltage, + OutVoltage, + AnalogInput, + Temperature, + Mode, + Fault, + Count, +}; +} + +class MultiPumaNode + : public rclcpp::Node +{ +public: + MultiPumaNode(const std::string node_name); + + /** + * Receives desired motor speeds in sensor_msgs::JointState format and + * sends commands to each motor over CAN. + */ + void cmdCallback(const sensor_msgs::msg::JointState::SharedPtr msg); + + /** + * Checks if feedback fields have been received from each motor driver. + * If feedback is avaiable, creates the feedback message and returns + * true. Otherwise, returns false. + */ + bool getFeedback(); + + /** + * Checks if status fields have been received from each motor driver. + * If status data is available, creates the status message and returns + * true. Otherwise, returns false. + */ + bool getStatus(); + + /** + * If feedback message was created, publishes feedback message. + */ + void publishFeedback(); + + /** + * If status message was created, publishes status message. + */ + void publishStatus(); + + /** + * Checks that all motor drivers have been configured and are active. + */ + bool areAllActive(); + + /** + * Checks if socket connection is active. If not, attempts to establish + * a connection. + */ + bool connectIfNotConnected(); + + /** + * @brief Callback function for processing incoming CAN frames. + * + * @param frame The received CAN frame. + */ + void frameCallback(const can_hardware::Frame & frame); + + /** + * Main control loop that checks and maintains the socket gateway, resets + * and reconfigures drivers that have disconnected, verifies parameters + * are set appropriately, receives motor data, and publishes the feedback + * and status messages. + */ + void run(); + +private: + using DiagnosticStatusWrapper = diagnostic_updater::DiagnosticStatusWrapper; + using PumaStatus = clearpath_motor_msgs::msg::PumaStatus; + + // std::shared_ptr interface_; + std::shared_ptr interface_; + std::vector drivers_; + + bool active_; + double gear_ratio_; + int encoder_cpr_; + int freq_; + uint8_t status_count_; + uint8_t desired_mode_; + std::string canbus_dev_; + std::vector joint_names_; + std::vector joint_can_ids_; + std::vector joint_directions_; + + std::queue recv_msg_queue_; + std::mutex recv_msg_mutex_; + + clearpath_motor_msgs::msg::PumaMultiStatus status_msg_; + clearpath_motor_msgs::msg::PumaMultiFeedback feedback_msg_; + + double gain_p_; + double gain_i_; + double gain_d_; + + rclcpp::Node::SharedPtr node_handle_; + rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::Publisher::SharedPtr feedback_pub_; + rclcpp::Subscription::SharedPtr cmd_sub_; + rclcpp::TimerBase::SharedPtr run_timer_; + + // Diagnostic Updater + diagnostic_updater::Updater updater_; + + // Diagnostic labels + const std::map MODE_FLAG_LABELS_ = { + {PumaStatus::MODE_VOLTAGE, "Voltage"}, + {PumaStatus::MODE_CURRENT, "Current"}, + {PumaStatus::MODE_SPEED, "Speed"}, + {PumaStatus::MODE_POSITION, "Position"}, + {PumaStatus::MODE_VCOMP, "V-Comp"}, + }; + const std::map FAULT_FLAG_LABELS_ = { + {PumaStatus::FAULT_CURRENT, "Current Fault"}, + {PumaStatus::FAULT_TEMPERATURE, "Temperature Fault"}, + {PumaStatus::FAULT_BUS_VOLTAGE, "Bus Voltage Fault"}, + {PumaStatus::FAULT_BRIDGE_DRIVER, "Bridge Driver Fault"}, + }; + const std::map PUMA_MOTOR_LABELS_ = { + {"front_left_wheel_joint", "Front Left"}, + {"front_right_wheel_joint", "Front Right"}, + {"rear_left_wheel_joint", "Rear Left"}, + {"rear_right_wheel_joint", "Rear Right"}, + }; + + + // Diagnostic Tasks + void driverDiagnostic(DiagnosticStatusWrapper & stat, int i); + void protectionDiagnostic(DiagnosticStatusWrapper & stat); +}; + +#endif // PUMA_MOTOR_DRIVER_PUMA_NODE_H diff --git a/src/puma_motor_driver/package.xml b/src/puma_motor_driver/package.xml new file mode 100644 index 0000000..603c70b --- /dev/null +++ b/src/puma_motor_driver/package.xml @@ -0,0 +1,26 @@ + + + + puma_motor_driver + 2.9.6 + Motor driver for Puma motor control board that utilizes ROS 2 Socketcan Gateway + Luis Camero + BSD-3-Clause + Mike Purvis + + ament_cmake + + can_hardware + clearpath_motor_msgs + diagnostic_updater + rclcpp + sensor_msgs + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/puma_motor_driver/src/driver.cpp b/src/puma_motor_driver/src/driver.cpp new file mode 100644 index 0000000..f39516b --- /dev/null +++ b/src/puma_motor_driver/src/driver.cpp @@ -0,0 +1,1057 @@ +/** +Software License Agreement (BSD) + +\authors Mike Purvis +\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that +the following conditions are met: + * Redistributions of source code must retain the above copyright notice, this list of conditions and the + following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- +RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- +DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +#include "clearpath_motor_msgs/msg/puma_status.hpp" + +#include "puma_motor_driver/driver.hpp" + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +// must match firmware +#define CAN_FEEDBACK_RATE 40.0 + +namespace puma_motor_driver +{ + +namespace ConfigurationStates +{ +enum ConfigurationState +{ + Unknown = -1, + Initializing, + PowerFlag, + EncoderPosRef, + EncoderSpdRef, + EncoderCounts, + ClosedLoop, + ControlMode, + PGain, + IGain, + DGain, + VerifiedParameters, + Configured +}; +} // namespace ConfigurationStates +typedef ConfigurationStates::ConfigurationState ConfigurationState; + +Driver::Driver( + const std::shared_ptr interface, + std::shared_ptr nh, + const uint8_t & device_number, + const std::string & device_name) +: interface_(interface), + nh_(nh), + device_number_(device_number), + device_name_(device_name), + configured_(false), + state_(ConfigurationState::Initializing), + last_power_clear_ts_(0.0), + control_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED), + gain_p_(1), + gain_i_(0), + gain_d_(0), + encoder_cpr_(1), + gear_ratio_(1) +{ + can_feedback_rate_ = std::make_shared(CAN_FEEDBACK_RATE); + can_feedback_freq_status_ = std::make_shared( + diagnostic_updater::FrequencyStatusParam( + can_feedback_rate_.get(), + can_feedback_rate_.get(), + 0.1, + 5 + ) + ); +} + +void Driver::processMessage(const can_hardware::Frame & received_msg) +{ + // If it's not our message, jump out. + if (getDeviceNumber(received_msg) != device_number_) { + return; + } + + // If there's no data then this is a request message, jump out. + if (received_msg.dlc == 0) { + return; + } + + Field * field = nullptr; + uint32_t received_api = getApi(received_msg); + if ((received_api & CAN_MSGID_API_M & CAN_API_MC_CFG) == CAN_API_MC_CFG) { + field = cfgFieldForMessage(received_api); + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_STATUS) == CAN_API_MC_STATUS) { + field = statusFieldForMessage(received_api); + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_ICTRL) == CAN_API_MC_ICTRL) { + field = ictrlFieldForMessage(received_api); + can_feedback_freq_status_->tick(); + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_POS) == CAN_API_MC_POS) { + field = posFieldForMessage(received_api); + can_feedback_freq_status_->tick(); + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VCOMP) == CAN_API_MC_VCOMP) { + field = vcompFieldForMessage(received_api); + can_feedback_freq_status_->tick(); + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_SPD) == CAN_API_MC_SPD) { + field = spdFieldForMessage(received_api); + can_feedback_freq_status_->tick(); + } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VOLTAGE) == CAN_API_MC_VOLTAGE) { + field = voltageFieldForMessage(received_api); + can_feedback_freq_status_->tick(); + } + + if (!field) { + return; + } + + // Copy the received data and mark that field as received. + std::copy_n(std::begin(received_msg.data), Field::FIELD_STRUCT_DATA_SIZE, + std::begin(field->data)); + field->received = true; +} + +double Driver::radPerSecToRpm() const +{ + return (60 * gear_ratio_) / (2 * M_PI); +} + +void Driver::sendId(const uint32_t id) +{ + auto msg = getMsg(id); + interface_->sendFrame(msg); +} + +void Driver::sendUint8(const uint32_t id, const uint8_t value) +{ + auto msg = getMsg(id); + msg.dlc = sizeof(uint8_t); + uint8_t data[8] = {0}; + std::memcpy(data, &value, sizeof(uint8_t)); + std::copy(std::begin(data), std::end(data), std::begin(msg.data)); + + interface_->sendFrame(msg); +} + +void Driver::sendUint16(const uint32_t id, const uint16_t value) +{ + auto msg = getMsg(id); + msg.dlc = sizeof(uint16_t); + uint8_t data[8] = {0}; + std::memcpy(data, &value, sizeof(uint16_t)); + std::copy(std::begin(data), std::end(data), std::begin(msg.data)); + + interface_->sendFrame(msg); +} + +void Driver::sendFixed8x8(const uint32_t id, const float value) +{ + auto msg = getMsg(id); + msg.dlc = sizeof(int16_t); + int16_t output_value = static_cast(static_cast(1 << 8) * value); + + uint8_t data[8] = {0}; + std::memcpy(data, &output_value, sizeof(int16_t)); + std::copy(std::begin(data), std::end(data), std::begin(msg.data)); + + interface_->sendFrame(msg); +} + +void Driver::sendFixed16x16(const uint32_t id, const double value) +{ + auto msg = getMsg(id); + msg.dlc = sizeof(int32_t); + int32_t output_value = static_cast(static_cast((1 << 16) * value)); + + uint8_t data[8] = {0}; + std::memcpy(data, &output_value, sizeof(int32_t)); + std::copy(std::begin(data), std::end(data), std::begin(msg.data)); + + interface_->sendFrame(msg); +} + +can_hardware::Frame Driver::getMsg(const uint32_t id) +{ + can_hardware::Frame msg; + msg.id = id; + msg.dlc = 0; + msg.is_extended = true; + return msg; +} + +uint32_t Driver::getApi(const can_hardware::Frame & msg) +{ + return msg.id & (CAN_MSGID_FULL_M ^ CAN_MSGID_DEVNO_M); +} + +uint32_t Driver::getDeviceNumber(const can_hardware::Frame & msg) +{ + return msg.id & CAN_MSGID_DEVNO_M; +} + +bool Driver::verifyRaw16x16(const uint8_t * received, const double expected) +{ + uint8_t data[4]; + int32_t output_value = static_cast(static_cast((1 << 16) * expected)); + std::memcpy(data, &output_value, 4); + for (uint8_t i = 0; i < 4; i++) { + if (*received != data[i]) { + return false; + } + received++; + } + return true; +} + +bool Driver::verifyRaw8x8(const uint8_t * received, const float expected) +{ + uint8_t data[2]; + int32_t output_value = static_cast(static_cast((1 << 8) * expected)); + std::memcpy(data, &output_value, 2); + for (uint8_t i = 0; i < 2; i++) { + if (*received != data[i]) { + return false; + } + received++; + } + return true; +} + +void Driver::setEncoderCPR(const uint16_t encoder_cpr) +{ + encoder_cpr_ = encoder_cpr; +} + +void Driver::setGearRatio(const float gear_ratio) +{ + gear_ratio_ = gear_ratio; +} + +void Driver::commandDutyCycle(const float cmd) +{ + sendFixed8x8((LM_API_VOLT_SET | device_number_), cmd); +} + +void Driver::commandSpeed(const double cmd) +{ + // Converting from rad/s to RPM through the gearbox. + sendFixed16x16((LM_API_SPD_SET | device_number_), (cmd * radPerSecToRpm())); +} + +void Driver::verifyParams() +{ + switch (state_) { + case ConfigurationState::Initializing: + // Don't advance state until we receive LM_API_STATUS_POWER msg + if (receivedPower()) { + state_ = ConfigurationState::PowerFlag; + } else { + sendId(LM_API_STATUS_POWER | device_number_); + } + break; + case ConfigurationState::PowerFlag: + if (receivedPower() && (lastPower() == 0)) { + // Only advance state when we have a new LM_API_STATUS_POWER msg + // with the power flag cleared + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): cleared power flag.", + device_name_.c_str(), device_number_); + state_ = ConfigurationState::EncoderPosRef; + } else { + sendId(LM_API_STATUS_POWER | device_number_); + } + break; + case ConfigurationState::EncoderPosRef: + if (posEncoderRef() == LM_REF_ENCODER) { + state_ = ConfigurationState::EncoderSpdRef; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): set position encoder reference.", + device_name_.c_str(), device_number_); + } else { + sendId(LM_API_POS_REF | device_number_); + } + break; + case ConfigurationState::EncoderSpdRef: + if (spdEncoderRef() == LM_REF_QUAD_ENCODER) { + state_ = ConfigurationState::EncoderCounts; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): set speed encoder reference.", + device_name_.c_str(), device_number_); + } else { + sendId(LM_API_SPD_REF | device_number_); + } + break; + case ConfigurationState::EncoderCounts: + if (encoderCounts() == encoder_cpr_) { + state_ = ConfigurationState::ClosedLoop; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): set encoder counts to %i.", + device_name_.c_str(), device_number_, encoder_cpr_); + } else { + sendId(LM_API_CFG_ENC_LINES | device_number_); + } + break; + case ConfigurationState::ClosedLoop: // Need to enter a close loop mode to record encoder data. + if (lastMode() == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { + state_ = ConfigurationState::ControlMode; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): entered a close-loop control mode.", + device_name_.c_str(), device_number_); + } else { + sendId(LM_API_STATUS_CMODE | device_number_); + } + break; + case ConfigurationState::ControlMode: + if (lastMode() == control_mode_) { + if (control_mode_ != clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { + state_ = ConfigurationState::PGain; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): was set to a close loop control mode.", + device_name_.c_str(), device_number_); + } else { + state_ = ConfigurationState::VerifiedParameters; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): was set to voltage control mode.", + device_name_.c_str(), device_number_); + } + } + break; + case ConfigurationState::PGain: + if (verifyRaw16x16(getRawP(), gain_p_)) { + state_ = ConfigurationState::IGain; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getP(), gain_p_); + } else { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getP(), gain_p_); + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendId(LM_API_ICTRL_PC | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendId(LM_API_POS_PC | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendId(LM_API_SPD_PC | device_number_); + break; + } + } + break; + case ConfigurationState::IGain: + if (verifyRaw16x16(getRawI(), gain_i_)) { + state_ = ConfigurationState::DGain; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getI(), gain_i_); + } else { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getI(), gain_i_); + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendId(LM_API_ICTRL_IC | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendId(LM_API_POS_IC | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendId(LM_API_SPD_IC | device_number_); + break; + } + } + break; + case ConfigurationState::DGain: + if (verifyRaw16x16(getRawD(), gain_d_)) { + state_ = ConfigurationState::VerifiedParameters; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getD(), gain_d_); + } else { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", + device_name_.c_str(), device_number_, getD(), gain_d_); + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendId(LM_API_ICTRL_DC | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendId(LM_API_POS_DC | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendId(LM_API_SPD_DC | device_number_); + break; + } + } + break; + } + if (state_ == ConfigurationState::VerifiedParameters) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): all parameters verified.", + device_name_.c_str(), device_number_); + configured_ = true; + state_ = ConfigurationState::Configured; + } +} + +void Driver::configureParams() +{ + const double now = nh_->get_clock()->now().seconds(); + switch (state_) { + case ConfigurationState::Initializing: + break; + case ConfigurationState::PowerFlag: + // Continue to check last power flag until it has been cleared + if (lastPower() == 1) { + // Send request every second + if ((now - last_power_clear_ts_) > 1.0) { + sendUint8((LM_API_STATUS_POWER | device_number_), 1); + last_power_clear_ts_ = now; + } + } + break; + case ConfigurationState::EncoderPosRef: + sendUint8((LM_API_POS_REF | device_number_), LM_REF_ENCODER); + break; + case ConfigurationState::EncoderSpdRef: + sendUint8((LM_API_SPD_REF | device_number_), LM_REF_QUAD_ENCODER); + break; + case ConfigurationState::EncoderCounts: + // Set encoder CPR + sendUint16((LM_API_CFG_ENC_LINES | device_number_), encoder_cpr_); + break; + case ConfigurationState::ClosedLoop: // Need to enter a close loop mode to record encoder data. + sendId(LM_API_SPD_EN | device_number_); + break; + case ConfigurationState::ControlMode: + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: + sendId(LM_API_VOLT_EN | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendId(LM_API_ICTRL_EN | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendId(LM_API_POS_EN | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendId(LM_API_SPD_EN | device_number_); + break; + } + break; + case ConfigurationState::PGain: + // Set P + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendFixed16x16((LM_API_ICTRL_PC | device_number_), gain_p_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendFixed16x16((LM_API_POS_PC | device_number_), gain_p_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendFixed16x16((LM_API_SPD_PC | device_number_), gain_p_); + break; + } + break; + case ConfigurationState::IGain: + // Set I + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendFixed16x16((LM_API_ICTRL_IC | device_number_), gain_i_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendFixed16x16((LM_API_POS_IC | device_number_), gain_i_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendFixed16x16((LM_API_SPD_IC | device_number_), gain_i_); + break; + } + break; + case ConfigurationState::DGain: + // Set D + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendFixed16x16((LM_API_ICTRL_DC | device_number_), gain_d_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendFixed16x16((LM_API_POS_DC | device_number_), gain_d_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendFixed16x16((LM_API_SPD_DC | device_number_), gain_d_); + break; + } + break; + } +} + +bool Driver::isConfigured() const +{ + return configured_; +} + +void Driver::setGains(const double p, const double i, const double d) +{ + gain_p_ = p; + gain_i_ = i; + gain_d_ = d; + + if (configured_) { + updateGains(); + } +} + +void Driver::setMode(const uint8_t mode) +{ + if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { + control_mode_ = mode; + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): mode set to voltage control.", + device_name_.c_str(), device_number_); + if (configured_) { + resetConfiguration(); + } + } else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): Close loop modes need PID gains.", + device_name_.c_str(), device_number_); + } +} + +void Driver::setMode(const uint8_t mode, const double p, const double i, const double d) +{ + if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { + control_mode_ = mode; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): mode set to voltage control but PID gains are not needed.", + device_name_.c_str(), device_number_); + if (configured_) { + resetConfiguration(); + } + } else { + control_mode_ = mode; + if (configured_) { + resetConfiguration(); + } + setGains(p, i, d); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Puma Motor Controller on %s (%i): mode set to a closed-loop control with PID gains of P:%f, I:%f and D:%f.", + device_name_.c_str(), device_number_, gain_p_, gain_i_, gain_d_); + } +} + +void Driver::clearMsgCache() +{ + // Set it all to zero, which will in part clear + // the boolean flags to be false. + memset(voltage_fields_, 0, sizeof(voltage_fields_)); + memset(spd_fields_, 0, sizeof(spd_fields_)); + memset(vcomp_fields_, 0, sizeof(vcomp_fields_)); + memset(pos_fields_, 0, sizeof(pos_fields_)); + memset(ictrl_fields_, 0, sizeof(ictrl_fields_)); + memset(status_fields_, 0, sizeof(status_fields_)); + memset(cfg_fields_, 0, sizeof(cfg_fields_)); +} + +void Driver::requestStatusMessages() +{ + sendId(LM_API_STATUS_POWER | device_number_); +} + +void Driver::requestFeedbackMessages() +{ + sendId(LM_API_STATUS_VOLTOUT | device_number_); + sendId(LM_API_STATUS_CURRENT | device_number_); + sendId(LM_API_STATUS_POS | device_number_); + sendId(LM_API_STATUS_SPD | device_number_); + sendId(LM_API_SPD_SET | device_number_); +} +void Driver::requestFeedbackDutyCycle() +{ + sendId(LM_API_STATUS_VOLTOUT | device_number_); +} + +void Driver::requestFeedbackCurrent() +{ + sendId(LM_API_STATUS_CURRENT | device_number_); +} + +void Driver::requestFeedbackPosition() +{ + sendId(LM_API_STATUS_POS | device_number_); +} + +void Driver::requestFeedbackSpeed() +{ + sendId(LM_API_STATUS_SPD | device_number_); +} + +void Driver::requestFeedbackPowerState() +{ + sendId(LM_API_STATUS_POWER | device_number_); +} + +void Driver::requestFeedbackSetpoint() +{ + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + sendId(LM_API_ICTRL_SET | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + sendId(LM_API_POS_SET | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + sendId(LM_API_SPD_SET | device_number_); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: + sendId(LM_API_VOLT_SET | device_number_); + break; + } +} + +void Driver::resetConfiguration() +{ + configured_ = false; + state_ = ConfigurationState::Initializing; +} + +void Driver::updateGains() +{ + configured_ = false; + state_ = ConfigurationState::PGain; +} + +bool Driver::receivedDutyCycle() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); + return field->received; +} + +bool Driver::receivedBusVoltage() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); + return field->received; +} + +bool Driver::receivedCurrent() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); + return field->received; +} + +bool Driver::receivedPosition() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); + return field->received; +} + +bool Driver::receivedSpeed() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); + return field->received; +} + +bool Driver::receivedFault() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); + return field->received; +} + +bool Driver::receivedPower() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); + return field->received; +} + +bool Driver::receivedMode() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); + return field->received; +} + +bool Driver::receivedOutVoltage() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); + return field->received; +} + +bool Driver::receivedTemperature() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); + return field->received; +} + +bool Driver::receivedAnalogInput() +{ + Field * field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); + return field->received; +} + +bool Driver::receivedSetpoint() +{ + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + return receivedCurrentSetpoint(); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + return receivedPositionSetpoint(); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + return receivedSpeedSetpoint(); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: + return receivedDutyCycleSetpoint(); + break; + default: + return 0; + break; + } +} + +bool Driver::receivedSpeedSetpoint() +{ + Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); + return field->received; +} + +bool Driver::receivedDutyCycleSetpoint() +{ + Field * field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); + return field->received; +} + +bool Driver::receivedCurrentSetpoint() +{ + Field * field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); + return field->received; +} + +bool Driver::receivedPositionSetpoint() +{ + Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); + return field->received; +} + +float Driver::lastDutyCycle() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); + field->received = false; + return field->interpretFixed8x8() / 128.0; +} + +float Driver::lastBusVoltage() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); + field->received = false; + return field->interpretFixed8x8(); +} + +float Driver::lastCurrent() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); + field->received = false; + return field->interpretFixed8x8(); +} + +double Driver::lastPosition() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); + field->received = false; + return field->interpretFixed16x16() * ((2 * M_PI) / gear_ratio_); // Convert rev to rad +} + +double Driver::lastSpeed() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); + field->received = false; + return field->interpretFixed16x16() * ((2 * M_PI) / (gear_ratio_ * 60)); // Convert RPM to rad/s +} + +uint8_t Driver::lastFault() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); + field->received = false; + return field->data[0]; +} + +uint8_t Driver::lastPower() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); + field->received = false; + return field->data[0]; +} + +uint8_t Driver::lastMode() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); + field->received = false; + return field->data[0]; +} + +float Driver::lastOutVoltage() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); + field->received = false; + return field->interpretFixed8x8(); +} + +float Driver::lastTemperature() +{ + Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); + field->received = false; + return field->interpretFixed8x8(); +} + +float Driver::lastAnalogInput() +{ + Field * field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); + field->received = false; + return field->interpretFixed8x8(); +} + +double Driver::lastSetpoint() +{ + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + return statusCurrentGet(); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + return statusPositionGet(); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + return statusSpeedGet(); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: + return statusDutyCycleGet(); + break; + default: + return 0; + break; + } +} +double Driver::statusSpeedGet() +{ + Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); + field->received = false; + return field->interpretFixed16x16() * ((2 * M_PI) / (gear_ratio_ * 60)); // Convert RPM to rad/s +} + +float Driver::statusDutyCycleGet() +{ + Field * field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); + field->received = false; + return field->interpretFixed8x8() / 128.0; +} + +float Driver::statusCurrentGet() +{ + Field * field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); + field->received = false; + return field->interpretFixed8x8(); +} + +double Driver::statusPositionGet() +{ + Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); + field->received = false; + return field->interpretFixed16x16() * (( 2 * M_PI) / gear_ratio_); // Convert rev to rad +} + +uint8_t Driver::posEncoderRef() +{ + Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_REF))); + return field->data[0]; +} + +uint8_t Driver::spdEncoderRef() +{ + Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_REF))); + return field->data[0]; +} + +uint16_t Driver::encoderCounts() +{ + Field * field = cfgFieldForMessage(getApi(getMsg(LM_API_CFG_ENC_LINES))); + return static_cast(field->data[0]) | static_cast(field->data[1] << 8); +} + +double Driver::getP() +{ + Field * field = nullptr; + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_PC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + field = posFieldForMessage(getApi(getMsg(LM_API_POS_PC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_PC))); + break; + } + return field->interpretFixed16x16(); +} + +double Driver::getI() +{ + Field * field = nullptr; + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_IC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + field = posFieldForMessage(getApi(getMsg(LM_API_POS_IC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_IC))); + break; + } + return field->interpretFixed16x16(); +} + +double Driver::getD() +{ + Field * field = nullptr; + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_DC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + field = posFieldForMessage(getApi(getMsg(LM_API_POS_DC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_DC))); + break; + } + return field->interpretFixed16x16(); +} + +uint8_t * Driver::getRawP() +{ + Field * field = nullptr; + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_PC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + field = posFieldForMessage(getApi(getMsg(LM_API_POS_PC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_PC))); + break; + } + return field->data; +} + +uint8_t * Driver::getRawI() +{ + Field * field = nullptr; + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_IC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + field = posFieldForMessage(getApi(getMsg(LM_API_POS_IC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_IC))); + break; + } + return field->data; +} + +uint8_t * Driver::getRawD() +{ + Field * field = nullptr; + switch (control_mode_) { + case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: + field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_DC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: + field = posFieldForMessage(getApi(getMsg(LM_API_POS_DC))); + break; + case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: + field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_DC))); + break; + } + return field->data; +} + +Driver::Field * Driver::voltageFieldForMessage(uint32_t api) +{ + uint32_t voltage_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &voltage_fields_[voltage_field_index]; +} + +Driver::Field * Driver::spdFieldForMessage(uint32_t api) +{ + uint32_t spd_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &spd_fields_[spd_field_index]; +} + +Driver::Field * Driver::vcompFieldForMessage(uint32_t api) +{ + uint32_t vcomp_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &vcomp_fields_[vcomp_field_index]; +} + +Driver::Field * Driver::posFieldForMessage(uint32_t api) +{ + uint32_t pos_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &pos_fields_[pos_field_index]; +} + +Driver::Field * Driver::ictrlFieldForMessage(uint32_t api) +{ + uint32_t ictrl_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &ictrl_fields_[ictrl_field_index]; +} + +Driver::Field * Driver::statusFieldForMessage(uint32_t api) +{ + uint32_t status_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &status_fields_[status_field_index]; +} + +Driver::Field * Driver::cfgFieldForMessage(uint32_t api) +{ + uint32_t cfg_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; + return &cfg_fields_[cfg_field_index]; +} + +/** + * @brief Runs the frequency diagnostic update to populate the status message + */ +void Driver::runFreqStatus(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + can_feedback_freq_status_->run(stat); + + stat.add("Duty cycle", lastDutyCycle()); + stat.add("Current (A)", lastCurrent()); + stat.add("Speed (rad/s)", lastSpeed()); + stat.add("Position", lastPosition()); + stat.add("Setpoint", lastSetpoint()); +} + +} // namespace puma_motor_driver diff --git a/src/puma_motor_driver/src/multi_puma_node.cpp b/src/puma_motor_driver/src/multi_puma_node.cpp new file mode 100644 index 0000000..33e96e8 --- /dev/null +++ b/src/puma_motor_driver/src/multi_puma_node.cpp @@ -0,0 +1,368 @@ +/** +Software License Agreement (BSD) + +\authors Luis Camero +\copyright Copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that +the following conditions are met: + * Redistributions of source code must retain the above copyright notice, this list of conditions and the + following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + following disclaimer in the documentation and/or other materials provided with the distribution. + * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote + products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- +RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- +DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +#include +#include +#include + +#include "puma_motor_driver/multi_puma_node.hpp" + +MultiPumaNode::MultiPumaNode(const std::string node_name) +:Node(node_name), + active_(false), + status_count_(0), + desired_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED), + updater_(this) +{ + // Parameters + this->declare_parameter("canbus_dev", "vcan0"); + this->declare_parameter("encoder_cpr", 1024); + this->declare_parameter("frequency", 20); + this->declare_parameter("gain.p", 0.1); + this->declare_parameter("gain.i", 0.01); + this->declare_parameter("gain.d", 0.0); + this->declare_parameter("gear_ratio", 24.0); + this->declare_parameter("joint_can_ids", std::vector()); + this->declare_parameter("joint_directions", std::vector()); + this->declare_parameter("joint_names", std::vector()); + + this->get_parameter("canbus_dev", canbus_dev_); + this->get_parameter("encoder_cpr", encoder_cpr_); + this->get_parameter("frequency", freq_); + this->get_parameter("gain.p", gain_p_); + this->get_parameter("gain.i", gain_i_); + this->get_parameter("gain.d", gain_d_); + this->get_parameter("gear_ratio", gear_ratio_); + joint_can_ids_ = this->get_parameter("joint_can_ids").as_integer_array(); + joint_directions_ = this->get_parameter("joint_directions").as_integer_array(); + joint_names_ = this->get_parameter("joint_names").as_string_array(); + + RCLCPP_INFO( + this->get_logger(), + "Gear Ratio %f\nEncoder CPR %d\nFrequency %d\nGain PID %f,%f,%f\nCANBus Device %s", + gear_ratio_, + encoder_cpr_, + freq_, + gain_p_, + gain_i_, + gain_d_, + canbus_dev_.c_str() + ); + + // Validate Parameters + if (joint_names_.size() != joint_can_ids_.size() || + joint_names_.size() != joint_directions_.size()) + { + RCLCPP_ERROR( + this->get_logger(), + "Length of joint_name list must match length of joint_can_ids list and joint_directions list."); + return; + } + + // Subsciber + cmd_sub_ = this->create_subscription( + "platform/motors/cmd", + rclcpp::SensorDataQoS(), + std::bind(&MultiPumaNode::cmdCallback, this, std::placeholders::_1)); + + // Publishers + feedback_pub_ = this->create_publisher( + "platform/motors/feedback", + rclcpp::SensorDataQoS()); + status_pub_ = this->create_publisher( + "platform/motors/status", + rclcpp::SensorDataQoS()); + + node_handle_ = std::shared_ptr(this, [](rclcpp::Node *){}); + + // SocketCAN Interface + interface_ = std::make_shared(canbus_dev_); + interface_->registerFrameCallback( + std::bind(&MultiPumaNode::frameCallback, this, std::placeholders::_1)); + + for (uint8_t i = 0; i < joint_names_.size(); i++) { + drivers_.push_back(puma_motor_driver::Driver( + interface_, + node_handle_, + joint_can_ids_[i], + joint_names_[i] + )); + } + + feedback_msg_.drivers_feedback.resize(drivers_.size()); + status_msg_.drivers.resize(drivers_.size()); + + uint8_t i = 0; + for (auto & driver : drivers_) { + driver.clearMsgCache(); + driver.setEncoderCPR(encoder_cpr_); + driver.setGearRatio(gear_ratio_ * joint_directions_[i]); + driver.setMode(desired_mode_, gain_p_, gain_i_, gain_d_); + i++; + } + + run_timer_ = this->create_wall_timer( + std::chrono::milliseconds(1000 / freq_), std::bind(&MultiPumaNode::run, this)); + + // Setup diagnostics + updater_.setHardwareID("Puma"); + for (uint8_t i = 0; i < joint_names_.size(); i++) { + std::string name = "Puma Motor Driver " + std::to_string(i + 1) + " (" + joint_names_[i] + ")"; + updater_.add(name, std::bind(&MultiPumaNode::driverDiagnostic, this, std::placeholders::_1, i)); + } +} + +bool MultiPumaNode::getFeedback() +{ + // Check All Fields Received + uint8_t received = 0; + for (auto & driver : drivers_) { + received |= driver.receivedDutyCycle() << FeedbackBit::DutyCycle; + received |= driver.receivedCurrent() << FeedbackBit::Current; + received |= driver.receivedPosition() << FeedbackBit::Position; + received |= driver.receivedSpeed() << FeedbackBit::Speed; + received |= driver.receivedSetpoint() << FeedbackBit::Setpoint; + } + + if (received != (1 << FeedbackBit::Count) - 1) { + return false; + } + + uint8_t feedback_index = 0; + for (auto & driver : drivers_) { + clearpath_motor_msgs::msg::PumaFeedback * f = &feedback_msg_.drivers_feedback[feedback_index]; + f->device_number = driver.deviceNumber(); + f->device_name = driver.deviceName(); + f->duty_cycle = driver.lastDutyCycle(); + f->current = driver.lastCurrent(); + f->travel = driver.lastPosition(); + f->speed = driver.lastSpeed(); + f->setpoint = driver.lastSetpoint(); + + feedback_index++; + } + feedback_msg_.header.stamp = this->get_clock()->now(); + return true; +} + +bool MultiPumaNode::getStatus() +{ + // Check All Fields Received + uint8_t status_index = 0; + uint8_t received_fields = 0; + uint8_t received_status = 0; + for (auto & driver : drivers_) { + received_fields |= driver.receivedBusVoltage() << StatusBit::BusVoltage; + received_fields |= driver.receivedOutVoltage() << StatusBit::OutVoltage; + received_fields |= driver.receivedAnalogInput() << StatusBit::AnalogInput; + received_fields |= 1 << StatusBit::AnalogInput; + received_fields |= driver.receivedTemperature() << StatusBit::Temperature; + received_fields |= driver.receivedMode() << StatusBit::Mode; + received_fields |= driver.receivedFault() << StatusBit::Fault; + if (received_fields != (1 << StatusBit::Count) - 1) { + RCLCPP_DEBUG(this->get_logger(), "Received Status Fields %x", received_fields); + } else { + received_status |= 1 << status_index; + } + status_index++; + } + + if (received_status != (1 << status_index) - 1) { + RCLCPP_DEBUG(this->get_logger(), "Received Status %x", received_status); + return false; + } + + // Prepare output status message to ROS. + status_index = 0; + for (auto & driver : drivers_) { + clearpath_motor_msgs::msg::PumaStatus * s = &status_msg_.drivers[status_index]; + s->device_number = driver.deviceNumber(); + s->device_name = driver.deviceName(); + s->bus_voltage = driver.lastBusVoltage(); + s->output_voltage = driver.lastOutVoltage(); + s->analog_input = driver.lastAnalogInput(); + s->temperature = driver.lastTemperature(); + s->mode = driver.lastMode(); + s->fault = driver.lastFault(); + + status_index++; + } + status_msg_.header.stamp = this->get_clock()->now(); + return true; +} + +void MultiPumaNode::publishFeedback() +{ + if (getFeedback()) { + feedback_pub_->publish(feedback_msg_); + } +} + +void MultiPumaNode::publishStatus() +{ + if (getStatus()) { + status_pub_->publish(status_msg_); + } +} + +/** + * @brief Diagnostic task to report details for each motor driver + * + * @param i Driver index number + */ +void MultiPumaNode::driverDiagnostic(DiagnosticStatusWrapper & stat, int i) +{ + // Assume we're OK. This will be merged over later on if we aren't + stat.summary(DiagnosticStatusWrapper::OK, "OK"); + + drivers_[i].runFreqStatus(stat); + + // basic stats + stat.add("CAN ID", (int)status_msg_.drivers[i].device_number); + stat.add("Joint Name", status_msg_.drivers[i].device_name); + stat.add("Bus Voltage (V)", status_msg_.drivers[i].bus_voltage); + stat.add("Motor Temperature (C)", status_msg_.drivers[i].temperature); + stat.add("Output Voltage (V)", status_msg_.drivers[i].output_voltage); + stat.add("Analogue Input (V)", status_msg_.drivers[i].analog_input); + + // control mode; convert to a string + stat.add("Mode", MODE_FLAG_LABELS_.at((int)status_msg_.drivers[i].mode)); + + // fault flags; these are a bit field + for (auto label : FAULT_FLAG_LABELS_) { + bool flag = (status_msg_.drivers[i].fault >> label.first) & 0x01; + stat.add(label.second, flag ? "True" : "False"); + if (flag) { + // raise a warning if there's a fault + stat.mergeSummary(DiagnosticStatusWrapper::WARN, label.second); + } + } +} + +void MultiPumaNode::cmdCallback(const sensor_msgs::msg::JointState::SharedPtr msg) +{ + if (active_) { + for (auto & driver : drivers_) { + for (int i = 0; i < static_cast(msg->name.size()); i++) { + if (driver.deviceName() == msg->name[i]) { + if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { + driver.commandDutyCycle(msg->velocity[i]); + } else if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { + driver.commandSpeed(std::isnan(msg->velocity[i]) ? 0.0 : msg->velocity[i]); + } + } + } + } + } +} + +bool MultiPumaNode::areAllActive() +{ + for (auto & driver : drivers_) { + if (!driver.isConfigured()) { + return false; + } + } + return true; +} + +void MultiPumaNode::frameCallback(const can_hardware::Frame & frame) +{ + std::lock_guard lock(recv_msg_mutex_); + recv_msg_queue_.push(frame); +} + +void MultiPumaNode::run() +{ + if (active_) { + // Checks to see if power flag has been reset for each driver + for (auto & driver : drivers_) { + if (driver.lastPower() != 0) { + active_ = false; + RCLCPP_WARN(this->get_logger(), + "Power reset detected on device ID %d, will reconfigure all drivers.", + driver.deviceNumber()); + for (auto & driver : drivers_) { + driver.resetConfiguration(); + } + } + } + // Queue data requests for the drivers in order to assemble an amalgamated status message. + for (auto & driver : drivers_) { + driver.requestStatusMessages(); + driver.requestFeedbackSetpoint(); + } + } else { + // Set parameters for each driver. + for (auto & driver : drivers_) { + driver.configureParams(); + } + } + + while (!recv_msg_queue_.empty()) { + can_hardware::Frame frame; + { + std::lock_guard lock(recv_msg_mutex_); + frame = std::move(recv_msg_queue_.front()); + recv_msg_queue_.pop(); + } + for (auto & driver : drivers_) { + driver.processMessage(frame); + } + } + + // Check parameters of each driver instance. + if (!active_) { + for (auto & driver : drivers_) { + driver.verifyParams(); + } + } + + // Verify that the all drivers are configured. + if (areAllActive() == true && active_ == false) { + active_ = true; + RCLCPP_INFO(this->get_logger(), "All controllers active."); + } + // Broadcast feedback and status messages + if (active_) { + publishFeedback(); + publishStatus(); + } + status_count_++; +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + rclcpp::executors::MultiThreadedExecutor exe; + + std::shared_ptr multi_puma_node = + std::make_shared("multi_puma_node"); + + exe.add_node(multi_puma_node); + exe.spin(); + + rclcpp::shutdown(); + return 0; +} From 0b4ea499875cd4d456f2d70578db334144901cd3 Mon Sep 17 00:00:00 2001 From: Mark Paterson Date: Tue, 1 Sep 2026 11:56:22 -0500 Subject: [PATCH 6/9] Update UR GUI to the current version --- src/drt_ur_gui | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drt_ur_gui b/src/drt_ur_gui index d95bee7..a7776d0 160000 --- a/src/drt_ur_gui +++ b/src/drt_ur_gui @@ -1 +1 @@ -Subproject commit d95bee725f8056173f47a6ae2a45f1c42233b557 +Subproject commit a7776d0d47400a8ebe65c7a782373849ac686ad9 From c50c97947e08446f09ecd82c2f0a5b5901abc037 Mon Sep 17 00:00:00 2001 From: Mark Paterson Date: Tue, 1 Sep 2026 12:46:08 -0500 Subject: [PATCH 7/9] Update to latest phoebe_bridgeback --- src/phoebe_bridgeback | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/phoebe_bridgeback b/src/phoebe_bridgeback index eb7950d..08599a8 160000 --- a/src/phoebe_bridgeback +++ b/src/phoebe_bridgeback @@ -1 +1 @@ -Subproject commit eb7950de0332adf1aa484d0b548a5f5e013863ab +Subproject commit 08599a8c6bc4818892831ddd955e05b8d874fcf3 From 04900f392eeb1ee70c44f921461f2b4214c8498f Mon Sep 17 00:00:00 2001 From: Mark Paterson Date: Wed, 2 Sep 2026 10:46:08 -0500 Subject: [PATCH 8/9] Move puma_motor_driver to phoebe_bridgeback submodule --- src/phoebe_bridgeback | 2 +- src/puma_motor_driver/CHANGELOG.rst | 245 ---- src/puma_motor_driver/CMakeLists.txt | 55 - src/puma_motor_driver/LICENSE | 29 - .../README.clearpath_robot.md | 107 -- src/puma_motor_driver/README.md | 11 - .../include/puma_motor_driver/can_proto.hpp | 399 ------- .../include/puma_motor_driver/driver.hpp | 539 --------- .../puma_motor_driver/multi_puma_node.hpp | 202 ---- src/puma_motor_driver/package.xml | 26 - src/puma_motor_driver/src/driver.cpp | 1057 ----------------- src/puma_motor_driver/src/multi_puma_node.cpp | 368 ------ 12 files changed, 1 insertion(+), 3039 deletions(-) delete mode 100644 src/puma_motor_driver/CHANGELOG.rst delete mode 100644 src/puma_motor_driver/CMakeLists.txt delete mode 100644 src/puma_motor_driver/LICENSE delete mode 100644 src/puma_motor_driver/README.clearpath_robot.md delete mode 100644 src/puma_motor_driver/README.md delete mode 100644 src/puma_motor_driver/include/puma_motor_driver/can_proto.hpp delete mode 100644 src/puma_motor_driver/include/puma_motor_driver/driver.hpp delete mode 100644 src/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp delete mode 100644 src/puma_motor_driver/package.xml delete mode 100644 src/puma_motor_driver/src/driver.cpp delete mode 100644 src/puma_motor_driver/src/multi_puma_node.cpp diff --git a/src/phoebe_bridgeback b/src/phoebe_bridgeback index 08599a8..1a40f8a 160000 --- a/src/phoebe_bridgeback +++ b/src/phoebe_bridgeback @@ -1 +1 @@ -Subproject commit 08599a8c6bc4818892831ddd955e05b8d874fcf3 +Subproject commit 1a40f8a0f695bda3ef43fa8da6c6ea8edf94e347 diff --git a/src/puma_motor_driver/CHANGELOG.rst b/src/puma_motor_driver/CHANGELOG.rst deleted file mode 100644 index f092871..0000000 --- a/src/puma_motor_driver/CHANGELOG.rst +++ /dev/null @@ -1,245 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package puma_motor_driver -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.1.0 (2025-01-15) ------------------- - -2.9.6 (2026-08-13) ------------------- - -2.9.5 (2026-08-07) ------------------- - -2.9.4 (2026-07-17) ------------------- - -2.9.3 (2026-05-21) ------------------- - -2.9.2 (2026-04-14) ------------------- - -2.9.1 (2026-02-11) ------------------- - -2.9.0 (2026-02-11) ------------------- -* Fix: CMake Verision and CI (`#299 `_) - * Updated cmake version to 3.20 - * Switch action to run on container - * Update repos update action version - * Increase test timeout -* Contributors: luis-camero - -2.8.6 (2026-01-20) ------------------- - -2.8.5 (2026-01-19) ------------------- -* CORE-36182: [puma_motor_driver] Port to can_hardware socketcan driver (`#286 `_) - * CORE-36182: [puma_motor_driver] Port to can_hardware socketcan driver - * misc clean up -* Contributors: Natesh Narain - -2.8.4 (2025-11-13) ------------------- - -2.8.3 (2025-11-10) ------------------- - -2.8.2 (2025-10-28) ------------------- - -2.8.1 (2025-10-23) ------------------- - -2.8.0 (2025-10-23) ------------------- -* Add additional Puma diagnostics (`#270 `_) -* Contributors: Chris Iverach-Brereton - -2.7.2 (2025-09-18) ------------------- - -2.7.1 (2025-09-16) ------------------- - -2.7.0 (2025-08-25) ------------------- - -2.6.3 (2025-08-18) ------------------- - -2.6.2 (2025-07-15) ------------------- - -2.6.1 (2025-07-04) ------------------- - -2.6.0 (2025-07-04) ------------------- - -2.5.1 (2025-06-17) ------------------- - -2.5.0 (2025-05-29) ------------------- - -2.4.1 (2025-05-20) ------------------- - -2.4.0 (2025-04-30) ------------------- - -2.3.3 (2025-04-17) ------------------- - -2.3.2 (2025-04-16) ------------------- -* Fixed depend version. -* Contributors: Tony Baltovski - -2.3.1 (2025-04-14) ------------------- - -2.3.0 (2025-04-11) ------------------- -* Reduced feedback frequency to 20hz (`#195 `_) -* Jazzy dingo and ridgeback fixes (`#169 `_) -* Contributors: Roni Kreinin - -2.2.4 (2025-04-07) ------------------- - -2.2.3 (2025-03-20) ------------------- - -2.2.2 (2025-03-17) ------------------- - -2.2.1 (2025-03-12) ------------------- - -2.2.0 (2025-03-11) ------------------- -* [clearpath_motor_drivers/puma_motor_driver] Linting fix. -* [clearpath_motor_drivers/puma_motor_driver] Fixed copy size. -* Contributors: Tony Baltovski - -2.1.2 (2025-02-28) ------------------- - -2.1.1 (2025-02-06) ------------------- - -2.1.0 (2025-01-31) ------------------- - -2.0.4 (2025-01-22) ------------------- - -2.0.3 (2025-01-17) ------------------- - -2.0.2 (2025-01-17) ------------------- - -2.0.1 (2025-01-17) ------------------- - -2.0.0 (2025-01-17) ------------------- -* 1.1.0 -* Changes. -* Add clearpath_motor_drivers, puma_motor_driver -* Contributors: Luis Camero, Tony Baltovski - -1.0.1 (2024-11-28) ------------------- - -1.0.0 (2024-11-26) ------------------- -* Added minimum version. -* Add clearpath_motor_drivers, puma_motor_driver -* Contributors: Luis Camero, Tony Baltovski - -* Added minimum version. -* Merge pull request `#103 `_ from clearpathrobotics/humble-1.0RC - Humble 1.0 RC -* Merge pull request `#104 `_ from clearpathrobotics/feature/reorganization - Package Re-organization -* Add clearpath_motor_drivers, puma_motor_driver -* Contributors: Luis Camero, Tony Baltovski, luis-camero - -0.3.2 (2024-10-04) ------------------- - -0.2.15 (2024-08-12) -------------------- - -0.2.14 (2024-08-08) -------------------- - -0.2.13 (2024-07-30) -------------------- - -0.2.12 (2024-07-22) -------------------- - -0.2.11 (2024-05-28) -------------------- - -0.2.10 (2024-05-16 17:09) -------------------------- - -0.2.9 (2024-05-16 12:19) ------------------------- - -0.2.8 (2024-05-14) ------------------- - -0.2.7 (2024-04-10) ------------------- - -0.2.6 (2024-04-08) ------------------- - -0.2.5 (2024-03-07) ------------------- - -0.2.4 (2024-01-19) ------------------- - -0.2.3 (2024-01-18) ------------------- - -0.2.2 (2024-01-10) ------------------- - -0.2.1 (2023-12-18) ------------------- - -0.2.0 (2023-12-13) ------------------- - -0.1.3 (2023-10-04) ------------------- - -0.1.2 (2023-09-27) ------------------- - -0.1.1 (2023-09-11) ------------------- - -0.1.0 (2023-08-31) ------------------- - -0.0.3 (2023-08-15) ------------------- - -0.0.2 (2023-07-25) ------------------- - -0.0.1 (2023-07-20) ------------------- diff --git a/src/puma_motor_driver/CMakeLists.txt b/src/puma_motor_driver/CMakeLists.txt deleted file mode 100644 index d987fff..0000000 --- a/src/puma_motor_driver/CMakeLists.txt +++ /dev/null @@ -1,55 +0,0 @@ -cmake_minimum_required(VERSION 3.20) -project(puma_motor_driver) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(can_hardware REQUIRED) -find_package(clearpath_motor_msgs REQUIRED) -find_package(diagnostic_updater REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) - - -set(DEPENDENCIES - ament_cmake - can_hardware - clearpath_motor_msgs - diagnostic_updater - rclcpp - std_msgs - sensor_msgs -) - -add_executable(multi_puma_node - src/multi_puma_node.cpp - src/driver.cpp -) -target_include_directories(multi_puma_node PUBLIC - $ - $) -target_compile_features(multi_puma_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 - -ament_target_dependencies(multi_puma_node ${DEPENDENCIES}) -target_link_libraries(multi_puma_node) - -install(TARGETS multi_puma_node - DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/puma_motor_driver/LICENSE b/src/puma_motor_driver/LICENSE deleted file mode 100644 index e9b530e..0000000 --- a/src/puma_motor_driver/LICENSE +++ /dev/null @@ -1,29 +0,0 @@ -BSD 3-Clause License - -Copyright (c) 2023, clearpathrobotics - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - diff --git a/src/puma_motor_driver/README.clearpath_robot.md b/src/puma_motor_driver/README.clearpath_robot.md deleted file mode 100644 index d5c3958..0000000 --- a/src/puma_motor_driver/README.clearpath_robot.md +++ /dev/null @@ -1,107 +0,0 @@ -# Contributing to clearpath_robot - -Thanks for your interest in improving `clearpath_robot`! This is the **on-robot** half of the -stack: it runs on the physical platform's onboard computer, talks to the motor controllers and -sensors, and brings the robot up as a set of `systemd` services. Because it drives real hardware, -please read the notes below before opening a pull request. - -## Getting started - -1. Fork the repository and clone your fork. -2. Create a feature branch off `jazzy`: - - ```bash - git checkout -b my-feature jazzy - ``` - -3. Build the workspace and source it: - - ```bash - rosdep install --from-paths src --ignore-src -r -y - colcon build --symlink-install - source install/setup.bash - ``` - -4. Install the pre-commit hooks (one-time setup): - - ```bash - pip install pre-commit - pre-commit install - ``` - -## Linting - -This repository uses [pre-commit](https://pre-commit.com/) to run linting and formatting checks -(trailing whitespace, end-of-file, YAML/JSON checks, `markdownlint`, `flake8`, and `cspell` -spell-checking) before each commit. Run them against the whole tree before pushing: - -```bash -pre-commit run --all-files -``` - -## Where things live - -See the [Packages section of the README](README.md#packages) for the full map: - -- [`clearpath_robot`](clearpath_robot) — bringup scripts and the `systemd` services that start the - robot on boot. -- [`clearpath_generator_robot`](clearpath_generator_robot) — generates the robot-side launch and - parameter files (builds on `clearpath_generator_common`). -- [`clearpath_hardware_interfaces`](clearpath_hardware_interfaces) and - [`clearpath_motor_drivers`](clearpath_motor_drivers) — hardware and motor controller drivers. -- [`clearpath_sensors`](clearpath_sensors) — default sensor launch/param files. -- [`clearpath_tests`](clearpath_tests) — on-robot hardware/functional test suite. - -## Testing your changes - -Most of this repository interacts with real hardware and the `systemd` bringup, so validate changes -on a robot where possible: - -```bash -sudo systemctl restart clearpath-robot # regenerate + restart everything -sudo systemctl status clearpath-robot # overall bringup status -journalctl -u clearpath-platform -f # follow a single service's logs -ros2 run clearpath_robot generate # regenerate launch/params from robot.yaml -``` - -The [`clearpath_tests`](clearpath_tests) package provides an on-robot functional test suite — see -its [README](clearpath_tests/README.md). Build and run any package unit tests through `colcon`: - -```bash -colcon test --packages-select -colcon test-result --verbose -``` - -Be careful with changes that move the platform — ensure the area is clear and an e-stop is within -reach. - -## Generator tests - -Changes to the generators in this repository (`clearpath_generator_robot`) may affect the generated -output for launch and parameter files. The -[clearpath_generator_tests](https://github.com/clearpathrobotics/clearpath_generator_tests) -repository versions the expected output and validates it through CI. - -Before merging, ensure a corresponding branch with the **same name** exists in -`clearpath_generator_tests` with regenerated samples. See the -[Development Workflow](https://github.com/clearpathrobotics/clearpath_generator_tests#development-workflow) -section of that repository for the full process. - -## Submitting a pull request - -1. Make sure the workspace builds and any tests pass. -2. Push your branch and open a pull request against `jazzy`. -3. Write a clear description of what the change does and the platform/hardware you tested against. -4. If your change touches generator output, note the matching `clearpath_generator_tests` branch. - -## Reporting issues - -Please open issues on the -[GitHub issue tracker](https://github.com/clearpathrobotics/clearpath_robot/issues) and fill out -the bug report template, which walks you through the details we need to reproduce the problem. - -## License - -By contributing, you agree that your contributions will be licensed under the -[BSD-3-Clause license](LICENSE) that covers this project. - diff --git a/src/puma_motor_driver/README.md b/src/puma_motor_driver/README.md deleted file mode 100644 index bc8b898..0000000 --- a/src/puma_motor_driver/README.md +++ /dev/null @@ -1,11 +0,0 @@ -This package is excerpted from https://github.com/clearpathrobotics/clearpath_robot/tree/jazzy -to address one temporary specific issue as follows. - -When the platform velocity controller is deactivated, ros2 control sends NaNs to the -driver. The Puma driver forwards these to the motor controller, which spins the wheels at -maximum velocity. We have made a ros2control MR to send zeroes instead -(https://github.com/ros-controls/ros2_controllers/pull/2578). -In the meantime, we intercept the NaNs in the driver and subsitute zeroes. - -The original README.md from the Clearpath parent project is included here as -README.clearpath_robot.md along with the original LICENSE. diff --git a/src/puma_motor_driver/include/puma_motor_driver/can_proto.hpp b/src/puma_motor_driver/include/puma_motor_driver/can_proto.hpp deleted file mode 100644 index 9a14fe2..0000000 --- a/src/puma_motor_driver/include/puma_motor_driver/can_proto.hpp +++ /dev/null @@ -1,399 +0,0 @@ -//***************************************************************************** -// -// can_proto.h - Definitions for the CAN protocol used to communicate with the -// BDC motor controller. -// -// Copyright (c) 2008-2013 Texas Instruments Incorporated. All rights reserved. -// Software License Agreement -// -// Texas Instruments (TI) is supplying this software for use solely and -// exclusively on TI's microcontroller products. The software is owned by -// TI and/or its suppliers, and is protected under applicable copyright -// laws. You may not combine this software with "viral" open-source -// software in order to form a larger program. -// -// THIS SOFTWARE IS PROVIDED "AS IS" AND WITH ALL FAULTS. -// NO WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT -// NOT LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -// A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. TI SHALL NOT, UNDER ANY -// CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR CONSEQUENTIAL -// DAMAGES, FOR ANY REASON WHATSOEVER. -// -// This is part of revision 10636 of the RDK-BDC24 Firmware Package. -// -//***************************************************************************** - -#ifndef __CAN_PROTO_H__ -#define __CAN_PROTO_H__ - -//***************************************************************************** -// -// The masks of the fields that are used in the message identifier. -// -//***************************************************************************** -#define CAN_MSGID_FULL_M 0x1fffffff -#define CAN_MSGID_DEVNO_M 0x0000003f -#define CAN_MSGID_API_M 0x0000ffc0 -#define CAN_MSGID_MFR_M 0x00ff0000 -#define CAN_MSGID_DTYPE_M 0x1f000000 -#define CAN_MSGID_DEVNO_S 0 -#define CAN_MSGID_API_S 6 -#define CAN_MSGID_MFR_S 16 -#define CAN_MSGID_DTYPE_S 24 - -//***************************************************************************** -// -// The Reserved device number values in the Message Id. -// -//***************************************************************************** -#define CAN_MSGID_DEVNO_BCAST 0x00000000 - -//***************************************************************************** -// -// The Reserved system control API numbers in the Message Id. -// -//***************************************************************************** -#define CAN_MSGID_API_SYSHALT 0x00000000 -#define CAN_MSGID_API_SYSRST 0x00000040 -#define CAN_MSGID_API_DEVASSIGN 0x00000080 -#define CAN_MSGID_API_DEVQUERY 0x000000c0 -#define CAN_MSGID_API_HEARTBEAT 0x00000140 -#define CAN_MSGID_API_SYNC 0x00000180 -#define CAN_MSGID_API_UPDATE 0x000001c0 -#define CAN_MSGID_API_FIRMVER 0x00000200 -#define CAN_MSGID_API_ENUMERATE 0x00000240 -#define CAN_MSGID_API_SYSRESUME 0x00000280 - -//***************************************************************************** -// -// The 32 bit values associated with the CAN_MSGID_API_STATUS request. -// -//***************************************************************************** -#define CAN_STATUS_CODE_M 0x0000ffff -#define CAN_STATUS_MFG_M 0x00ff0000 -#define CAN_STATUS_DTYPE_M 0x1f000000 -#define CAN_STATUS_CODE_S 0 -#define CAN_STATUS_MFG_S 16 -#define CAN_STATUS_DTYPE_S 24 - -//***************************************************************************** -// -// The Reserved manufacturer identifiers in the Message Id. -// -//***************************************************************************** -#define CAN_MSGID_MFR_NI 0x00010000 -#define CAN_MSGID_MFR_LM 0x00020000 -#define CAN_MSGID_MFR_DEKA 0x00030000 - -//***************************************************************************** -// -// The Reserved device type identifiers in the Message Id. -// -//***************************************************************************** -#define CAN_MSGID_DTYPE_BCAST 0x00000000 -#define CAN_MSGID_DTYPE_ROBOT 0x01000000 -#define CAN_MSGID_DTYPE_MOTOR 0x02000000 -#define CAN_MSGID_DTYPE_RELAY 0x03000000 -#define CAN_MSGID_DTYPE_GYRO 0x04000000 -#define CAN_MSGID_DTYPE_ACCEL 0x05000000 -#define CAN_MSGID_DTYPE_USONIC 0x06000000 -#define CAN_MSGID_DTYPE_GEART 0x07000000 -#define CAN_MSGID_DTYPE_UPDATE 0x1f000000 - -//***************************************************************************** -// -// LM Motor Control API Classes API Class and ID masks. -// -//***************************************************************************** -#define CAN_MSGID_API_CLASS_M 0x0000fc00 -#define CAN_MSGID_API_ID_M 0x000003c0 - -//***************************************************************************** -// -// LM Motor Control API Classes in the Message Id for non-broadcast. -// These are the upper 6 bits of the API field, the lower 4 bits determine -// the APIId. -// -//***************************************************************************** -#define CAN_API_MC_VOLTAGE 0x00000000 -#define CAN_API_MC_SPD 0x00000400 -#define CAN_API_MC_VCOMP 0x00000800 -#define CAN_API_MC_POS 0x00000c00 -#define CAN_API_MC_ICTRL 0x00001000 -#define CAN_API_MC_STATUS 0x00001400 -#define CAN_API_MC_PSTAT 0x00001800 -#define CAN_API_MC_CFG 0x00001c00 -#define CAN_API_MC_ACK 0x00002000 - -//***************************************************************************** -// -// The Stellaris Motor Class Control Voltage API definitions. -// -//***************************************************************************** -#define LM_API_VOLT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VOLTAGE) -#define LM_API_VOLT_EN (LM_API_VOLT | (0 << CAN_MSGID_API_S)) -#define LM_API_VOLT_DIS (LM_API_VOLT | (1 << CAN_MSGID_API_S)) -#define LM_API_VOLT_SET (LM_API_VOLT | (2 << CAN_MSGID_API_S)) -#define LM_API_VOLT_SET_RAMP (LM_API_VOLT | (3 << CAN_MSGID_API_S)) -#define LM_API_VOLT_SET_NO_ACK (LM_API_VOLT | (8 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The Stellaris Motor Class Control API definitions for LM_API_VOLT_SET_RAMP. -// -//***************************************************************************** -#define LM_API_VOLT_RAMP_DIS 0 - -//***************************************************************************** -// -// The Stellaris Motor Class Control API definitions for CAN_MSGID_API_SYNC. -// -//***************************************************************************** -#define LM_API_SYNC_PEND_NOW 0 - -//***************************************************************************** -// -// The Stellaris Motor Class Speed Control API definitions. -// -//***************************************************************************** -#define LM_API_SPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_SPD) -#define LM_API_SPD_EN (LM_API_SPD | (0 << CAN_MSGID_API_S)) -#define LM_API_SPD_DIS (LM_API_SPD | (1 << CAN_MSGID_API_S)) -#define LM_API_SPD_SET (LM_API_SPD | (2 << CAN_MSGID_API_S)) -#define LM_API_SPD_PC (LM_API_SPD | (3 << CAN_MSGID_API_S)) -#define LM_API_SPD_IC (LM_API_SPD | (4 << CAN_MSGID_API_S)) -#define LM_API_SPD_DC (LM_API_SPD | (5 << CAN_MSGID_API_S)) -#define LM_API_SPD_REF (LM_API_SPD | (6 << CAN_MSGID_API_S)) -#define LM_API_SPD_SET_NO_ACK (LM_API_SPD | (11 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The Stellaris Motor Control Voltage Compensation Control API definitions. -// -//***************************************************************************** -#define LM_API_VCOMP (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VCOMP) -#define LM_API_VCOMP_EN (LM_API_VCOMP | (0 << CAN_MSGID_API_S)) -#define LM_API_VCOMP_DIS (LM_API_VCOMP | (1 << CAN_MSGID_API_S)) -#define LM_API_VCOMP_SET (LM_API_VCOMP | (2 << CAN_MSGID_API_S)) -#define LM_API_VCOMP_IN_RAMP (LM_API_VCOMP | (3 << CAN_MSGID_API_S)) -#define LM_API_VCOMP_COMP_RAMP (LM_API_VCOMP | (4 << CAN_MSGID_API_S)) -#define LM_API_VCOMP_SET_NO_ACK (LM_API_VCOMP | (9 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The Stellaris Motor Class Position Control API definitions. -// -//***************************************************************************** -#define LM_API_POS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_POS) -#define LM_API_POS_EN (LM_API_POS | (0 << CAN_MSGID_API_S)) -#define LM_API_POS_DIS (LM_API_POS | (1 << CAN_MSGID_API_S)) -#define LM_API_POS_SET (LM_API_POS | (2 << CAN_MSGID_API_S)) -#define LM_API_POS_PC (LM_API_POS | (3 << CAN_MSGID_API_S)) -#define LM_API_POS_IC (LM_API_POS | (4 << CAN_MSGID_API_S)) -#define LM_API_POS_DC (LM_API_POS | (5 << CAN_MSGID_API_S)) -#define LM_API_POS_REF (LM_API_POS | (6 << CAN_MSGID_API_S)) -#define LM_API_POS_SET_NO_ACK (LM_API_POS | (11 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The Stellaris Motor Class Current Control API definitions. -// -//***************************************************************************** -#define LM_API_ICTRL (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ICTRL) -#define LM_API_ICTRL_EN (LM_API_ICTRL | (0 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_DIS (LM_API_ICTRL | (1 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_SET (LM_API_ICTRL | (2 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_PC (LM_API_ICTRL | (3 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_IC (LM_API_ICTRL | (4 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_DC (LM_API_ICTRL | (5 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_SET_NO_ACK (LM_API_ICTRL | (10 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The Stellaris Motor Class Firmware Update API definitions. -// -//***************************************************************************** -#define LM_API_UPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_UPDATE) -#define LM_API_UPD_PING (LM_API_UPD | (0 << CAN_MSGID_API_S)) -#define LM_API_UPD_DOWNLOAD (LM_API_UPD | (1 << CAN_MSGID_API_S)) -#define LM_API_UPD_SEND_DATA (LM_API_UPD | (2 << CAN_MSGID_API_S)) -#define LM_API_UPD_RESET (LM_API_UPD | (3 << CAN_MSGID_API_S)) -#define LM_API_UPD_ACK (LM_API_UPD | (4 << CAN_MSGID_API_S)) -#define LM_API_HWVER (LM_API_UPD | (5 << CAN_MSGID_API_S)) -#define LM_API_UPD_REQUEST (LM_API_UPD | (6 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The Stellaris Motor Class Status API definitions. -// -//***************************************************************************** -#define LM_API_STATUS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_STATUS) -#define LM_API_STATUS_VOLTOUT (LM_API_STATUS | (0 << CAN_MSGID_API_S)) -#define LM_API_STATUS_VOLTBUS (LM_API_STATUS | (1 << CAN_MSGID_API_S)) -#define LM_API_STATUS_CURRENT (LM_API_STATUS | (2 << CAN_MSGID_API_S)) -#define LM_API_STATUS_TEMP (LM_API_STATUS | (3 << CAN_MSGID_API_S)) -#define LM_API_STATUS_POS (LM_API_STATUS | (4 << CAN_MSGID_API_S)) -#define LM_API_STATUS_SPD (LM_API_STATUS | (5 << CAN_MSGID_API_S)) -#define LM_API_STATUS_LIMIT (LM_API_STATUS | (6 << CAN_MSGID_API_S)) -#define LM_API_STATUS_FAULT (LM_API_STATUS | (7 << CAN_MSGID_API_S)) -#define LM_API_STATUS_POWER (LM_API_STATUS | (8 << CAN_MSGID_API_S)) -#define LM_API_STATUS_CMODE (LM_API_STATUS | (9 << CAN_MSGID_API_S)) -#define LM_API_STATUS_VOUT (LM_API_STATUS | (10 << CAN_MSGID_API_S)) - -#define CPR_API_STATUS_ANALOG (LM_API_STATUS | (15 << CAN_MSGID_API_S)) -#define LM_API_STATUS_STKY_FLT (LM_API_STATUS | (11 << CAN_MSGID_API_S)) -#define LM_API_STATUS_FLT_COUNT (LM_API_STATUS | (12 << CAN_MSGID_API_S)) - - -//***************************************************************************** -// -// These definitions are used with the byte that is returned from -// the status request for LM_API_STATUS_LIMIT. -// -//***************************************************************************** -#define LM_STATUS_LIMIT_FWD 0x01 -#define LM_STATUS_LIMIT_REV 0x02 - -//***************************************************************************** -// -// LM Motor Control status codes returned due to the CAN_STATUS_CODE_M field. -// -//***************************************************************************** -#define LM_STATUS_FAULT_ILIMIT 0x01 -#define LM_STATUS_FAULT_TLIMIT 0x02 -#define LM_STATUS_FAULT_VLIMIT 0x04 - -//***************************************************************************** -// -// The Stellaris Motor Class Configuration API definitions. -// -//***************************************************************************** -#define LM_API_CFG (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_CFG) -#define LM_API_CFG_NUM_BRUSHES (LM_API_CFG | (0 << CAN_MSGID_API_S)) -#define LM_API_CFG_ENC_LINES (LM_API_CFG | (1 << CAN_MSGID_API_S)) -#define LM_API_CFG_POT_TURNS (LM_API_CFG | (2 << CAN_MSGID_API_S)) -#define LM_API_CFG_BRAKE_COAST (LM_API_CFG | (3 << CAN_MSGID_API_S)) -#define LM_API_CFG_LIMIT_MODE (LM_API_CFG | (4 << CAN_MSGID_API_S)) -#define LM_API_CFG_LIMIT_FWD (LM_API_CFG | (5 << CAN_MSGID_API_S)) -#define LM_API_CFG_LIMIT_REV (LM_API_CFG | (6 << CAN_MSGID_API_S)) -#define LM_API_CFG_MAX_VOUT (LM_API_CFG | (7 << CAN_MSGID_API_S)) -#define LM_API_CFG_FAULT_TIME (LM_API_CFG | (8 << CAN_MSGID_API_S)) - -#define CPR_API_CFG_SHUTDOWN_TEMP (LM_API_CFG | (11 << CAN_MSGID_API_S)) -#define CPR_API_CFG_MINIMUM_LEVEL (LM_API_CFG | (12 << CAN_MSGID_API_S)) -#define CPR_API_CFG_NOMINAL_LEVEL (LM_API_CFG | (13 << CAN_MSGID_API_S)) -#define CPR_API_CFG_SHUTOFF_LEVEL (LM_API_CFG | (14 << CAN_MSGID_API_S)) -#define CPR_API_CFG_SHUTOFF_TIME (LM_API_CFG | (15 << CAN_MSGID_API_S)) - - -//***************************************************************************** -// -// The Stellaris ACK API definition. -// -//***************************************************************************** -#define LM_API_ACK (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ACK) - -//***************************************************************************** -// -// The 8 bit values that can be returned by a call to LM_API_STATUS_HWVER. -// -//***************************************************************************** -#define LM_HWVER_UNKNOWN 0x00 -#define LM_HWVER_JAG_1_0 0x01 -#define LM_HWVER_JAG_2_0 0x02 - -//***************************************************************************** -// -// The 8 bit values that can be returned by a call to LM_API_STATUS_CMODE. -// -//***************************************************************************** -#define LM_STATUS_CMODE_VOLT 0x00 -#define LM_STATUS_CMODE_CURRENT 0x01 -#define LM_STATUS_CMODE_SPEED 0x02 -#define LM_STATUS_CMODE_POS 0x03 -#define LM_STATUS_CMODE_VCOMP 0x04 - -//***************************************************************************** -// -// The values that can specified as the position or speed reference. Not all -// values are valid for each reference; if an invalid reference is set, then -// none will be selected. -// -//***************************************************************************** -#define LM_REF_ENCODER 0x00 -#define LM_REF_POT 0x01 -#define LM_REF_INV_ENCODER 0x02 -#define LM_REF_QUAD_ENCODER 0x03 -#define LM_REF_NONE 0xff - -//***************************************************************************** -// -// The flags that are used to indicate the currently active fault sources. -// -//***************************************************************************** -#define LM_FAULT_CURRENT 0x01 -#define LM_FAULT_TEMP 0x02 -#define LM_FAULT_VBUS 0x04 -#define LM_FAULT_GATE_DRIVE 0x08 -#define LM_FAULT_COMM 0x10 - -//***************************************************************************** -// -// The Stellaris Motor Class Periodic Status API definitions. -// -//***************************************************************************** -#define LM_API_PSTAT (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_PSTAT) -#define LM_API_PSTAT_PER_EN_S0 (LM_API_PSTAT | (0 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_PER_EN_S1 (LM_API_PSTAT | (1 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_PER_EN_S2 (LM_API_PSTAT | (2 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_PER_EN_S3 (LM_API_PSTAT | (3 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_CFG_S0 (LM_API_PSTAT | (4 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_CFG_S1 (LM_API_PSTAT | (5 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_CFG_S2 (LM_API_PSTAT | (6 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_CFG_S3 (LM_API_PSTAT | (7 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_DATA_S0 (LM_API_PSTAT | (8 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_DATA_S1 (LM_API_PSTAT | (9 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_DATA_S2 (LM_API_PSTAT | (10 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_DATA_S3 (LM_API_PSTAT | (11 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The values that can be used to configure the data the Periodic Status -// Message bytes. Bytes of a multi-byte data values are encoded as -// little-endian, therefore B0 is the least significant byte. -// -//***************************************************************************** -#define LM_PSTAT_END 0 -#define LM_PSTAT_VOLTOUT_B0 1 -#define LM_PSTAT_VOLTOUT_B1 2 -#define LM_PSTAT_VOLTBUS_B0 3 -#define LM_PSTAT_VOLTBUS_B1 4 -#define LM_PSTAT_CURRENT_B0 5 -#define LM_PSTAT_CURRENT_B1 6 -#define LM_PSTAT_TEMP_B0 7 -#define LM_PSTAT_TEMP_B1 8 -#define LM_PSTAT_POS_B0 9 -#define LM_PSTAT_POS_B1 10 -#define LM_PSTAT_POS_B2 11 -#define LM_PSTAT_POS_B3 12 -#define LM_PSTAT_SPD_B0 13 -#define LM_PSTAT_SPD_B1 14 -#define LM_PSTAT_SPD_B2 15 -#define LM_PSTAT_SPD_B3 16 -#define LM_PSTAT_LIMIT_NCLR 17 -#define LM_PSTAT_LIMIT_CLR 18 -#define LM_PSTAT_FAULT 19 -#define LM_PSTAT_STKY_FLT_NCLR 20 -#define LM_PSTAT_STKY_FLT_CLR 21 -#define LM_PSTAT_VOUT_B0 22 -#define LM_PSTAT_VOUT_B1 23 -#define LM_PSTAT_FLT_COUNT_CURRENT 24 -#define LM_PSTAT_FLT_COUNT_TEMP 25 -#define LM_PSTAT_FLT_COUNT_VOLTBUS 26 -#define LM_PSTAT_FLT_COUNT_GATE 27 -#define LM_PSTAT_FLT_COUNT_COMM 28 -#define LM_PSTAT_CANSTS 29 -#define LM_PSTAT_CANERR_B0 30 -#define LM_PSTAT_CANERR_B1 31 - -#endif // __CAN_PROTO_H__ diff --git a/src/puma_motor_driver/include/puma_motor_driver/driver.hpp b/src/puma_motor_driver/include/puma_motor_driver/driver.hpp deleted file mode 100644 index 23055fd..0000000 --- a/src/puma_motor_driver/include/puma_motor_driver/driver.hpp +++ /dev/null @@ -1,539 +0,0 @@ -/** -Software License Agreement (BSD) - -\authors Mike Purvis -\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved. - -Redistribution and use in source and binary forms, with or without modification, are permitted provided that -the following conditions are met: - * Redistributions of source code must retain the above copyright notice, this list of conditions and the - following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the - following disclaimer in the documentation and/or other materials provided with the distribution. - * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote - products derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- -RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- -DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT -OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ -#ifndef PUMA_MOTOR_DRIVER_DRIVER_H -#define PUMA_MOTOR_DRIVER_DRIVER_H - -#include -#include - -#include - -#include "can_hardware/common/types.hpp" -#include "can_hardware/drivers/socketcan_driver.hpp" - -#include "clearpath_motor_msgs/msg/puma_status.hpp" - -#include "diagnostic_updater/update_functions.hpp" - -#include "puma_motor_driver/can_proto.hpp" - -namespace puma_motor_driver -{ - -class Driver -{ -public: - Driver( - const std::shared_ptr interface, - std::shared_ptr nh, - const uint8_t & device_number, - const std::string & device_name); - - void processMessage(const can_hardware::Frame & frame); - - double radPerSecToRpm() const; - - /** - * Sends messages to the motor controller requesting all missing elements to - * populate the cache of status data. Returns true if any messages were sent, - * false if the cache is already complete. - */ - void requestStatusMessages(); - - /** - * Sends messages to the motor controller requesting all missing elements to - * populate the cache of feedback data. Returns true if any messages were sent, - * false if the cache is already complete. - */ - void requestFeedbackMessages(); - /** - * Sends a message to the motor controller requesting the instantaneous duty cycle to - * populate the cache of feedback data. - */ - void requestFeedbackDutyCycle(); - /** - * Sends a message to the motor controller requesting the instantaneous current consumption to - * populate the cache of feedback data. - */ - void requestFeedbackCurrent(); - /** - * Sends a message to the motor controller requesting the instantaneous angular distance to - * populate the cache of feedback data. - */ - void requestFeedbackPosition(); - /** - * Sends a message to the motor controller requesting the instantaneous angular speed to - * populate the cache of feedback data. - */ - void requestFeedbackSpeed(); - /** - * Sends a message to the motor controller requesting the state of the power flag. - */ - void requestFeedbackPowerState(); - /** - * Sends a message to the motor controller requesting the instantaneous set point of the - * current control mode to populate the cache of feedback data. - */ - void requestFeedbackSetpoint(); - /** - * Clear the received flags from the status cache, in preparation for the next - * request batch to go out. - */ - void clearMsgCache(); - /** - * Command the supplied value in open-loop voltage control. - * - * @param[in] cmd Value to command, ranging from -1.0 to 1.0, where zero is neutral. - */ - void commandDutyCycle(const float cmd); - /** - * Command the desired speed set-point in close-loop speed control. - * - * @param[in] cmd Value to command in rad/s. - */ - void commandSpeed(const double cmd); - // void currentSet(float cmd); - // void positionSet(float cmd); - // void neutralSet(); - - /** - * Set the encoders resolution in counts per rev. - * - * @param[in] encoder_cpr Value to set. - */ - void setEncoderCPR(const uint16_t encoder_cpr); - /** - * Set the gear ratio of the motors. - * - * @param[in] gear_ratio Value to set. - */ - void setGearRatio(const float gear_ratio); - /** - * Set the control mode of the motor drivers. - * - * @param[in] mode Value to set. - */ - void setMode(const uint8_t mode); - /** - * Set the control mode of the motor drivers - * with PID gains for close loop control. - * - * @param[in] mode Value to set. - * @param[in] p Value to set. - * @param[in] i Value to set. - * @param[in] d Value to set. - */ - void setMode(const uint8_t mode, const double p, const double i, const double d); - /** - * Set the control mode's PID gains for close loop control. - * - * @param[in] p Value to set. - * @param[in] i Value to set. - * @param[in] d Value to set. - */ - void setGains(const double p, const double i, const double d); - - /** - * Check fault response field was received. - * - * @return received flag - */ - bool receivedFault(); - /** - * Check power field was received. - * - * @return received flag - */ - bool receivedPower(); - /** - * Check mode field was received. - * - * @return received flag - */ - bool receivedMode(); - /** - * Check duty cycle field was received. - * - * @return received flag - */ - bool receivedDutyCycle(); - /** - * Check bus voltage field was received. - * - * @return received flag - */ - bool receivedBusVoltage(); - /** - * Check current field was received. - * - * @return received flag - */ - bool receivedCurrent(); - /** - * Check out voltage field was received. - * - * @return received flag - */ - bool receivedOutVoltage(); - /** - * Check teperature field was received. - * - * @return received flag - */ - bool receivedTemperature(); - /** - * Check analog input field was received. - * - * @return received flag - */ - bool receivedAnalogInput(); - /** - * Check position field was received. - * - * @return received flag - */ - bool receivedPosition(); - /** - * Check speed field was received. - * - * @return received flag - */ - bool receivedSpeed(); - /** - * Check setpoint field was received. - * - * @return received flag - */ - bool receivedSetpoint(); - /** - * Check the set-point response in voltage - * open-loop control was received. - * - * @return received flag - */ - bool receivedDutyCycleSetpoint(); - /** - * Check the set-point response in speed - * closed-loop control was received. - * - * @return received flag - */ - bool receivedSpeedSetpoint(); - /** - * Check the set-point response in currrent - * closed-loop control was received. - * - * @return received flag - */ - bool receivedCurrentSetpoint(); - /** - * Check the set-point response in position - * closed-loop control was received. - * - * @return received flag - */ - bool receivedPositionSetpoint(); - /** - * Process the last received fault response. - * - * @return state of fault status. - */ - uint8_t lastFault(); - /** - * Process the last received power response. - * - * @return state of power status. - */ - uint8_t lastPower(); - /** - * Process the last received mode response. - * - * @return current mode of motor driver. - */ - uint8_t lastMode(); - /** - * Process the last received duty cycle response. - * - * @return value of the instantaneous duty cycle. - */ - float lastDutyCycle(); - /** - * Process the last received bus voltage response. - * - * @return value of the instantaneous bus voltage. - */ - float lastBusVoltage(); - /** - * Process the last received current response. - * - * @return value of the instantaneous current. - */ - float lastCurrent(); - /** - * Process the last received out voltage response. - * - * @return value of the instantaneous out voltage. - */ - float lastOutVoltage(); - /** - * Process the last received temperature response. - * - * @return value of the instantaneous temperature. - */ - float lastTemperature(); - /** - * Process the last received analog_input response. - * - * @return value of the instantaneous analog_input. - */ - float lastAnalogInput(); - /** - * Process the last received travel response. - * - * @return value of the instantaneous angular position. - */ - double lastPosition(); - /** - * Process the last received speed response. - * - * @return value of the instantaneous angular speed. - */ - double lastSpeed(); - /** - * Process the last received set-point response - * for the current control mode. - * - * @return value of the set-point response. - */ - double lastSetpoint(); - - /** - * The requesting part of the state machine that sends a message to the - * motor controller requesting a parameter be set. - */ - void configureParams(); - /** - * The verifying part of the state machine that checks the response of - * the motor controller to ensure the value was set. - */ - void verifyParams(); - /** - * Gets if the driver has been configured. - * - * @return bool if driver is configured. - */ - bool isConfigured() const; - void resetConfiguration(); - /** - * Reset the configured flag to restart the verification process. - */ - void updateGains(); - /** - * Updates the PID gains. - */ - - /** - * Process the last received position encoder reference response - * - * @return value of the reference response. - */ - uint8_t posEncoderRef(); - /** - * Process the last received speed encoder reference response - * - * @return value of the reference response. - */ - uint8_t spdEncoderRef(); - /** - * Process the last received encoder counts response - * - * @return value of the encoder counts. - */ - uint16_t encoderCounts(); - - /** - * Process the last received P gain - * for the current control mode. - * - * @return value of the P gain response. - */ - double getP(); - /** - * Process the last received I gain - * for the current control mode. - * - * @return value of the I gain response. - */ - double getI(); - /** - * Process the last received D gain - * for the current control mode. - * - * @return value of the D gain response. - */ - double getD(); - /** - * Process the last received P gain - * for the current control mode. - * - * @return pointer to raw 4 bytes of the P gain response. - */ - uint8_t * getRawP(); - /** - * Process the last received I gain - * for the current control mode. - * - * @return pointer to raw 4 bytes of the I gain response. - */ - uint8_t * getRawI(); - /** - * Process the last received I gain - * for the current control mode. - * - * @return pointer to raw 4 bytes of the I gain response. - */ - uint8_t * getRawD(); - /** - * Process the last received set-point response - * in voltage open-loop control. - * - * @return value of the set-point response. - */ - float statusDutyCycleGet(); - /** - * Process the last received set-point response - * in speed closed-loop control. - * - * @return value of the set-point response. - */ - double statusSpeedGet(); - /** - * Process the last received set-point response - * in currrent closed-loop control. - * - * @return value of the set-point response. - */ - float statusCurrentGet(); - /** - * Process the last received set-point response - * in position closed-loop control. - * - * @return value of the set-point response. - */ - double statusPositionGet(); - - std::string deviceName() const {return device_name_;} - - uint8_t deviceNumber() const {return device_number_;} - - struct Field - { - static constexpr int FIELD_STRUCT_DATA_SIZE = 4; - uint8_t data[FIELD_STRUCT_DATA_SIZE]; - bool received; - - float interpretFixed8x8() - { - return *(reinterpret_cast(data)) / static_cast(1 << 8); - } - - double interpretFixed16x16() - { - return *(reinterpret_cast(data)) / static_cast(1 << 16); - } - }; - - // Diagnostics - void runFreqStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); - void driverUpdateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat, bool updating); - -private: - std::shared_ptr interface_; - std::shared_ptr nh_; - uint8_t device_number_; - std::string device_name_; - - bool configured_; - uint8_t state_; - double last_power_clear_ts_; - - uint8_t control_mode_; - double gain_p_; - double gain_i_; - double gain_d_; - uint16_t encoder_cpr_; - float gear_ratio_; - - /** - * Helpers to generate data for CAN messages. - */ - void sendId(const uint32_t id); - void sendUint8(const uint32_t id, const uint8_t value); - void sendUint16(const uint32_t id, const uint16_t value); - void sendFixed8x8(const uint32_t id, const float value); - void sendFixed16x16(const uint32_t id, const double value); - can_hardware::Frame getMsg(const uint32_t id); - uint32_t getApi(const can_hardware::Frame & msg); - uint32_t getDeviceNumber(const can_hardware::Frame & msg); - - /** - * Comparing the raw bytes of the 16x16 fixed-point numbers - * to avoid comparing the floating point values. - * - * @return boolean if received is equal to expected. - */ - bool verifyRaw16x16(const uint8_t * received, const double expected); - - /** - * Comparing the raw bytes of the 8x8 fixed-point numbers - * to avoid comparing the floating point values. - * - * @return boolean if received is equal to expected. - */ - bool verifyRaw8x8(const uint8_t * received, const float expected); - - Field voltage_fields_[4]; - Field spd_fields_[7]; - Field vcomp_fields_[5]; - Field pos_fields_[7]; - Field ictrl_fields_[6]; - Field status_fields_[15]; - Field cfg_fields_[15]; - - Field * voltageFieldForMessage(uint32_t api); - Field * spdFieldForMessage(uint32_t api); - Field * vcompFieldForMessage(uint32_t api); - Field * posFieldForMessage(uint32_t api); - Field * ictrlFieldForMessage(uint32_t api); - Field * statusFieldForMessage(uint32_t api); - Field * cfgFieldForMessage(uint32_t api); - - // Frequency Status for diagnostics - std::shared_ptr can_feedback_rate_; // Shared ptr prevents copy errors of FrequencyStatus - std::shared_ptr can_feedback_freq_status_; -}; - -} // namespace puma_motor_driver - -#endif // PUMA_MOTOR_DRIVER_DRIVER_H diff --git a/src/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp b/src/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp deleted file mode 100644 index 6bbd172..0000000 --- a/src/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp +++ /dev/null @@ -1,202 +0,0 @@ -/** -Software License Agreement (BSD) - -\authors Luis Camero -\copyright Copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved. - -Redistribution and use in source and binary forms, with or without modification, are permitted provided that -the following conditions are met: - * Redistributions of source code must retain the above copyright notice, this list of conditions and the - following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the - following disclaimer in the documentation and/or other materials provided with the distribution. - * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote - products derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- -RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- -DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT -OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ -#ifndef PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H -#define PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H - -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/float64.hpp" -#include "sensor_msgs/msg/joint_state.hpp" - -#include "clearpath_motor_msgs/msg/puma_multi_status.hpp" -#include "clearpath_motor_msgs/msg/puma_status.hpp" -#include "clearpath_motor_msgs/msg/puma_multi_feedback.hpp" -#include "clearpath_motor_msgs/msg/puma_feedback.hpp" - -#include "can_hardware/common/types.hpp" -#include "can_hardware/drivers/socketcan_driver.hpp" - -#include "diagnostic_updater/diagnostic_updater.hpp" - -#include "puma_motor_driver/driver.hpp" -// #include "puma_motor_driver/diagnostic_updater.hpp" - -namespace FeedbackBit -{ -enum -{ - DutyCycle, - Current, - Position, - Speed, - Setpoint, - Count, -}; -} - -namespace StatusBit -{ -enum -{ - BusVoltage, - OutVoltage, - AnalogInput, - Temperature, - Mode, - Fault, - Count, -}; -} - -class MultiPumaNode - : public rclcpp::Node -{ -public: - MultiPumaNode(const std::string node_name); - - /** - * Receives desired motor speeds in sensor_msgs::JointState format and - * sends commands to each motor over CAN. - */ - void cmdCallback(const sensor_msgs::msg::JointState::SharedPtr msg); - - /** - * Checks if feedback fields have been received from each motor driver. - * If feedback is avaiable, creates the feedback message and returns - * true. Otherwise, returns false. - */ - bool getFeedback(); - - /** - * Checks if status fields have been received from each motor driver. - * If status data is available, creates the status message and returns - * true. Otherwise, returns false. - */ - bool getStatus(); - - /** - * If feedback message was created, publishes feedback message. - */ - void publishFeedback(); - - /** - * If status message was created, publishes status message. - */ - void publishStatus(); - - /** - * Checks that all motor drivers have been configured and are active. - */ - bool areAllActive(); - - /** - * Checks if socket connection is active. If not, attempts to establish - * a connection. - */ - bool connectIfNotConnected(); - - /** - * @brief Callback function for processing incoming CAN frames. - * - * @param frame The received CAN frame. - */ - void frameCallback(const can_hardware::Frame & frame); - - /** - * Main control loop that checks and maintains the socket gateway, resets - * and reconfigures drivers that have disconnected, verifies parameters - * are set appropriately, receives motor data, and publishes the feedback - * and status messages. - */ - void run(); - -private: - using DiagnosticStatusWrapper = diagnostic_updater::DiagnosticStatusWrapper; - using PumaStatus = clearpath_motor_msgs::msg::PumaStatus; - - // std::shared_ptr interface_; - std::shared_ptr interface_; - std::vector drivers_; - - bool active_; - double gear_ratio_; - int encoder_cpr_; - int freq_; - uint8_t status_count_; - uint8_t desired_mode_; - std::string canbus_dev_; - std::vector joint_names_; - std::vector joint_can_ids_; - std::vector joint_directions_; - - std::queue recv_msg_queue_; - std::mutex recv_msg_mutex_; - - clearpath_motor_msgs::msg::PumaMultiStatus status_msg_; - clearpath_motor_msgs::msg::PumaMultiFeedback feedback_msg_; - - double gain_p_; - double gain_i_; - double gain_d_; - - rclcpp::Node::SharedPtr node_handle_; - rclcpp::Publisher::SharedPtr status_pub_; - rclcpp::Publisher::SharedPtr feedback_pub_; - rclcpp::Subscription::SharedPtr cmd_sub_; - rclcpp::TimerBase::SharedPtr run_timer_; - - // Diagnostic Updater - diagnostic_updater::Updater updater_; - - // Diagnostic labels - const std::map MODE_FLAG_LABELS_ = { - {PumaStatus::MODE_VOLTAGE, "Voltage"}, - {PumaStatus::MODE_CURRENT, "Current"}, - {PumaStatus::MODE_SPEED, "Speed"}, - {PumaStatus::MODE_POSITION, "Position"}, - {PumaStatus::MODE_VCOMP, "V-Comp"}, - }; - const std::map FAULT_FLAG_LABELS_ = { - {PumaStatus::FAULT_CURRENT, "Current Fault"}, - {PumaStatus::FAULT_TEMPERATURE, "Temperature Fault"}, - {PumaStatus::FAULT_BUS_VOLTAGE, "Bus Voltage Fault"}, - {PumaStatus::FAULT_BRIDGE_DRIVER, "Bridge Driver Fault"}, - }; - const std::map PUMA_MOTOR_LABELS_ = { - {"front_left_wheel_joint", "Front Left"}, - {"front_right_wheel_joint", "Front Right"}, - {"rear_left_wheel_joint", "Rear Left"}, - {"rear_right_wheel_joint", "Rear Right"}, - }; - - - // Diagnostic Tasks - void driverDiagnostic(DiagnosticStatusWrapper & stat, int i); - void protectionDiagnostic(DiagnosticStatusWrapper & stat); -}; - -#endif // PUMA_MOTOR_DRIVER_PUMA_NODE_H diff --git a/src/puma_motor_driver/package.xml b/src/puma_motor_driver/package.xml deleted file mode 100644 index 603c70b..0000000 --- a/src/puma_motor_driver/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - puma_motor_driver - 2.9.6 - Motor driver for Puma motor control board that utilizes ROS 2 Socketcan Gateway - Luis Camero - BSD-3-Clause - Mike Purvis - - ament_cmake - - can_hardware - clearpath_motor_msgs - diagnostic_updater - rclcpp - sensor_msgs - std_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/puma_motor_driver/src/driver.cpp b/src/puma_motor_driver/src/driver.cpp deleted file mode 100644 index f39516b..0000000 --- a/src/puma_motor_driver/src/driver.cpp +++ /dev/null @@ -1,1057 +0,0 @@ -/** -Software License Agreement (BSD) - -\authors Mike Purvis -\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved. - -Redistribution and use in source and binary forms, with or without modification, are permitted provided that -the following conditions are met: - * Redistributions of source code must retain the above copyright notice, this list of conditions and the - following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the - following disclaimer in the documentation and/or other materials provided with the distribution. - * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote - products derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- -RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- -DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT -OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ -#include "clearpath_motor_msgs/msg/puma_status.hpp" - -#include "puma_motor_driver/driver.hpp" - -#include -#include -#include -#include "rclcpp/rclcpp.hpp" - -// must match firmware -#define CAN_FEEDBACK_RATE 40.0 - -namespace puma_motor_driver -{ - -namespace ConfigurationStates -{ -enum ConfigurationState -{ - Unknown = -1, - Initializing, - PowerFlag, - EncoderPosRef, - EncoderSpdRef, - EncoderCounts, - ClosedLoop, - ControlMode, - PGain, - IGain, - DGain, - VerifiedParameters, - Configured -}; -} // namespace ConfigurationStates -typedef ConfigurationStates::ConfigurationState ConfigurationState; - -Driver::Driver( - const std::shared_ptr interface, - std::shared_ptr nh, - const uint8_t & device_number, - const std::string & device_name) -: interface_(interface), - nh_(nh), - device_number_(device_number), - device_name_(device_name), - configured_(false), - state_(ConfigurationState::Initializing), - last_power_clear_ts_(0.0), - control_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED), - gain_p_(1), - gain_i_(0), - gain_d_(0), - encoder_cpr_(1), - gear_ratio_(1) -{ - can_feedback_rate_ = std::make_shared(CAN_FEEDBACK_RATE); - can_feedback_freq_status_ = std::make_shared( - diagnostic_updater::FrequencyStatusParam( - can_feedback_rate_.get(), - can_feedback_rate_.get(), - 0.1, - 5 - ) - ); -} - -void Driver::processMessage(const can_hardware::Frame & received_msg) -{ - // If it's not our message, jump out. - if (getDeviceNumber(received_msg) != device_number_) { - return; - } - - // If there's no data then this is a request message, jump out. - if (received_msg.dlc == 0) { - return; - } - - Field * field = nullptr; - uint32_t received_api = getApi(received_msg); - if ((received_api & CAN_MSGID_API_M & CAN_API_MC_CFG) == CAN_API_MC_CFG) { - field = cfgFieldForMessage(received_api); - } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_STATUS) == CAN_API_MC_STATUS) { - field = statusFieldForMessage(received_api); - } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_ICTRL) == CAN_API_MC_ICTRL) { - field = ictrlFieldForMessage(received_api); - can_feedback_freq_status_->tick(); - } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_POS) == CAN_API_MC_POS) { - field = posFieldForMessage(received_api); - can_feedback_freq_status_->tick(); - } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VCOMP) == CAN_API_MC_VCOMP) { - field = vcompFieldForMessage(received_api); - can_feedback_freq_status_->tick(); - } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_SPD) == CAN_API_MC_SPD) { - field = spdFieldForMessage(received_api); - can_feedback_freq_status_->tick(); - } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_VOLTAGE) == CAN_API_MC_VOLTAGE) { - field = voltageFieldForMessage(received_api); - can_feedback_freq_status_->tick(); - } - - if (!field) { - return; - } - - // Copy the received data and mark that field as received. - std::copy_n(std::begin(received_msg.data), Field::FIELD_STRUCT_DATA_SIZE, - std::begin(field->data)); - field->received = true; -} - -double Driver::radPerSecToRpm() const -{ - return (60 * gear_ratio_) / (2 * M_PI); -} - -void Driver::sendId(const uint32_t id) -{ - auto msg = getMsg(id); - interface_->sendFrame(msg); -} - -void Driver::sendUint8(const uint32_t id, const uint8_t value) -{ - auto msg = getMsg(id); - msg.dlc = sizeof(uint8_t); - uint8_t data[8] = {0}; - std::memcpy(data, &value, sizeof(uint8_t)); - std::copy(std::begin(data), std::end(data), std::begin(msg.data)); - - interface_->sendFrame(msg); -} - -void Driver::sendUint16(const uint32_t id, const uint16_t value) -{ - auto msg = getMsg(id); - msg.dlc = sizeof(uint16_t); - uint8_t data[8] = {0}; - std::memcpy(data, &value, sizeof(uint16_t)); - std::copy(std::begin(data), std::end(data), std::begin(msg.data)); - - interface_->sendFrame(msg); -} - -void Driver::sendFixed8x8(const uint32_t id, const float value) -{ - auto msg = getMsg(id); - msg.dlc = sizeof(int16_t); - int16_t output_value = static_cast(static_cast(1 << 8) * value); - - uint8_t data[8] = {0}; - std::memcpy(data, &output_value, sizeof(int16_t)); - std::copy(std::begin(data), std::end(data), std::begin(msg.data)); - - interface_->sendFrame(msg); -} - -void Driver::sendFixed16x16(const uint32_t id, const double value) -{ - auto msg = getMsg(id); - msg.dlc = sizeof(int32_t); - int32_t output_value = static_cast(static_cast((1 << 16) * value)); - - uint8_t data[8] = {0}; - std::memcpy(data, &output_value, sizeof(int32_t)); - std::copy(std::begin(data), std::end(data), std::begin(msg.data)); - - interface_->sendFrame(msg); -} - -can_hardware::Frame Driver::getMsg(const uint32_t id) -{ - can_hardware::Frame msg; - msg.id = id; - msg.dlc = 0; - msg.is_extended = true; - return msg; -} - -uint32_t Driver::getApi(const can_hardware::Frame & msg) -{ - return msg.id & (CAN_MSGID_FULL_M ^ CAN_MSGID_DEVNO_M); -} - -uint32_t Driver::getDeviceNumber(const can_hardware::Frame & msg) -{ - return msg.id & CAN_MSGID_DEVNO_M; -} - -bool Driver::verifyRaw16x16(const uint8_t * received, const double expected) -{ - uint8_t data[4]; - int32_t output_value = static_cast(static_cast((1 << 16) * expected)); - std::memcpy(data, &output_value, 4); - for (uint8_t i = 0; i < 4; i++) { - if (*received != data[i]) { - return false; - } - received++; - } - return true; -} - -bool Driver::verifyRaw8x8(const uint8_t * received, const float expected) -{ - uint8_t data[2]; - int32_t output_value = static_cast(static_cast((1 << 8) * expected)); - std::memcpy(data, &output_value, 2); - for (uint8_t i = 0; i < 2; i++) { - if (*received != data[i]) { - return false; - } - received++; - } - return true; -} - -void Driver::setEncoderCPR(const uint16_t encoder_cpr) -{ - encoder_cpr_ = encoder_cpr; -} - -void Driver::setGearRatio(const float gear_ratio) -{ - gear_ratio_ = gear_ratio; -} - -void Driver::commandDutyCycle(const float cmd) -{ - sendFixed8x8((LM_API_VOLT_SET | device_number_), cmd); -} - -void Driver::commandSpeed(const double cmd) -{ - // Converting from rad/s to RPM through the gearbox. - sendFixed16x16((LM_API_SPD_SET | device_number_), (cmd * radPerSecToRpm())); -} - -void Driver::verifyParams() -{ - switch (state_) { - case ConfigurationState::Initializing: - // Don't advance state until we receive LM_API_STATUS_POWER msg - if (receivedPower()) { - state_ = ConfigurationState::PowerFlag; - } else { - sendId(LM_API_STATUS_POWER | device_number_); - } - break; - case ConfigurationState::PowerFlag: - if (receivedPower() && (lastPower() == 0)) { - // Only advance state when we have a new LM_API_STATUS_POWER msg - // with the power flag cleared - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): cleared power flag.", - device_name_.c_str(), device_number_); - state_ = ConfigurationState::EncoderPosRef; - } else { - sendId(LM_API_STATUS_POWER | device_number_); - } - break; - case ConfigurationState::EncoderPosRef: - if (posEncoderRef() == LM_REF_ENCODER) { - state_ = ConfigurationState::EncoderSpdRef; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): set position encoder reference.", - device_name_.c_str(), device_number_); - } else { - sendId(LM_API_POS_REF | device_number_); - } - break; - case ConfigurationState::EncoderSpdRef: - if (spdEncoderRef() == LM_REF_QUAD_ENCODER) { - state_ = ConfigurationState::EncoderCounts; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): set speed encoder reference.", - device_name_.c_str(), device_number_); - } else { - sendId(LM_API_SPD_REF | device_number_); - } - break; - case ConfigurationState::EncoderCounts: - if (encoderCounts() == encoder_cpr_) { - state_ = ConfigurationState::ClosedLoop; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): set encoder counts to %i.", - device_name_.c_str(), device_number_, encoder_cpr_); - } else { - sendId(LM_API_CFG_ENC_LINES | device_number_); - } - break; - case ConfigurationState::ClosedLoop: // Need to enter a close loop mode to record encoder data. - if (lastMode() == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { - state_ = ConfigurationState::ControlMode; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): entered a close-loop control mode.", - device_name_.c_str(), device_number_); - } else { - sendId(LM_API_STATUS_CMODE | device_number_); - } - break; - case ConfigurationState::ControlMode: - if (lastMode() == control_mode_) { - if (control_mode_ != clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { - state_ = ConfigurationState::PGain; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): was set to a close loop control mode.", - device_name_.c_str(), device_number_); - } else { - state_ = ConfigurationState::VerifiedParameters; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): was set to voltage control mode.", - device_name_.c_str(), device_number_); - } - } - break; - case ConfigurationState::PGain: - if (verifyRaw16x16(getRawP(), gain_p_)) { - state_ = ConfigurationState::IGain; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getP(), gain_p_); - } else { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): P gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getP(), gain_p_); - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - sendId(LM_API_ICTRL_PC | device_number_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - sendId(LM_API_POS_PC | device_number_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - sendId(LM_API_SPD_PC | device_number_); - break; - } - } - break; - case ConfigurationState::IGain: - if (verifyRaw16x16(getRawI(), gain_i_)) { - state_ = ConfigurationState::DGain; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getI(), gain_i_); - } else { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): I gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getI(), gain_i_); - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - sendId(LM_API_ICTRL_IC | device_number_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - sendId(LM_API_POS_IC | device_number_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - sendId(LM_API_SPD_IC | device_number_); - break; - } - } - break; - case ConfigurationState::DGain: - if (verifyRaw16x16(getRawD(), gain_d_)) { - state_ = ConfigurationState::VerifiedParameters; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getD(), gain_d_); - } else { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): D gain constant was set to %f and %f was requested.", - device_name_.c_str(), device_number_, getD(), gain_d_); - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - sendId(LM_API_ICTRL_DC | device_number_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - sendId(LM_API_POS_DC | device_number_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - sendId(LM_API_SPD_DC | device_number_); - break; - } - } - break; - } - if (state_ == ConfigurationState::VerifiedParameters) { - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): all parameters verified.", - device_name_.c_str(), device_number_); - configured_ = true; - state_ = ConfigurationState::Configured; - } -} - -void Driver::configureParams() -{ - const double now = nh_->get_clock()->now().seconds(); - switch (state_) { - case ConfigurationState::Initializing: - break; - case ConfigurationState::PowerFlag: - // Continue to check last power flag until it has been cleared - if (lastPower() == 1) { - // Send request every second - if ((now - last_power_clear_ts_) > 1.0) { - sendUint8((LM_API_STATUS_POWER | device_number_), 1); - last_power_clear_ts_ = now; - } - } - break; - case ConfigurationState::EncoderPosRef: - sendUint8((LM_API_POS_REF | device_number_), LM_REF_ENCODER); - break; - case ConfigurationState::EncoderSpdRef: - sendUint8((LM_API_SPD_REF | device_number_), LM_REF_QUAD_ENCODER); - break; - case ConfigurationState::EncoderCounts: - // Set encoder CPR - sendUint16((LM_API_CFG_ENC_LINES | device_number_), encoder_cpr_); - break; - case ConfigurationState::ClosedLoop: // Need to enter a close loop mode to record encoder data. - sendId(LM_API_SPD_EN | device_number_); - break; - case ConfigurationState::ControlMode: - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: - sendId(LM_API_VOLT_EN | device_number_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - sendId(LM_API_ICTRL_EN | device_number_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - sendId(LM_API_POS_EN | device_number_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - sendId(LM_API_SPD_EN | device_number_); - break; - } - break; - case ConfigurationState::PGain: - // Set P - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - sendFixed16x16((LM_API_ICTRL_PC | device_number_), gain_p_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - sendFixed16x16((LM_API_POS_PC | device_number_), gain_p_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - sendFixed16x16((LM_API_SPD_PC | device_number_), gain_p_); - break; - } - break; - case ConfigurationState::IGain: - // Set I - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - sendFixed16x16((LM_API_ICTRL_IC | device_number_), gain_i_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - sendFixed16x16((LM_API_POS_IC | device_number_), gain_i_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - sendFixed16x16((LM_API_SPD_IC | device_number_), gain_i_); - break; - } - break; - case ConfigurationState::DGain: - // Set D - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - sendFixed16x16((LM_API_ICTRL_DC | device_number_), gain_d_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - sendFixed16x16((LM_API_POS_DC | device_number_), gain_d_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - sendFixed16x16((LM_API_SPD_DC | device_number_), gain_d_); - break; - } - break; - } -} - -bool Driver::isConfigured() const -{ - return configured_; -} - -void Driver::setGains(const double p, const double i, const double d) -{ - gain_p_ = p; - gain_i_ = i; - gain_d_ = d; - - if (configured_) { - updateGains(); - } -} - -void Driver::setMode(const uint8_t mode) -{ - if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { - control_mode_ = mode; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): mode set to voltage control.", - device_name_.c_str(), device_number_); - if (configured_) { - resetConfiguration(); - } - } else { - RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): Close loop modes need PID gains.", - device_name_.c_str(), device_number_); - } -} - -void Driver::setMode(const uint8_t mode, const double p, const double i, const double d) -{ - if (mode == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { - control_mode_ = mode; - RCLCPP_WARN(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): mode set to voltage control but PID gains are not needed.", - device_name_.c_str(), device_number_); - if (configured_) { - resetConfiguration(); - } - } else { - control_mode_ = mode; - if (configured_) { - resetConfiguration(); - } - setGains(p, i, d); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), - "Puma Motor Controller on %s (%i): mode set to a closed-loop control with PID gains of P:%f, I:%f and D:%f.", - device_name_.c_str(), device_number_, gain_p_, gain_i_, gain_d_); - } -} - -void Driver::clearMsgCache() -{ - // Set it all to zero, which will in part clear - // the boolean flags to be false. - memset(voltage_fields_, 0, sizeof(voltage_fields_)); - memset(spd_fields_, 0, sizeof(spd_fields_)); - memset(vcomp_fields_, 0, sizeof(vcomp_fields_)); - memset(pos_fields_, 0, sizeof(pos_fields_)); - memset(ictrl_fields_, 0, sizeof(ictrl_fields_)); - memset(status_fields_, 0, sizeof(status_fields_)); - memset(cfg_fields_, 0, sizeof(cfg_fields_)); -} - -void Driver::requestStatusMessages() -{ - sendId(LM_API_STATUS_POWER | device_number_); -} - -void Driver::requestFeedbackMessages() -{ - sendId(LM_API_STATUS_VOLTOUT | device_number_); - sendId(LM_API_STATUS_CURRENT | device_number_); - sendId(LM_API_STATUS_POS | device_number_); - sendId(LM_API_STATUS_SPD | device_number_); - sendId(LM_API_SPD_SET | device_number_); -} -void Driver::requestFeedbackDutyCycle() -{ - sendId(LM_API_STATUS_VOLTOUT | device_number_); -} - -void Driver::requestFeedbackCurrent() -{ - sendId(LM_API_STATUS_CURRENT | device_number_); -} - -void Driver::requestFeedbackPosition() -{ - sendId(LM_API_STATUS_POS | device_number_); -} - -void Driver::requestFeedbackSpeed() -{ - sendId(LM_API_STATUS_SPD | device_number_); -} - -void Driver::requestFeedbackPowerState() -{ - sendId(LM_API_STATUS_POWER | device_number_); -} - -void Driver::requestFeedbackSetpoint() -{ - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - sendId(LM_API_ICTRL_SET | device_number_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - sendId(LM_API_POS_SET | device_number_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - sendId(LM_API_SPD_SET | device_number_); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: - sendId(LM_API_VOLT_SET | device_number_); - break; - } -} - -void Driver::resetConfiguration() -{ - configured_ = false; - state_ = ConfigurationState::Initializing; -} - -void Driver::updateGains() -{ - configured_ = false; - state_ = ConfigurationState::PGain; -} - -bool Driver::receivedDutyCycle() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); - return field->received; -} - -bool Driver::receivedBusVoltage() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); - return field->received; -} - -bool Driver::receivedCurrent() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); - return field->received; -} - -bool Driver::receivedPosition() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); - return field->received; -} - -bool Driver::receivedSpeed() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); - return field->received; -} - -bool Driver::receivedFault() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); - return field->received; -} - -bool Driver::receivedPower() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); - return field->received; -} - -bool Driver::receivedMode() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); - return field->received; -} - -bool Driver::receivedOutVoltage() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); - return field->received; -} - -bool Driver::receivedTemperature() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); - return field->received; -} - -bool Driver::receivedAnalogInput() -{ - Field * field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); - return field->received; -} - -bool Driver::receivedSetpoint() -{ - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - return receivedCurrentSetpoint(); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - return receivedPositionSetpoint(); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - return receivedSpeedSetpoint(); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: - return receivedDutyCycleSetpoint(); - break; - default: - return 0; - break; - } -} - -bool Driver::receivedSpeedSetpoint() -{ - Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); - return field->received; -} - -bool Driver::receivedDutyCycleSetpoint() -{ - Field * field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); - return field->received; -} - -bool Driver::receivedCurrentSetpoint() -{ - Field * field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); - return field->received; -} - -bool Driver::receivedPositionSetpoint() -{ - Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); - return field->received; -} - -float Driver::lastDutyCycle() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTOUT))); - field->received = false; - return field->interpretFixed8x8() / 128.0; -} - -float Driver::lastBusVoltage() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOLTBUS))); - field->received = false; - return field->interpretFixed8x8(); -} - -float Driver::lastCurrent() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CURRENT))); - field->received = false; - return field->interpretFixed8x8(); -} - -double Driver::lastPosition() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POS))); - field->received = false; - return field->interpretFixed16x16() * ((2 * M_PI) / gear_ratio_); // Convert rev to rad -} - -double Driver::lastSpeed() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_SPD))); - field->received = false; - return field->interpretFixed16x16() * ((2 * M_PI) / (gear_ratio_ * 60)); // Convert RPM to rad/s -} - -uint8_t Driver::lastFault() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_FAULT))); - field->received = false; - return field->data[0]; -} - -uint8_t Driver::lastPower() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_POWER))); - field->received = false; - return field->data[0]; -} - -uint8_t Driver::lastMode() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_CMODE))); - field->received = false; - return field->data[0]; -} - -float Driver::lastOutVoltage() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_VOUT))); - field->received = false; - return field->interpretFixed8x8(); -} - -float Driver::lastTemperature() -{ - Field * field = statusFieldForMessage(getApi(getMsg(LM_API_STATUS_TEMP))); - field->received = false; - return field->interpretFixed8x8(); -} - -float Driver::lastAnalogInput() -{ - Field * field = statusFieldForMessage(getApi(getMsg(CPR_API_STATUS_ANALOG))); - field->received = false; - return field->interpretFixed8x8(); -} - -double Driver::lastSetpoint() -{ - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - return statusCurrentGet(); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - return statusPositionGet(); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - return statusSpeedGet(); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE: - return statusDutyCycleGet(); - break; - default: - return 0; - break; - } -} -double Driver::statusSpeedGet() -{ - Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_SET))); - field->received = false; - return field->interpretFixed16x16() * ((2 * M_PI) / (gear_ratio_ * 60)); // Convert RPM to rad/s -} - -float Driver::statusDutyCycleGet() -{ - Field * field = voltageFieldForMessage(getApi(getMsg(LM_API_VOLT_SET))); - field->received = false; - return field->interpretFixed8x8() / 128.0; -} - -float Driver::statusCurrentGet() -{ - Field * field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_SET))); - field->received = false; - return field->interpretFixed8x8(); -} - -double Driver::statusPositionGet() -{ - Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_SET))); - field->received = false; - return field->interpretFixed16x16() * (( 2 * M_PI) / gear_ratio_); // Convert rev to rad -} - -uint8_t Driver::posEncoderRef() -{ - Field * field = posFieldForMessage(getApi(getMsg(LM_API_POS_REF))); - return field->data[0]; -} - -uint8_t Driver::spdEncoderRef() -{ - Field * field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_REF))); - return field->data[0]; -} - -uint16_t Driver::encoderCounts() -{ - Field * field = cfgFieldForMessage(getApi(getMsg(LM_API_CFG_ENC_LINES))); - return static_cast(field->data[0]) | static_cast(field->data[1] << 8); -} - -double Driver::getP() -{ - Field * field = nullptr; - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_PC))); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - field = posFieldForMessage(getApi(getMsg(LM_API_POS_PC))); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_PC))); - break; - } - return field->interpretFixed16x16(); -} - -double Driver::getI() -{ - Field * field = nullptr; - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_IC))); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - field = posFieldForMessage(getApi(getMsg(LM_API_POS_IC))); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_IC))); - break; - } - return field->interpretFixed16x16(); -} - -double Driver::getD() -{ - Field * field = nullptr; - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_DC))); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - field = posFieldForMessage(getApi(getMsg(LM_API_POS_DC))); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_DC))); - break; - } - return field->interpretFixed16x16(); -} - -uint8_t * Driver::getRawP() -{ - Field * field = nullptr; - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_PC))); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - field = posFieldForMessage(getApi(getMsg(LM_API_POS_PC))); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_PC))); - break; - } - return field->data; -} - -uint8_t * Driver::getRawI() -{ - Field * field = nullptr; - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_IC))); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - field = posFieldForMessage(getApi(getMsg(LM_API_POS_IC))); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_IC))); - break; - } - return field->data; -} - -uint8_t * Driver::getRawD() -{ - Field * field = nullptr; - switch (control_mode_) { - case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT: - field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_DC))); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_POSITION: - field = posFieldForMessage(getApi(getMsg(LM_API_POS_DC))); - break; - case clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED: - field = spdFieldForMessage(getApi(getMsg(LM_API_SPD_DC))); - break; - } - return field->data; -} - -Driver::Field * Driver::voltageFieldForMessage(uint32_t api) -{ - uint32_t voltage_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; - return &voltage_fields_[voltage_field_index]; -} - -Driver::Field * Driver::spdFieldForMessage(uint32_t api) -{ - uint32_t spd_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; - return &spd_fields_[spd_field_index]; -} - -Driver::Field * Driver::vcompFieldForMessage(uint32_t api) -{ - uint32_t vcomp_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; - return &vcomp_fields_[vcomp_field_index]; -} - -Driver::Field * Driver::posFieldForMessage(uint32_t api) -{ - uint32_t pos_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; - return &pos_fields_[pos_field_index]; -} - -Driver::Field * Driver::ictrlFieldForMessage(uint32_t api) -{ - uint32_t ictrl_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; - return &ictrl_fields_[ictrl_field_index]; -} - -Driver::Field * Driver::statusFieldForMessage(uint32_t api) -{ - uint32_t status_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; - return &status_fields_[status_field_index]; -} - -Driver::Field * Driver::cfgFieldForMessage(uint32_t api) -{ - uint32_t cfg_field_index = (api & CAN_MSGID_API_ID_M) >> CAN_MSGID_API_S; - return &cfg_fields_[cfg_field_index]; -} - -/** - * @brief Runs the frequency diagnostic update to populate the status message - */ -void Driver::runFreqStatus(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - can_feedback_freq_status_->run(stat); - - stat.add("Duty cycle", lastDutyCycle()); - stat.add("Current (A)", lastCurrent()); - stat.add("Speed (rad/s)", lastSpeed()); - stat.add("Position", lastPosition()); - stat.add("Setpoint", lastSetpoint()); -} - -} // namespace puma_motor_driver diff --git a/src/puma_motor_driver/src/multi_puma_node.cpp b/src/puma_motor_driver/src/multi_puma_node.cpp deleted file mode 100644 index 33e96e8..0000000 --- a/src/puma_motor_driver/src/multi_puma_node.cpp +++ /dev/null @@ -1,368 +0,0 @@ -/** -Software License Agreement (BSD) - -\authors Luis Camero -\copyright Copyright (c) 2024, Clearpath Robotics, Inc., All rights reserved. - -Redistribution and use in source and binary forms, with or without modification, are permitted provided that -the following conditions are met: - * Redistributions of source code must retain the above copyright notice, this list of conditions and the - following disclaimer. - * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the - following disclaimer in the documentation and/or other materials provided with the distribution. - * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote - products derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR- -RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN- -DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT -OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ -#include -#include -#include - -#include "puma_motor_driver/multi_puma_node.hpp" - -MultiPumaNode::MultiPumaNode(const std::string node_name) -:Node(node_name), - active_(false), - status_count_(0), - desired_mode_(clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED), - updater_(this) -{ - // Parameters - this->declare_parameter("canbus_dev", "vcan0"); - this->declare_parameter("encoder_cpr", 1024); - this->declare_parameter("frequency", 20); - this->declare_parameter("gain.p", 0.1); - this->declare_parameter("gain.i", 0.01); - this->declare_parameter("gain.d", 0.0); - this->declare_parameter("gear_ratio", 24.0); - this->declare_parameter("joint_can_ids", std::vector()); - this->declare_parameter("joint_directions", std::vector()); - this->declare_parameter("joint_names", std::vector()); - - this->get_parameter("canbus_dev", canbus_dev_); - this->get_parameter("encoder_cpr", encoder_cpr_); - this->get_parameter("frequency", freq_); - this->get_parameter("gain.p", gain_p_); - this->get_parameter("gain.i", gain_i_); - this->get_parameter("gain.d", gain_d_); - this->get_parameter("gear_ratio", gear_ratio_); - joint_can_ids_ = this->get_parameter("joint_can_ids").as_integer_array(); - joint_directions_ = this->get_parameter("joint_directions").as_integer_array(); - joint_names_ = this->get_parameter("joint_names").as_string_array(); - - RCLCPP_INFO( - this->get_logger(), - "Gear Ratio %f\nEncoder CPR %d\nFrequency %d\nGain PID %f,%f,%f\nCANBus Device %s", - gear_ratio_, - encoder_cpr_, - freq_, - gain_p_, - gain_i_, - gain_d_, - canbus_dev_.c_str() - ); - - // Validate Parameters - if (joint_names_.size() != joint_can_ids_.size() || - joint_names_.size() != joint_directions_.size()) - { - RCLCPP_ERROR( - this->get_logger(), - "Length of joint_name list must match length of joint_can_ids list and joint_directions list."); - return; - } - - // Subsciber - cmd_sub_ = this->create_subscription( - "platform/motors/cmd", - rclcpp::SensorDataQoS(), - std::bind(&MultiPumaNode::cmdCallback, this, std::placeholders::_1)); - - // Publishers - feedback_pub_ = this->create_publisher( - "platform/motors/feedback", - rclcpp::SensorDataQoS()); - status_pub_ = this->create_publisher( - "platform/motors/status", - rclcpp::SensorDataQoS()); - - node_handle_ = std::shared_ptr(this, [](rclcpp::Node *){}); - - // SocketCAN Interface - interface_ = std::make_shared(canbus_dev_); - interface_->registerFrameCallback( - std::bind(&MultiPumaNode::frameCallback, this, std::placeholders::_1)); - - for (uint8_t i = 0; i < joint_names_.size(); i++) { - drivers_.push_back(puma_motor_driver::Driver( - interface_, - node_handle_, - joint_can_ids_[i], - joint_names_[i] - )); - } - - feedback_msg_.drivers_feedback.resize(drivers_.size()); - status_msg_.drivers.resize(drivers_.size()); - - uint8_t i = 0; - for (auto & driver : drivers_) { - driver.clearMsgCache(); - driver.setEncoderCPR(encoder_cpr_); - driver.setGearRatio(gear_ratio_ * joint_directions_[i]); - driver.setMode(desired_mode_, gain_p_, gain_i_, gain_d_); - i++; - } - - run_timer_ = this->create_wall_timer( - std::chrono::milliseconds(1000 / freq_), std::bind(&MultiPumaNode::run, this)); - - // Setup diagnostics - updater_.setHardwareID("Puma"); - for (uint8_t i = 0; i < joint_names_.size(); i++) { - std::string name = "Puma Motor Driver " + std::to_string(i + 1) + " (" + joint_names_[i] + ")"; - updater_.add(name, std::bind(&MultiPumaNode::driverDiagnostic, this, std::placeholders::_1, i)); - } -} - -bool MultiPumaNode::getFeedback() -{ - // Check All Fields Received - uint8_t received = 0; - for (auto & driver : drivers_) { - received |= driver.receivedDutyCycle() << FeedbackBit::DutyCycle; - received |= driver.receivedCurrent() << FeedbackBit::Current; - received |= driver.receivedPosition() << FeedbackBit::Position; - received |= driver.receivedSpeed() << FeedbackBit::Speed; - received |= driver.receivedSetpoint() << FeedbackBit::Setpoint; - } - - if (received != (1 << FeedbackBit::Count) - 1) { - return false; - } - - uint8_t feedback_index = 0; - for (auto & driver : drivers_) { - clearpath_motor_msgs::msg::PumaFeedback * f = &feedback_msg_.drivers_feedback[feedback_index]; - f->device_number = driver.deviceNumber(); - f->device_name = driver.deviceName(); - f->duty_cycle = driver.lastDutyCycle(); - f->current = driver.lastCurrent(); - f->travel = driver.lastPosition(); - f->speed = driver.lastSpeed(); - f->setpoint = driver.lastSetpoint(); - - feedback_index++; - } - feedback_msg_.header.stamp = this->get_clock()->now(); - return true; -} - -bool MultiPumaNode::getStatus() -{ - // Check All Fields Received - uint8_t status_index = 0; - uint8_t received_fields = 0; - uint8_t received_status = 0; - for (auto & driver : drivers_) { - received_fields |= driver.receivedBusVoltage() << StatusBit::BusVoltage; - received_fields |= driver.receivedOutVoltage() << StatusBit::OutVoltage; - received_fields |= driver.receivedAnalogInput() << StatusBit::AnalogInput; - received_fields |= 1 << StatusBit::AnalogInput; - received_fields |= driver.receivedTemperature() << StatusBit::Temperature; - received_fields |= driver.receivedMode() << StatusBit::Mode; - received_fields |= driver.receivedFault() << StatusBit::Fault; - if (received_fields != (1 << StatusBit::Count) - 1) { - RCLCPP_DEBUG(this->get_logger(), "Received Status Fields %x", received_fields); - } else { - received_status |= 1 << status_index; - } - status_index++; - } - - if (received_status != (1 << status_index) - 1) { - RCLCPP_DEBUG(this->get_logger(), "Received Status %x", received_status); - return false; - } - - // Prepare output status message to ROS. - status_index = 0; - for (auto & driver : drivers_) { - clearpath_motor_msgs::msg::PumaStatus * s = &status_msg_.drivers[status_index]; - s->device_number = driver.deviceNumber(); - s->device_name = driver.deviceName(); - s->bus_voltage = driver.lastBusVoltage(); - s->output_voltage = driver.lastOutVoltage(); - s->analog_input = driver.lastAnalogInput(); - s->temperature = driver.lastTemperature(); - s->mode = driver.lastMode(); - s->fault = driver.lastFault(); - - status_index++; - } - status_msg_.header.stamp = this->get_clock()->now(); - return true; -} - -void MultiPumaNode::publishFeedback() -{ - if (getFeedback()) { - feedback_pub_->publish(feedback_msg_); - } -} - -void MultiPumaNode::publishStatus() -{ - if (getStatus()) { - status_pub_->publish(status_msg_); - } -} - -/** - * @brief Diagnostic task to report details for each motor driver - * - * @param i Driver index number - */ -void MultiPumaNode::driverDiagnostic(DiagnosticStatusWrapper & stat, int i) -{ - // Assume we're OK. This will be merged over later on if we aren't - stat.summary(DiagnosticStatusWrapper::OK, "OK"); - - drivers_[i].runFreqStatus(stat); - - // basic stats - stat.add("CAN ID", (int)status_msg_.drivers[i].device_number); - stat.add("Joint Name", status_msg_.drivers[i].device_name); - stat.add("Bus Voltage (V)", status_msg_.drivers[i].bus_voltage); - stat.add("Motor Temperature (C)", status_msg_.drivers[i].temperature); - stat.add("Output Voltage (V)", status_msg_.drivers[i].output_voltage); - stat.add("Analogue Input (V)", status_msg_.drivers[i].analog_input); - - // control mode; convert to a string - stat.add("Mode", MODE_FLAG_LABELS_.at((int)status_msg_.drivers[i].mode)); - - // fault flags; these are a bit field - for (auto label : FAULT_FLAG_LABELS_) { - bool flag = (status_msg_.drivers[i].fault >> label.first) & 0x01; - stat.add(label.second, flag ? "True" : "False"); - if (flag) { - // raise a warning if there's a fault - stat.mergeSummary(DiagnosticStatusWrapper::WARN, label.second); - } - } -} - -void MultiPumaNode::cmdCallback(const sensor_msgs::msg::JointState::SharedPtr msg) -{ - if (active_) { - for (auto & driver : drivers_) { - for (int i = 0; i < static_cast(msg->name.size()); i++) { - if (driver.deviceName() == msg->name[i]) { - if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_VOLTAGE) { - driver.commandDutyCycle(msg->velocity[i]); - } else if (desired_mode_ == clearpath_motor_msgs::msg::PumaStatus::MODE_SPEED) { - driver.commandSpeed(std::isnan(msg->velocity[i]) ? 0.0 : msg->velocity[i]); - } - } - } - } - } -} - -bool MultiPumaNode::areAllActive() -{ - for (auto & driver : drivers_) { - if (!driver.isConfigured()) { - return false; - } - } - return true; -} - -void MultiPumaNode::frameCallback(const can_hardware::Frame & frame) -{ - std::lock_guard lock(recv_msg_mutex_); - recv_msg_queue_.push(frame); -} - -void MultiPumaNode::run() -{ - if (active_) { - // Checks to see if power flag has been reset for each driver - for (auto & driver : drivers_) { - if (driver.lastPower() != 0) { - active_ = false; - RCLCPP_WARN(this->get_logger(), - "Power reset detected on device ID %d, will reconfigure all drivers.", - driver.deviceNumber()); - for (auto & driver : drivers_) { - driver.resetConfiguration(); - } - } - } - // Queue data requests for the drivers in order to assemble an amalgamated status message. - for (auto & driver : drivers_) { - driver.requestStatusMessages(); - driver.requestFeedbackSetpoint(); - } - } else { - // Set parameters for each driver. - for (auto & driver : drivers_) { - driver.configureParams(); - } - } - - while (!recv_msg_queue_.empty()) { - can_hardware::Frame frame; - { - std::lock_guard lock(recv_msg_mutex_); - frame = std::move(recv_msg_queue_.front()); - recv_msg_queue_.pop(); - } - for (auto & driver : drivers_) { - driver.processMessage(frame); - } - } - - // Check parameters of each driver instance. - if (!active_) { - for (auto & driver : drivers_) { - driver.verifyParams(); - } - } - - // Verify that the all drivers are configured. - if (areAllActive() == true && active_ == false) { - active_ = true; - RCLCPP_INFO(this->get_logger(), "All controllers active."); - } - // Broadcast feedback and status messages - if (active_) { - publishFeedback(); - publishStatus(); - } - status_count_++; -} - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - rclcpp::executors::MultiThreadedExecutor exe; - - std::shared_ptr multi_puma_node = - std::make_shared("multi_puma_node"); - - exe.add_node(multi_puma_node); - exe.spin(); - - rclcpp::shutdown(); - return 0; -} From 236c6a2136d2c1a7926aece41dd8b0c61382a629 Mon Sep 17 00:00:00 2001 From: Nathan Dunkelberger Date: Tue, 8 Sep 2026 11:02:56 -0500 Subject: [PATCH 9/9] bumping pb for hw changes --- src/phoebe_bridgeback | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/phoebe_bridgeback b/src/phoebe_bridgeback index 1a40f8a..9d4ba58 160000 --- a/src/phoebe_bridgeback +++ b/src/phoebe_bridgeback @@ -1 +1 @@ -Subproject commit 1a40f8a0f695bda3ef43fa8da6c6ea8edf94e347 +Subproject commit 9d4ba58287b31b706a1afe861da1b8cacbf1eba8