From 75a47dd8c9aed7165f73c1e073353dc7c9cc6bee Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+Pimpest@users.noreply.github.com> Date: Sat, 25 Jan 2025 02:29:05 +0100 Subject: [PATCH] Switched odrive interface with stepper motor interface --- .../assets/mg_base_controllers.yaml | 2 +- mg_wheel_interface/src/mg_wheel_interface.cpp | 51 ++++--------------- 2 files changed, 12 insertions(+), 41 deletions(-) diff --git a/mg_wheel_interface/assets/mg_base_controllers.yaml b/mg_wheel_interface/assets/mg_base_controllers.yaml index 11a2550..84b4b79 100644 --- a/mg_wheel_interface/assets/mg_base_controllers.yaml +++ b/mg_wheel_interface/assets/mg_base_controllers.yaml @@ -16,7 +16,7 @@ diffdrive_controller: odom_frame_id: odom base_frame_id: base-link - # open_loop: true + open_loop: true wheel_separation: 0.0964 wheel_radius: 0.035 \ No newline at end of file diff --git a/mg_wheel_interface/src/mg_wheel_interface.cpp b/mg_wheel_interface/src/mg_wheel_interface.cpp index 8c53b0d..8dbe94b 100644 --- a/mg_wheel_interface/src/mg_wheel_interface.cpp +++ b/mg_wheel_interface/src/mg_wheel_interface.cpp @@ -43,24 +43,6 @@ CallbackReturn mg_wheel_interface::MgOdriveInterface::on_configure(const rclcpp_ CallbackReturn mg_wheel_interface::MgOdriveInterface::on_shutdown(const rclcpp_lifecycle::State &) { if(odrive_serial_interface.IsOpen()){ - std::string cmd_left; - std::string cmd_right; - int hash; - - cmd_left = "v 1 " + std::to_string(0) + " "; - cmd_right = "v 0 " + std::to_string(0) + " "; - - hash = 0; - for(const auto &c: cmd_left) { - hash ^= c; - } - cmd_left += "*"+std::to_string(hash)+"\n"; - - hash = 0; - for(const auto &c: cmd_right) { - hash ^= c; - } - cmd_right += "*"+std::to_string(hash)+"\n"; odrive_serial_interface.Close(); } return CallbackReturn::SUCCESS; @@ -96,13 +78,15 @@ std::vector mg_wheel_interface::MgOdriveIn hardware_interface::return_type mg_wheel_interface::MgOdriveInterface::read(const rclcpp::Time &time, const rclcpp::Duration &period) { - std::string resp_l, resp_r; - odrive_serial_interface.Write("f 1\n"); - odrive_serial_interface.ReadLine(resp_l,'\n',10); - odrive_serial_interface.Write("f 0\n"); - odrive_serial_interface.ReadLine(resp_r,'\n',10); - left_wheel_pos_state = std::stod(resp_l)*2*M_PI; - right_wheel_pos_state = -std::stod(resp_r)*2*M_PI; + /* + * std::string resp_l, resp_r; + * odrive_serial_interface.Write("f 1\n"); + * odrive_serial_interface.ReadLine(resp_l,'\n',10); + * odrive_serial_interface.Write("f 0\n"); + * odrive_serial_interface.ReadLine(resp_r,'\n',10); + * left_wheel_pos_state = std::stod(resp_l)*2*M_PI; + * right_wheel_pos_state = -std::stod(resp_r)*2*M_PI; + */ return hardware_interface::return_type::OK ; } @@ -113,21 +97,8 @@ hardware_interface::return_type mg_wheel_interface::MgOdriveInterface::write(con std::string cmd_right; int hash; - cmd_left = "v 1 " + std::to_string(left_wheel_vel_cmd/2*M_PI) + " "; - cmd_right = "v 0 " + std::to_string(-right_wheel_vel_cmd/2*M_PI) + " "; - - hash = 0; - for(const auto &c: cmd_left) { - hash ^= c; - } - cmd_left += "*"+std::to_string(hash)+"\n"; - - hash = 0; - for(const auto &c: cmd_right) { - hash ^= c; - } - cmd_right += "*"+std::to_string(hash)+"\n"; - + cmd_left = std::to_string(left_wheel_vel_cmd/2*M_PI) + " "; + cmd_right = std::to_string(-right_wheel_vel_cmd/2*M_PI); try { odrive_serial_interface.Write(cmd_left + cmd_right);