diff --git a/config/control-config.yaml b/config/control-config.yaml
index 975dca1da1c14790a576f1ecd210ca0bc18df0e3..7662b5a7aee4fa6a366bd7be217e4de0b9cc0662 100644
--- a/config/control-config.yaml
+++ b/config/control-config.yaml
@@ -2,11 +2,11 @@
   pid_heading:
     ros__parameters:
         angular: true
-        clamp: 1.0
+        clamp: 3.5
         inverted: false
-        kd: 0.9
-        ki: 0.0
-        kp: 0.9
+        kd: 300.0
+        ki: 1.0
+        kp: 20.0
         start_type_description_service: true
         use_sim_time: false
   pid_distance:
diff --git a/lesson_07/step_1.py b/lesson_07/step_1.py
new file mode 100644
index 0000000000000000000000000000000000000000..6271c36c7402f44a9db7eab0131434c3986a71b5
--- /dev/null
+++ b/lesson_07/step_1.py
@@ -0,0 +1,150 @@
+import rclpy
+from rclpy.node import Node
+                                
+from std_msgs.msg import Float64
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import PoseStamped, PointStamped, Pose
+
+from tf_transformations import euler_from_quaternion
+
+import math                                                                    
+
+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]
+
+def get_distance(delta_point):
+    return sum(map(lambda x: x**2, delta_point))**0.5
+
+def get_course_to_point(delta_point):
+    ln = get_distance(delta_point)
+    if ln:
+        dxn = delta_point[0]/ln
+        dyn = delta_point[1]/ln
+        ang = math.acos(dxn)
+        if dyn < 0:
+            ang = 2*math.pi - ang
+        return ang
+    return 0
+
+def get_dist_course(delta_point):
+    dist = get_distance(delta_point)
+    course = get_course_to_point(delta_point)
+    return dist, course
+
+def get_delta_point(from_point, to_point):
+    return [to_point[0] - from_point[0], to_point[1] - from_point[1]]
+
+
+class RurMover(Node):
+    def __init__(self):
+        super().__init__('controller')
+        self.CONTROLER_PERIOD = 1/10
+        self.DIST_THRESHOLD = 0.2
+
+        self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10)
+
+        zero_point = Float64()
+        zero_point.data = 0.0
+        self.heading_state = self.create_publisher(Float64, '/waterstrider/pid_heading/state', 10)
+        self.heading_setpoint = self.create_publisher(Float64, '/waterstrider/pid_heading/setpoint', 10)
+        self.heading_result = self.create_subscription(Float64, '/waterstrider/pid_heading/output', self.pid_heading_callback, 10)
+        self.heading_setpoint.publish(zero_point)
+
+        self.dist_state = self.create_publisher(Float64, '/waterstrider/pid_distance/state', 10)
+        self.dist_setpoint = self.create_publisher(Float64, '/waterstrider/pid_distance/setpoint', 10)
+        self.dist_result = self.create_subscription(Float64, '/waterstrider/pid_distance/output',
+                                                    self.pid_distance_callback, 10)
+        self.dist_setpoint.publish(zero_point)
+
+        self.create_subscription(PoseStamped, '/waterstrider/ground_truth_to_tf_waterstrider/pose', self.pose_callback, 10)
+        self.create_subscription(PoseStamped, '/goal_pose', self.goal_callback, 10)
+
+        self.Flag = False
+        self.Flag_dist = False
+        self.Flag_head = False
+
+        self.heading = 0
+        self.self_point = None
+        self.goal_point = None
+
+        self.timer = self.create_timer(self.CONTROLER_PERIOD, self.update)
+        self.result = Twist()
+
+    def update(self):
+        if self.Flag_dist == True & self.Flag_head == True:
+            if self.dist <= self.DIST_THRESHOLD :
+                self.result.linear.x = 0.0
+                self.result.angular.z = 0.0
+                self.Flag = False
+            self.publisher_twist.publish(self.result)
+
+            self.Flag_dist = False
+            self.Flag_head = False
+
+
+    def pose_callback(self, msg):
+        if self.Flag == True:
+            heading = get_yaw_from_quaternion(msg.pose.orientation)
+            self.self_point = []
+
+            self.self_point.insert(0, msg.pose.position.x)
+            self.self_point.insert(1, msg.pose.position.y)
+
+
+            self.dist, crs = get_dist_course(get_delta_point(self.self_point, self.goal_point))
+
+            diff = angle_diff(heading, crs)
+
+            msg_pub1 = Float64()
+            msg_pub1.data = diff
+            self.heading_state.publish(msg_pub1)
+
+            msg_pub2 = Float64()
+            msg_pub2.data = 0.0
+            self.heading_setpoint.publish(msg_pub2)
+
+            msg_pub3 = Float64()
+            msg_pub3.data = self.dist
+            self.dist_setpoint.publish(msg_pub3)
+
+            msg_pub4 = Float64()
+            msg_pub4.data = 0.0
+            self.dist_state.publish(msg_pub4)
+
+
+    def goal_callback(self, msg):
+        self.goal_point = []
+
+        self.goal_point.insert(0, msg.pose.position.x)
+        self.goal_point.insert(1, msg.pose.position.y)
+
+        self.Flag = True
+
+    def pid_distance_callback(self, msg):
+        self.result.linear.x = msg.data
+        self.Flag_dist = True
+
+    def pid_heading_callback(self, msg):
+        self.result.angular.z = msg.data
+        self.Flag_head = True
+
+    def log_info(self, msg: str) -> None:
+        self.get_logger().info(msg)
+
+
+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