diff --git a/src/robot/control/src/control_core.cpp b/src/robot/control/src/control_core.cpp
index 6f5f7a5..87c8dac 100644
--- a/src/robot/control/src/control_core.cpp
+++ b/src/robot/control/src/control_core.cpp
@@ -138,7 +138,9 @@ geometry_msgs::msg::Twist ControlCore::calculateControlCommand(
   // Or just constant linear velocity as per prompt "make sure the robot follows the path"
 
   // Just use the configured linear velocity
-  twist.linear.x = linear_velocity_;
+  double speed_factor =
+    1.0 - (std::abs(output) / max_steering_angle_ * 0.5);  // during turns slows down to avoid overshoot
+  twist.linear.x = linear_velocity_ * speed_factor;
   twist.angular.z = output;
 
   return twist;
diff --git a/src/robot/control/src/control_node.cpp b/src/robot/control/src/control_node.cpp
index 1bed921..b8402e8 100644
--- a/src/robot/control/src/control_node.cpp
+++ b/src/robot/control/src/control_node.cpp
@@ -48,8 +48,8 @@ void ControlNode::processParameters()
   this->declare_parameter<std::string>("cmd_vel_topic", "/cmd_vel");
   this->declare_parameter<int>("control_period_ms", 100);
   this->declare_parameter<double>("kp", 1.0);
-  this->declare_parameter<double>("ki", 0.0);
-  this->declare_parameter<double>("kd", 0.0);
+  this->declare_parameter<double>("ki", 0.6);
+  this->declare_parameter<double>("kd", 0.4);
   this->declare_parameter<double>("max_steering_angle", 1.5);
   this->declare_parameter<double>("linear_velocity", 1.5);
 
@@ -87,6 +87,9 @@ void ControlNode::followPath()
 {
   if (control_.isPathEmpty()) {
     RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 3000, "Path is empty. Waiting for new path.");
+    // Publish stop command to halt the robot
+    geometry_msgs::msg::Twist stop_cmd;
+    cmd_vel_publisher_->publish(stop_cmd);
     return;
   }