IMU needs to be powered on and then the offset should be looked at. NEeds to be tested in the lab

ros2/src/robobin/robobin/helpers/message_handler.py

@@ -114,10 +114,10 @@ class MessageHandler:
         # D: linear.x = 0, angular.z = -0.5
         
         directions = {
-            "W": (0.5, 0),    # Move forward
-            "A": (0, 0.5),    # Turn left
-            "S": (-0.5, 0),   # Move backward
-            "D": (0, -0.5)    # Turn right
+            "W": (0.25, 0),    # Move forward
+            "A": (0, 0.25),    # Turn left
+            "S": (-0.25, 0),   # Move backward
+            "D": (0, -0.25)    # Turn right
         }
         
         # Get direction data and ensure it's a tuple of floats
@@ -195,10 +195,10 @@ if __name__ == "__main__":
     message_handler = MessageHandler(None, testing=True)
     assert message_handler.handle_message(None, "PING") is None
     assert message_handler.handle_message(None, "TIME") is None
-    assert message_handler.handle_message(None, "MANUALCTRL W") == ("cmd_vel", (0.5, 0))
-    assert message_handler.handle_message(None, "MANUALCTRL A") == ("cmd_vel", (0, 0.5))
-    assert message_handler.handle_message(None, "MANUALCTRL S") == ("cmd_vel", (-0.5, 0))
-    assert message_handler.handle_message(None, "MANUALCTRL D") == ("cmd_vel", (0, -0.5))
+    assert message_handler.handle_message(None, "MANUALCTRL W") == ("cmd_vel", (0.25, 0))
+    assert message_handler.handle_message(None, "MANUALCTRL A") == ("cmd_vel", (0, 0.25))
+    assert message_handler.handle_message(None, "MANUALCTRL S") == ("cmd_vel", (-0.25, 0))
+    assert message_handler.handle_message(None, "MANUALCTRL D") == ("cmd_vel", (0, -0.25))
 
     assert message_handler.handle_message(None, "STOP") == ("cmd_vel", (0.0, 0.0))
     assert message_handler.handle_message(None, "CALLME (212.9638, 283.98108)") == ("nav_command", "(212.9638, 283.98108)")

ros2/src/robobin/robobin/uwb_pathing_node.py

@@ -73,7 +73,7 @@ class UWBPathingNode(Node):
             return
 
         # Calculate yaw error (assuming robot orientation 0 = facing x-axis)
-        # If you don't have actual orientation, you might assume the robot always faces the target directly
+
         yaw_error = angle_to_target
         yaw_error = atan2(sin(yaw_error), cos(yaw_error))  # Normalize angle
         self.get_logger().info(f"Current Position: x={self.current_x:.2f}, y={self.current_y:.2f}")