Added logger to see what is sent

Ensmasteel · Apr 23, 2024 · bf7c600 · bf7c600
1 parent 9970d3a
commit bf7c600
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 0 deletions.
diff --git a/omnidrive_rppico/hardware/include/omnidrive_rppico/rppico_comms.hpp b/omnidrive_rppico/hardware/include/omnidrive_rppico/rppico_comms.hpp
@@ -5,6 +5,9 @@
 #include <libserial/SerialPort.h>
 #include <iostream>
 
+#include "rclcpp/rclcpp.hpp"
+
+
 
 LibSerial::BaudRate convert_baud_rate(int baud_rate)
 {
@@ -107,6 +110,7 @@ class RpPicoComs
     std::stringstream ss;
     ss << "m " << val_1 << " " << val_2 << " " << val_3 << " " << val_4 << "\r";
     send_msg(ss.str());
+    RCLCPP_INFO(rclcpp::get_logger("rppico_comms"), "Sent: %s", ss.str().c_str());
   }
 
   void set_pid_values(int k_p, int k_d, int k_i, int k_o)

diff --git a/omnidrive_rppico/hardware/omnibot_pico_system.cpp b/omnidrive_rppico/hardware/omnibot_pico_system.cpp
@@ -254,6 +254,8 @@ hardware_interface::return_type omnidrive_rppico ::OmniDriveRpPicoHardware::writ
   int motor_b_counts_per_loop = wheel_b_.cmd / wheel_b_.rads_per_count / cfg_.loop_rate;
   int motor_r_counts_per_loop = wheel_r_.cmd / wheel_r_.rads_per_count / cfg_.loop_rate;
 
+
+
   comms_.set_motor_values(motor_f_counts_per_loop, 
                           motor_l_counts_per_loop,
                           motor_b_counts_per_loop,