diff --git a/launch/step_1.launch.py b/launch/step_1.launch.py
deleted file mode 100644
index 2908c5e31e5e82e8b15f94ba0f50ad109b5e311f..0000000000000000000000000000000000000000
--- a/launch/step_1.launch.py
+++ /dev/null
@@ -1,26 +0,0 @@
-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_1',
-            name='controller'
-        ),
-        Node(
-            package='lesson_06',
-            namespace='waterstrider',
-            executable='pid_node',
-            name='pid',
-            parameters=[config_file]
-        )
-    ])
diff --git a/lesson_06/pid_node.py b/lesson_06/pid_node.py
index 6c390a5828b2978fbcf490f1b8891926e177a912..f78727f0f2ccc06574802d00e51eccb59da60b7c 100644
--- a/lesson_06/pid_node.py
+++ b/lesson_06/pid_node.py
@@ -4,6 +4,7 @@ import rclpy
 from rclpy.node import Node
 from std_msgs.msg import Float64
 from rcl_interfaces.msg import SetParametersResult
+import math
 
 
 class PID(Node):
@@ -15,11 +16,11 @@ class PID(Node):
 
     def __init__(self):
         super().__init__(f'pid')
-        self.config['kp'] = self.declare_parameter('kp', 0.0).value
+        self.config['kp'] = self.declare_parameter('kp', 3.0).value
         self.config['ki'] = self.declare_parameter('ki', 0.0).value
-        self.config['kd'] = self.declare_parameter('kd', 0.0).value
-        self.config['clamp'] = self.declare_parameter('clamp', 1.0).value
-        self.config['angular'] = self.declare_parameter('angular', False).value
+        self.config['kd'] = self.declare_parameter('kd', 40.0).value
+        self.config['clamp'] = self.declare_parameter('clamp', 15.0).value
+        self.config['angular'] = self.declare_parameter('angular', True).value
         self.config['inverted'] = self.declare_parameter('inverted', False).value
         self.add_on_set_parameters_callback(self.parameter_callback)
         self.get_logger().warn(f"Parameters updated {self.config}")
@@ -53,11 +54,11 @@ class PID(Node):
         self.pub_result.publish(message)
 
     def angular_constraint(self, e):
-        return (e + 180.0) % (2 * 180.0) - 180.0
+        return (e+math.pi)%(2*math.pi)-math.pi
 
     def update(self, state):
         error = self.sp - state if not self.config['inverted'] else state - self.sp
-        if self.config['angular']:
+        if True:
             error = self.angular_constraint(error)
         self.get_logger().warn(f"PID error: {error}")
         p = self.config['kp'] * error
diff --git a/lesson_06/square_pid.py b/lesson_06/square_pid.py
new file mode 100644
index 0000000000000000000000000000000000000000..4add1cb96456b44ecf2f2b42a1fafb60c2e4f51a
--- /dev/null
+++ b/lesson_06/square_pid.py
@@ -0,0 +1,68 @@
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import PoseStamped, Pose
+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..8913220ae654d6612b9ef68fbeb856eb216791a0 100644
--- a/setup.py
+++ b/setup.py
@@ -30,8 +30,8 @@ setup(
     entry_points={
         'console_scripts': [
             'step_0 = lesson_06.step_0:main',
-            'step_1 = lesson_06.step_1:main',
             'pid_node = lesson_06.pid_node:main',
+            'square_pid = lesson_06.square_pid:main',
         ],
     },
 )