diff --git a/include/robotem_rovne/Node.h b/include/robotem_rovne/Node.h
index e6dcf76..94c03f9 100644
--- a/include/robotem_rovne/Node.h
+++ b/include/robotem_rovne/Node.h
@@ -83,7 +83,8 @@ class Node : public rclcpp::Node
 
   enum class State { Stopped, Starting, Driving, Stopping };
   State _robot_state;
-  quantity<m/s> _motor_left_vel, _motor_right_vel;
+  quantity<m/s> _motor_base_vel;
+  void control_yaw();
   void handle_Stopped();
   void handle_Starting();
   void handle_Driving();
diff --git a/src/Node.cpp b/src/Node.cpp
index c6c49d3..aa7d25f 100644
--- a/src/Node.cpp
+++ b/src/Node.cpp
@@ -30,8 +30,7 @@ Node::Node()
 , _motor_left_qos_profile{rclcpp::KeepLast(1), rmw_qos_profile_sensor_data}
 , _motor_right_qos_profile{rclcpp::KeepLast(1), rmw_qos_profile_sensor_data}
 , _robot_state{State::Stopped}
-, _motor_left_vel{0. * m/s}
-, _motor_right_vel{0. * m/s}
+, _motor_base_vel{0. * m/s}
 {
   init_req_start_service_server();
   init_req_stop_service_server();
@@ -203,85 +202,86 @@ void Node::ctrl_loop()
   }
 }
 
+void Node::control_yaw()
+{
+  auto const yaw_err = (_yaw_target - _yaw_actual);
+
+  double const k = 0.01;
+  double const pid_res = k * yaw_err.numerical_value_in(deg);
+
+  auto const motor_vel_lower_limit = _motor_base_vel - 0.2 * m/s;
+  auto const motor_vel_upper_limit = _motor_base_vel + 0.2 * m/s;
+
+  auto motor_left_vel  = _motor_base_vel - pid_res * m/s;
+  motor_left_vel = std::max(motor_left_vel, motor_vel_lower_limit);
+  motor_left_vel = std::min(motor_left_vel, motor_vel_upper_limit);
+
+  auto motor_right_vel = _motor_base_vel + pid_res * m/s;
+  motor_right_vel = std::max(motor_right_vel, motor_vel_lower_limit);
+  motor_right_vel = std::min(motor_right_vel, motor_vel_upper_limit);
+
+  RCLCPP_INFO(get_logger(),
+              "actual = %0.2f, target = %0.2f, error = %0.2f, pid_res = %0.2f, LEFT = %0.2f m/s, RIGHT = %0.2f m/s",
+              _yaw_actual.numerical_value_in(deg),
+              _yaw_target.numerical_value_in(deg),
+              yaw_err.numerical_value_in(deg),
+              pid_res,
+              motor_left_vel.numerical_value_in(m/s),
+              motor_right_vel.numerical_value_in(m/s));
+
+  pub_motor_left (motor_left_vel);
+  pub_motor_right(motor_right_vel);
+}
+
 void Node::handle_Stopped()
 {
-  RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "handle_Stopped");
+  RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "State::Stopped");
 
-  _motor_left_vel  = 0. * m/s;
-  _motor_right_vel = 0. * m/s;
+  _motor_base_vel = 0. * m/s;
 
-  pub_motor_left (_motor_left_vel);
-  pub_motor_right(_motor_right_vel);
+  pub_motor_left (0. * m/s);
+  pub_motor_right(0. * m/s);
 }
 
 void Node::handle_Starting()
 {
-  RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "handle_Starting");
+  RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "State::Starting");
 
-  if (_motor_left_vel < 0.5 * m/s)
-    _motor_left_vel += 0.01 * m/s;
-  if (_motor_right_vel < 0.5 * m/s)
-    _motor_right_vel += 0.01 * m/s;
+  control_yaw();
 
-  if (_motor_right_vel >= 0.5 * m/s && _motor_right_vel >= 0.5 * m/s)
+  if (_motor_base_vel < 0.5 * m/s)
+  {
+    _motor_base_vel += 0.01 * m/s;
+  }
+  else
   {
-    _motor_left_vel  = 0.5 * m/s;
-    _motor_right_vel = 0.5 * m/s;
+    _motor_base_vel  = 0.5 * m/s;
     _robot_state = State::Driving;
   }
-
-  pub_motor_left (_motor_left_vel);
-  pub_motor_right(_motor_right_vel);
 }
 
 void Node::handle_Driving()
 {
-  RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "handle_Driving");
-
-  auto const yaw_err = (_yaw_target - _yaw_actual);
-
-  double const k = 0.01;
-  double const pid_res = k * yaw_err.numerical_value_in(deg);
-
-  _motor_left_vel  = 0.5 * m/s - pid_res * m/s;
-  _motor_left_vel = std::max(_motor_left_vel, 0.3 * m/s);
-  _motor_left_vel = std::min(_motor_left_vel, 0.7 * m/s);
+  RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "State::Driving");
 
-  _motor_right_vel = 0.5 * m/s + pid_res * m/s;
-  _motor_right_vel = std::max(_motor_right_vel, 0.3 * m/s);
-  _motor_right_vel = std::min(_motor_right_vel, 0.7 * m/s);
-
-  RCLCPP_INFO(get_logger(),
-              "actual = %0.2f, target = %0.2f, error = %0.2f, pid_res = %0.2f, LEFT = %0.2f m/s, RIGHT = %0.2f m/s",
-              _yaw_actual.numerical_value_in(deg),
-              _yaw_target.numerical_value_in(deg),
-              yaw_err.numerical_value_in(deg),
-              pid_res,
-              _motor_left_vel.numerical_value_in(m/s),
-              _motor_right_vel.numerical_value_in(m/s));
-
-  pub_motor_left (_motor_left_vel);
-  pub_motor_right(_motor_right_vel);
+  control_yaw();
 }
 
 void Node::handle_Stopping()
 {
-  RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "handle_Stopping");
+  RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000UL, "State::Stopping");
 
-  if (_motor_left_vel > 0. * m/s)
-    _motor_left_vel -= 0.025 * m/s;
-  if (_motor_right_vel > 0. * m/s)
-    _motor_right_vel -= 0.025 * m/s;
+  control_yaw();
 
-  if (_motor_right_vel <= 0. * m/s && _motor_right_vel <= 0. * m/s)
+  if (_motor_base_vel > 0. * m/s)
+  {
+    _motor_base_vel -= 0.025 * m/s;
+  }
+  else
   {
-    _motor_left_vel  = 0. * m/s;
-    _motor_right_vel = 0. * m/s;
+    _motor_base_vel  = 0. * m/s;
     _robot_state = State::Stopped;
   }
-
-  pub_motor_left (_motor_left_vel);
-  pub_motor_right(_motor_right_vel);
 }
 
 /**************************************************************************************