From d2baab8874eac863994ae0a1024b97228ce46e87 Mon Sep 17 00:00:00 2001 From: borednuna Date: Fri, 7 Apr 2023 13:15:15 +0700 Subject: [PATCH 1/4] feat: add backlash tuner main --- src/backlash_tuner_main.cpp | 126 ++++++++++++++++++++++++++++++++++++ 1 file changed, 126 insertions(+) create mode 100644 src/backlash_tuner_main.cpp diff --git a/src/backlash_tuner_main.cpp b/src/backlash_tuner_main.cpp new file mode 100644 index 0000000..c96e6d2 --- /dev/null +++ b/src/backlash_tuner_main.cpp @@ -0,0 +1,126 @@ +// Copyright (c) 2021 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include +#include +#include +#include + +#include "tachimawari/control/control.hpp" +#include "tachimawari/node/tachimawari_node.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char *argv[]) +{ + bool read_all = false; + std::string target_joint_str; + int target_joint; + double max = -1000, min = 1000; + rclcpp::init(argc, argv); + + if (argc < 2) + { + std::cerr << "Please specify the mode! [sdk / cm740]" << std::endl; + } + + if (argc < 3) + { + read_all = true; + } + else + { + target_joint_str = argv[2]; + target_joint = stoi(target_joint_str); + } + + std::string mode = argv[1]; + std::shared_ptr controller; + + if (mode == "sdk") + { + controller = std::make_shared("/dev/ttyUSB0"); + } + else if (mode == "cm740") + { + controller = std::make_shared("/dev/ttyUSB0"); + } + else + { + std::cerr << "Mode doesn't exist, select the correct mode! [sdk / cm740]" << std::endl; + return 0; + } + + if (!controller->connect()) + { + controller->set_port("/dev/ttyUSB1"); + + if (!controller->connect()) + { + std::cout << "failed to connect controller\n"; + return 1; + } + } + + // init_joints_main.cpp + auto joint_manager = std::make_shared(controller); + std::vector joints; + + if (!joint_manager->set_joints(joints)) + { + std::cout << "failed to sync write\n"; + } + + // spin + auto node = std::make_shared("tachimawari_node"); + tachimawari::TachimawariNode tachimawari_node(node, controller); + joints = joint_manager->get_current_joints(); + + // spin and read only one servo + rclcpp::Rate rcl_rate(8ms); + while (rclcpp::ok()) + { + rcl_rate.sleep(); + rclcpp::spin_some(node); + + // read_joints_main.cpp + if (!read_all) + { + for (const auto &joint : joints) + { + if (target_joint == static_cast(joint.get_id())) + { + min = joint.get_position() > min ? min : joint.get_position(); + max = joint.get_position() < max ? max : joint.get_position(); + std::cout << "id " << static_cast(joint.get_id()) << ": " << joint.get_position() << "\n"; + std::cout << "minimum: " << min << "\n"; + std::cout << "maximum: " << max << "\n"; + } + } + } + else + { + // spin and read all servos + for (const auto &joint : joints) + { + std::cout << "id " << static_cast(joint.get_id()) << ": " << joint.get_position() << "\n"; + } + } + } +} From ca2f4a5941cf334369f81ae0cd41a0b78360ca9c Mon Sep 17 00:00:00 2001 From: JayantiTA Date: Mon, 16 Oct 2023 22:57:12 +0700 Subject: [PATCH 2/4] refactor: optimize code --- src/backlash_tuner_main.cpp | 89 +++++++++++++++---------------------- 1 file changed, 35 insertions(+), 54 deletions(-) diff --git a/src/backlash_tuner_main.cpp b/src/backlash_tuner_main.cpp index c96e6d2..d3d8e5d 100644 --- a/src/backlash_tuner_main.cpp +++ b/src/backlash_tuner_main.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021 Ichiro ITS +// Copyright (c) 2023 Ichiro ITS // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -29,97 +29,78 @@ int main(int argc, char *argv[]) { - bool read_all = false; - std::string target_joint_str; - int target_joint; - double max = -1000, min = 1000; rclcpp::init(argc, argv); - if (argc < 2) - { + if (argc < 2) { std::cerr << "Please specify the mode! [sdk / cm740]" << std::endl; + return 1; } - if (argc < 3) - { + bool read_all = false; + std::string target_joint_str; + uint8_t target_joint; + + if (argc < 3) { read_all = true; - } - else - { + } else { target_joint_str = argv[2]; - target_joint = stoi(target_joint_str); + target_joint = std::stoi(target_joint_str); } std::string mode = argv[1]; std::shared_ptr controller; - if (mode == "sdk") - { + if (mode == "sdk") { controller = std::make_shared("/dev/ttyUSB0"); - } - else if (mode == "cm740") - { + } else if (mode == "cm740") { controller = std::make_shared("/dev/ttyUSB0"); - } - else - { + } else { std::cerr << "Mode doesn't exist, select the correct mode! [sdk / cm740]" << std::endl; - return 0; + return 1; } - if (!controller->connect()) - { + if (!controller->connect()) { controller->set_port("/dev/ttyUSB1"); - if (!controller->connect()) - { - std::cout << "failed to connect controller\n"; + if (!controller->connect()) { + std::cout << "failed to connect controller" << std::endl; return 1; } } - // init_joints_main.cpp auto joint_manager = std::make_shared(controller); std::vector joints; - if (!joint_manager->set_joints(joints)) - { - std::cout << "failed to sync write\n"; + if (!joint_manager->set_joints(joints)) { + std::cerr << "failed to sync write" << std::endl; } - // spin auto node = std::make_shared("tachimawari_node"); tachimawari::TachimawariNode tachimawari_node(node, controller); joints = joint_manager->get_current_joints(); - // spin and read only one servo rclcpp::Rate rcl_rate(8ms); - while (rclcpp::ok()) - { + while (rclcpp::ok()) { rcl_rate.sleep(); rclcpp::spin_some(node); - // read_joints_main.cpp - if (!read_all) - { - for (const auto &joint : joints) - { - if (target_joint == static_cast(joint.get_id())) - { - min = joint.get_position() > min ? min : joint.get_position(); - max = joint.get_position() < max ? max : joint.get_position(); - std::cout << "id " << static_cast(joint.get_id()) << ": " << joint.get_position() << "\n"; - std::cout << "minimum: " << min << "\n"; - std::cout << "maximum: " << max << "\n"; + if (!read_all) { + for (const auto &joint : joints) { + double max = std::numeric_limits::min(); + double min = std::numeric_limits::max(); + + if (target_joint == joint.get_id()) { + min = std::min(joint.get_position(), min); + max = std::max(joint.get_position(), max); + + std::cout << "id " << static_cast(joint.get_id()) << ": " << joint.get_position() << std::endl; + std::cout << "minimum: " << min << std::endl; + std::cout << "maximum: " << max << std::endl; } } - } - else - { - // spin and read all servos - for (const auto &joint : joints) - { - std::cout << "id " << static_cast(joint.get_id()) << ": " << joint.get_position() << "\n"; + } else { + for (const auto &joint : joints) { + std::cout << "id " << static_cast(joint.get_id()) << ": " << joint.get_position() << std::endl; } } } From 7247b5bb6f638b05495f2be7684692c75f7b496d Mon Sep 17 00:00:00 2001 From: JayantiTA Date: Mon, 16 Oct 2023 22:59:11 +0700 Subject: [PATCH 3/4] refactor: fix data type --- src/backlash_tuner_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/backlash_tuner_main.cpp b/src/backlash_tuner_main.cpp index d3d8e5d..cca42e2 100644 --- a/src/backlash_tuner_main.cpp +++ b/src/backlash_tuner_main.cpp @@ -86,8 +86,8 @@ int main(int argc, char *argv[]) if (!read_all) { for (const auto &joint : joints) { - double max = std::numeric_limits::min(); - double min = std::numeric_limits::max(); + double max = std::numeric_limits::min(); + double min = std::numeric_limits::max(); if (target_joint == joint.get_id()) { min = std::min(joint.get_position(), min); From eeff0f4f37b967d42beed830fecf78158025d15e Mon Sep 17 00:00:00 2001 From: JayantiTA Date: Mon, 16 Oct 2023 23:01:17 +0700 Subject: [PATCH 4/4] refactor: fix variable init --- src/backlash_tuner_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/backlash_tuner_main.cpp b/src/backlash_tuner_main.cpp index cca42e2..b1a7eca 100644 --- a/src/backlash_tuner_main.cpp +++ b/src/backlash_tuner_main.cpp @@ -79,6 +79,9 @@ int main(int argc, char *argv[]) tachimawari::TachimawariNode tachimawari_node(node, controller); joints = joint_manager->get_current_joints(); + double max = std::numeric_limits::min(); + double min = std::numeric_limits::max(); + rclcpp::Rate rcl_rate(8ms); while (rclcpp::ok()) { rcl_rate.sleep(); @@ -86,9 +89,6 @@ int main(int argc, char *argv[]) if (!read_all) { for (const auto &joint : joints) { - double max = std::numeric_limits::min(); - double min = std::numeric_limits::max(); - if (target_joint == joint.get_id()) { min = std::min(joint.get_position(), min); max = std::max(joint.get_position(), max);