diff --git a/config/control-config.yaml b/config/control-config.yaml
index 80bbe66d87876352396a2d3238f2f8625038b722..b4c8c8666d31e70a66eac1aa31af734140b0bc08 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: 0.3
         ki: 0.0
-        kp: 0.04
+        kp: 0.9
         start_type_description_service: true
         use_sim_time: false
diff --git a/lesson_06/step_1.py b/lesson_06/step_1.py
new file mode 100644
index 0000000000000000000000000000000000000000..9d9234cbe9811d188811698777d1f9a8855d0a77
--- /dev/null
+++ b/lesson_06/step_1.py
@@ -0,0 +1,95 @@
+from math import degrees
+
+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
+import queue
+
+def angle_diff(a1, a2):
+    a = a1-a2
+    return (a+math.pi)%(2*math.pi)-math.pi
+
+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.curr_heading = self.create_publisher(Float64, '/waterstrider/pid/state', 10)
+        self.heading_setpoint = self.create_publisher(Float64, '/waterstrider/pid/setpoint', 10)
+        self.pid_output = self.create_subscription(Float64, '/waterstrider/pid/output', self.pid_callback, 10)
+
+        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.heading = None
+        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)
+        self.prev_pose = None
+        self.curr_pose = None
+        self.pose_queue = queue.Queue()
+
+    def pose_callback(self, msg):
+        self.heading = get_yaw_from_quaternion(msg.pose.orientation)
+        self.get_logger().info('Current heading: "%f"' % self.heading)
+        self.get_logger().info('Setpoint: "%f"' % self.desired_heading)
+        msg_pub = Twist()
+        msg_pub.linear.x = self.LINEAR_SPEED
+
+        #для предотвращения разворотов по кругу при углах от pi до 2pi
+        if self.desired_heading <= math.pi / 2:
+            pass
+        elif self.heading < 0:
+            self.heading = 2 * math.pi + self.heading
+
+        heading_msg = Float64(); heading_msg.data = self.heading
+        desired_heading_msg = Float64(); desired_heading_msg.data = self.desired_heading
+        self.curr_heading.publish(heading_msg)
+        self.heading_setpoint.publish(desired_heading_msg)
+
+        self.pose_queue.put(msg.pose)
+        if len(self.pose_queue.queue) >= 2:
+            cog = self.get_cog()
+            self.get_logger().info(f'cog data: {cog}')
+
+    def get_cog(self):
+        self.prev_pose, self.curr_pose = self.pose_queue.get(), self.pose_queue.get()
+        delta_x = self.curr_pose.position.x - self.prev_pose.position.x
+        delta_y = self.curr_pose.position.y - self.prev_pose.position.y
+        cog_theta_radians = math.atan2(delta_y, delta_x)
+        cog_theta_degrees = degrees(cog_theta_radians)
+        return {' delta x' : delta_x, 'delta y' : delta_y, 'angle' : cog_theta_degrees}
+
+    def pid_callback(self, msg):
+        pid_out = msg.data
+        msg_pub = Twist()
+        msg_pub.linear.x = self.LINEAR_SPEED
+        msg_pub.angular.z = pid_out
+        self.publisher_twist.publish(msg_pub)
+
+    def pub_timer_callback(self):
+        self.desired_heading += math.pi / 2
+        self.desired_heading %= 2 * math.pi
+        self.get_logger().info('New angle setpoint: "%f"' % self.desired_heading)
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    mover = RurMover()
+
+    rclpy.spin(mover)
+
+    mover.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()