From 767d3f91c89d80fd4f07b2f288d28ab9e284ead8 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+Pimpest@users.noreply.github.com>
Date: Wed, 18 Dec 2024 11:35:49 +0100
Subject: [PATCH] Fixed converting rotations to radians

---
 mg_wheel_interface/src/mg_wheel_interface.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/mg_wheel_interface/src/mg_wheel_interface.cpp b/mg_wheel_interface/src/mg_wheel_interface.cpp
index 35b2d4a..8c53b0d 100644
--- a/mg_wheel_interface/src/mg_wheel_interface.cpp
+++ b/mg_wheel_interface/src/mg_wheel_interface.cpp
@@ -101,8 +101,8 @@ hardware_interface::return_type mg_wheel_interface::MgOdriveInterface::read(cons
     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);
-    right_wheel_pos_state = -std::stod(resp_r);
+    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,8 +113,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/M_PI)  + " ";
-    cmd_right = "v 0 " + std::to_string(-right_wheel_vel_cmd/M_PI) + " ";
+    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) {