diff --git a/config/control-config.yaml b/config/control-config.yaml
index 80bbe66d87876352396a2d3238f2f8625038b722..39c82b9e5d6657265b4aff4bea7ad17c68a42b29 100644
--- a/config/control-config.yaml
+++ b/config/control-config.yaml
@@ -4,8 +4,8 @@
         angular: true
         clamp: 15.0
         inverted: false
-        kd: 0.9
+        kd: 3.0
         ki: 0.0
-        kp: 0.04
+        kp: 10.0
         start_type_description_service: true
         use_sim_time: false
diff --git a/launch/step_2.launch.py b/launch/step_2.launch.py
new file mode 100644
index 0000000000000000000000000000000000000000..5c0682f7890b782a3c33a5838a6f26e220cc09b7
--- /dev/null
+++ b/launch/step_2.launch.py
@@ -0,0 +1,26 @@
+import os
+from ament_index_python import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    config_file = os.path.join(get_package_share_directory('lesson_06'),
+                                       'config',
+                                       'control-config.yaml')
+    return LaunchDescription([
+        Node(
+            package='lesson_06',
+            namespace='waterstrider',
+            executable='step_2',
+            name='controller'
+        ),
+        Node(
+            package='lesson_06',
+            namespace='waterstrider',
+            executable='pid_node',
+            name='pid',
+            parameters=[config_file]
+        )
+    ])
diff --git a/lesson_06/step_2.py b/lesson_06/step_2.py
new file mode 100644
index 0000000000000000000000000000000000000000..19c9892320158e4c0fdd233d6aa9fb624e360f83
--- /dev/null
+++ b/lesson_06/step_2.py
@@ -0,0 +1,56 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import PoseStamped
+from std_msgs.msg import Float64 
+from tf_transformations import euler_from_quaternion
+import math                                                                    
+
+def get_yaw_from_quaternion(q):
+    return euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
+
+
+class RurMover(Node):
+    def __init__(self):
+        super().__init__('controller')
+        self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10)
+        self.pose_subscriber = self.create_subscription(PoseStamped, '/waterstrider/ground_truth_to_tf_waterstrider/pose', self.pose_callback, 10)
+        self.result_subscription = self.create_subscription(Float64, '/pid/output', self.result_callback, 10)
+        self.pub_setpoint = self.create_publisher(Float64, '/pid/setpoint', 10)
+        self.pub_state = self.create_publisher(Float64, '/pid/state', 10)
+        self.desired_heading = - math.pi / 2
+        self.SIDE_TIME = 10.0
+        self.LINEAR_SPEED = 1.0
+        self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback)
+
+    def pose_callback(self, msg):
+        heading = get_yaw_from_quaternion(msg.pose.orientation)
+        self.get_logger().info('Current heading: "%f"' % heading)
+        self.get_logger().info('Setpoint: "%f"' % self.desired_heading)
+        message = Float64()
+        message.data = heading
+        self.pub_state.publish(message)
+
+    def result_callback(self, msg):
+        msg_pub = Twist()
+        msg_pub.linear.x = self.LINEAR_SPEED
+        msg_pub.angular.z = msg.data
+        self.publisher_twist.publish(msg_pub)
+
+    def pub_timer_callback(self):
+        message = Float64()
+        self.desired_heading += math.pi / 2
+        self.desired_heading %= 2 * math.pi
+        self.get_logger().info('New angle setpoint: "%f"' % self.desired_heading)
+        message.data = self.desired_heading
+        self.pub_setpoint.publish(message)
+
+def main(args=None):
+    rclpy.init(args=args)
+    mover = RurMover()
+    rclpy.spin(mover)
+    mover.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file
diff --git a/setup.py b/setup.py
index 1d4258459be8c7a7075d3e94d7f1b8fbea97ea06..f8aac3b30ae3d32cbce7be0c72d9ebad3f882da6 100644
--- a/setup.py
+++ b/setup.py
@@ -29,8 +29,7 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'step_0 = lesson_06.step_0:main',
-            'step_1 = lesson_06.step_1:main',
+            'step_2 = lesson_06.step_2:main',
             'pid_node = lesson_06.pid_node:main',
         ],
     },