diff --git a/launch/step_4.launch.py b/launch/step_4.launch.py
index bee9cc5330bf8a8a731dc426299d0698b596e4a2..607837c5b3c5c8ce63f3ba62a3224a2cc392c0d5 100644
--- a/launch/step_4.launch.py
+++ b/launch/step_4.launch.py
@@ -16,4 +16,10 @@ def generate_launch_description():
             executable='step_4',
             name='square'
         )
+        Node(
+            package='rqt_gui',
+            namespace='',
+            executable='rqt_gui',
+            name='rqt'
+        ),
     ])
diff --git a/lesson_05/step_4.py b/lesson_05/step_4.py
index 0aadceeac49995ffb174c923ca75f496161bf2eb..e4f0655012bc961c86e839052f7fce19ee067ed7 100644
--- a/lesson_05/step_4.py
+++ b/lesson_05/step_4.py
@@ -20,7 +20,7 @@ class RurMover(Node):
         self.SIDE_TIME = 5.0
         self.LINEAR_SPEED = 0.5
         self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback)
-
+        self.kp = 1
     def pose_callback(self, msg):
         self.get_logger().info('Current heading: "%f"' % msg.theta)
         self.get_logger().info('Setpoint: "%f"' % self.desired_heading)
@@ -28,10 +28,10 @@ class RurMover(Node):
         msg_pub.linear.x = self.LINEAR_SPEED
         error = angle_diff(self.desired_heading, msg.theta)
         if error < 0:
-            msg_pub.angular.z = math.radians(90.0)
+            msg_pub.angular.z = (math.radians(90.0)-error)*self.kp
             self.get_logger().info('Turn left')
         else:
-            msg_pub.angular.z = math.radians(-90.0)
+            msg_pub.angular.z = (math.radians(-90.0)-error)*self.kp
             self.get_logger().info('Turn right')
         self.publisher_twist.publish(msg_pub)
 
@@ -51,4 +51,4 @@ def main(args=None):
     rclpy.shutdown()
 
 if __name__ == '__main__':
-    main()
\ No newline at end of file
+    main()