diff --git a/src/modelec/src/pcb_odo_interface.cpp b/src/modelec/src/pcb_odo_interface.cpp
index 2c5f5d8..8a63c15 100644
--- a/src/modelec/src/pcb_odo_interface.cpp
+++ b/src/modelec/src/pcb_odo_interface.cpp
@@ -39,7 +39,9 @@ namespace Modelec
 
                     pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
                         res->publisher, 10,
-                        std::bind(&PCBOdoInterface::PCBCallback, this, std::placeholders::_1),
+                        [this](const std_msgs::msg::String::SharedPtr msg) {
+                            PCBCallback(msg);
+                        },
                         options);
 
                     pcb_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
@@ -83,44 +85,81 @@ namespace Modelec
 
         odo_add_waypoint_subscriber_ = this->create_subscription<modelec_interface::msg::OdometryAddWaypoint>(
             "odometry/add_waypoint", 10,
-            std::bind(&PCBOdoInterface::AddWaypointCallback, this, std::placeholders::_1));
+            [this](const modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg)
+            {
+                AddWaypointCallback(msg);
+            });
 
         odo_set_pos_subscriber_ = this->create_subscription<modelec_interface::msg::OdometryPos>(
             "odometry/set_position", 10,
-            std::bind(&PCBOdoInterface::SetPosCallback, this, std::placeholders::_1));
+            [this](const modelec_interface::msg::OdometryPos::SharedPtr msg)
+            {
+                SetPosCallback(msg);
+            });
 
         odo_set_pid_subscriber_ = this->create_subscription<modelec_interface::msg::OdometryPid>(
             "odometry/set_pid", 10,
-            std::bind(&PCBOdoInterface::SetPIDCallback, this, std::placeholders::_1));
+            [this](const modelec_interface::msg::OdometryPid::SharedPtr msg)
+            {
+                SetPIDCallback(msg);
+            });
 
         // Services
         get_tof_service_ = create_service<modelec_interface::srv::OdometryToF>(
             "odometry/tof",
-            std::bind(&PCBOdoInterface::HandleGetTof, this, std::placeholders::_1, std::placeholders::_2));
+            [this](const std::shared_ptr<modelec_interface::srv::OdometryToF::Request> request,
+                std::shared_ptr<modelec_interface::srv::OdometryToF::Response> response)
+            {
+                HandleGetTof(request, response);
+            });
 
         get_speed_service_ = create_service<modelec_interface::srv::OdometrySpeed>(
             "odometry/speed",
-            std::bind(&PCBOdoInterface::HandleGetSpeed, this, std::placeholders::_1, std::placeholders::_2));
+            [this](const std::shared_ptr<modelec_interface::srv::OdometrySpeed::Request> request,
+                std::shared_ptr<modelec_interface::srv::OdometrySpeed::Response> response)
+            {
+                HandleGetSpeed(request, response);
+            });
 
         get_position_service_ = create_service<modelec_interface::srv::OdometryPosition>(
             "odometry/get_position",
-            std::bind(&PCBOdoInterface::HandleGetPosition, this, std::placeholders::_1, std::placeholders::_2));
+            [this](const std::shared_ptr<modelec_interface::srv::OdometryPosition::Request> request,
+                std::shared_ptr<modelec_interface::srv::OdometryPosition::Response> response)
+            {
+                HandleGetPosition(request, response);
+            });
 
         set_start_service_ = create_service<modelec_interface::srv::OdometryStart>(
             "odometry/start",
-            std::bind(&PCBOdoInterface::HandleGetStart, this, std::placeholders::_1, std::placeholders::_2));
+            [this](const std::shared_ptr<modelec_interface::srv::OdometryStart::Request> request,
+                std::shared_ptr<modelec_interface::srv::OdometryStart::Response> response)
+            {
+                HandleGetStart(request, response);
+            });
 
         get_pid_service_ = create_service<modelec_interface::srv::OdometryGetPid>(
             "odometry/get_pid",
-            std::bind(&PCBOdoInterface::HandleGetPID, this, std::placeholders::_1, std::placeholders::_2));
+            [this](const std::shared_ptr<modelec_interface::srv::OdometryGetPid::Request> request,
+                std::shared_ptr<modelec_interface::srv::OdometryGetPid::Response> response)
+            {
+                HandleGetPID(request, response);
+            });
 
         set_pid_service_ = create_service<modelec_interface::srv::OdometrySetPid>(
             "odometry/set_pid",
-            std::bind(&PCBOdoInterface::HandleSetPID, this, std::placeholders::_1, std::placeholders::_2));
+            [this](const std::shared_ptr<modelec_interface::srv::OdometrySetPid::Request> request,
+                std::shared_ptr<modelec_interface::srv::OdometrySetPid::Response> response)
+            {
+                HandleSetPID(request, response);
+            });
 
         add_waypoint_service_ = create_service<modelec_interface::srv::OdometryAddWaypoint>(
             "odometry/add_waypoint",
-            std::bind(&PCBOdoInterface::HandleAddWaypoint, this, std::placeholders::_1, std::placeholders::_2));
+            [this](const std::shared_ptr<modelec_interface::srv::OdometryAddWaypoint::Request> request,
+                std::shared_ptr<modelec_interface::srv::OdometryAddWaypoint::Response> response)
+            {
+                HandleAddWaypoint(request, response);
+            });
     }
 
     PCBOdoInterface::~PCBOdoInterface()
@@ -220,6 +259,12 @@ namespace Modelec
                 bool success = true;
                 ResolveAddWaypointRequest(success);
             }
+
+            else if (tokens[1] == "PID")
+            {
+                bool success = true;
+                ResolveSetPIDRequest(success);
+            }
             else
             {
                 RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg->data.c_str());
diff --git a/src/modelec_interface/CMakeLists.txt b/src/modelec_interface/CMakeLists.txt
index c76da7b..db57708 100644
--- a/src/modelec_interface/CMakeLists.txt
+++ b/src/modelec_interface/CMakeLists.txt
@@ -18,7 +18,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Odometry/OdometryAddWaypoint.msg"
   "msg/Odometry/OdometryStart.msg"
   "msg/Odometry/PID/OdometryPid.msg"
-  "msg/WriteSerial.msg"
   "msg/PCA9685Servo.msg"
   "msg/ServoMode.msg"
   "msg/Solenoid.msg"
diff --git a/src/modelec_interface/msg/Odometry/OdometryAddWaypoint.msg b/src/modelec_interface/msg/Odometry/OdometryAddWaypoint.msg
index e62203f..3febaf0 100644
--- a/src/modelec_interface/msg/Odometry/OdometryAddWaypoint.msg
+++ b/src/modelec_interface/msg/Odometry/OdometryAddWaypoint.msg
@@ -1,4 +1,4 @@
-int8 id
+uint8 id
 bool is_end
 int64 x
 int64 y
diff --git a/src/modelec_interface/msg/Odometry/OdometryToF.msg b/src/modelec_interface/msg/Odometry/OdometryToF.msg
index 3f1cf2b..6974d89 100644
--- a/src/modelec_interface/msg/Odometry/OdometryToF.msg
+++ b/src/modelec_interface/msg/Odometry/OdometryToF.msg
@@ -1,2 +1,2 @@
-int8 n
+uint8 n
 int64 distance
diff --git a/src/modelec_interface/msg/Odometry/OdometryWaypointReach.msg b/src/modelec_interface/msg/Odometry/OdometryWaypointReach.msg
index f5fe691..98d97a8 100644
--- a/src/modelec_interface/msg/Odometry/OdometryWaypointReach.msg
+++ b/src/modelec_interface/msg/Odometry/OdometryWaypointReach.msg
@@ -1 +1 @@
-int8 id
\ No newline at end of file
+uint8 id
\ No newline at end of file
diff --git a/src/modelec_interface/msg/WriteSerial.msg b/src/modelec_interface/msg/WriteSerial.msg
deleted file mode 100644
index 589dd1d..0000000
--- a/src/modelec_interface/msg/WriteSerial.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-string name
-string data
\ No newline at end of file
diff --git a/src/modelec_interface/srv/Odometry/OdometryAddWaypoint.srv b/src/modelec_interface/srv/Odometry/OdometryAddWaypoint.srv
index e24f578..525de05 100644
--- a/src/modelec_interface/srv/Odometry/OdometryAddWaypoint.srv
+++ b/src/modelec_interface/srv/Odometry/OdometryAddWaypoint.srv
@@ -1,4 +1,4 @@
-int8 id
+uint8 id
 bool is_end
 int64 x
 int64 y
diff --git a/src/modelec_interface/srv/Odometry/OdometryToF.srv b/src/modelec_interface/srv/Odometry/OdometryToF.srv
index d00f314..770a044 100644
--- a/src/modelec_interface/srv/Odometry/OdometryToF.srv
+++ b/src/modelec_interface/srv/Odometry/OdometryToF.srv
@@ -1,3 +1,3 @@
-int8 n
+uint8 n
 ---
 int64 distance