From b1492b4c5dba03c45b8fa155ee80d1f6c2ec6507 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Fri, 28 Feb 2025 20:48:58 +0100 Subject: [PATCH] Changed the way we communicate with the stepper driver --- mg_control/src/mg_control.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/mg_control/src/mg_control.cpp b/mg_control/src/mg_control.cpp index 26ef4e7..625e96b 100644 --- a/mg_control/src/mg_control.cpp +++ b/mg_control/src/mg_control.cpp @@ -66,14 +66,18 @@ hardware_interface::return_type mg::MgStepperInterface::read(const rclcpp::Time& } hardware_interface::return_type mg::MgStepperInterface::write(const rclcpp::Time&, const rclcpp::Duration&) { - std::string cmd_left; - std::string cmd_right; - - cmd_left = std::to_string(left_wheel_vel_cmd / (2 * M_PI)) + " "; - cmd_right = std::to_string(-right_wheel_vel_cmd / (2 * M_PI)); + union { + std::array bytes; + double data; + } value{}; try { - odrive_serial_interface.Write(cmd_left + cmd_right); + odrive_serial_interface.Write("s;"); + value.data = left_wheel_vel_cmd / (2 * M_PI); + for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); } + value.data = -right_wheel_vel_cmd / (2 * M_PI); + for (const auto& bt : value.bytes) { odrive_serial_interface.WriteByte(bt); } + } catch (const std::runtime_error& e) { return hardware_interface::return_type::ERROR; } return hardware_interface::return_type::OK; } -- 2.49.0