diff --git a/Code/Control/Laptop_Code/constants.py b/Code/Control/Laptop_Code/constants.py
index 7d4416bf3702150f02d81d81f1b34eb217c43408..336b1d4ad468da7c2d177d72374d8f8fb72d00cf 100644
--- a/Code/Control/Laptop_Code/constants.py
+++ b/Code/Control/Laptop_Code/constants.py
@@ -46,3 +46,7 @@ kpy_g,kiy_g,kdy_g = 1.2, 0.01, 0.5
 
 # difference between center of AT and center of goal
 AT_goal_Delta = 110 #cm
+
+AT_h1 = 50 # meters
+
+kph,kih,kdh = 1.2,0.01,0.5
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index 082339b37fd4611c7786df0961cfb5f7f3644361..d82613177590319aeebe23d10a0037a787f11bde 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -122,6 +122,12 @@ def ball_capture(LIDAR_dist):
     else:
         return False
 
+def AT_detect(tx,ty):
+    if tx == 0 and ty == 0:
+        return False
+    else:
+        return True
+
 def stop_all():
     pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
     dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
@@ -201,17 +207,65 @@ def seeking():
         dir1, dir2, dir3, dir4
     """
 
-    pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
+    delta_h = 2
+    url = 'http://192.168.1.118/cam-hi.jpg'
+    tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
+
+    AT_detected = AT_detect(tx, ty)
+    count_h = 0
+
+    threshold_h = 0.3
+    if AT_detected:
+        current_h = get_altitude_from_AT(AT_h1,ty)
+        count_h += 1
+        if count_h == 1:
+            set_h = current_h + delta_h  # in meter
+        pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = vertical_control(current_h, set_h)
+        diff_h = abs(set_h - current_h)
+        if diff_h < threshold_h:
+            new_ssp = 200
+            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction_ssp(new_ssp)
+        else:
+            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = vertical_control(current_h, set_h)
+    else:
+        pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
 
 
     return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
 
+def vertical_control(current_h,set_h):
+
+    pid_h = PID(kph, kih, kdh, setpoint = set_h)
+    pid_h.auto_mode = True
+    pid_h.set_auto_mode(True, last_output = 8.0)
+    pid_h.output_limits = (-255,255)
+
+    input_h = current_h
+    output_h = pid_h(input_h)
+
+    pwm1 = abs(output_h)
+    pwm2 = abs(output_h)
+    pwm3 = 0
+    pwm4 = 0
+    dir1 = '+'
+    dir2 = '+'
+    dir3 = '+'
+    dir4 = '+'
+
+    return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
 def rotate_one_direction():
     pwm1, pwm2, pwm3, pwm4 = 0, 0, seeking_speed, seeking_speed
     dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
 
     return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
 
+def rotate_one_direction_ssp(ssp):
+    pwm1, pwm2, pwm3, pwm4 = 0, 0, ssp, ssp
+    dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
+
+    return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
 def move2ball(gbx, gby, gb_dist):
     """
     Description:
@@ -307,13 +361,13 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
             pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
         else:  # Goal not detected
             # stop_all()  # Debug
-            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
     else:  # Ball not captured
         if ballDetect:  # Ball detected
             pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
         else:  # Ball not detected
             # stop_all()  # Debug
-            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+            pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
 
     return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
 
@@ -485,6 +539,13 @@ def decode_ctl(Ctl_com):
     dir4 = Ctl_com[7]
     return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
 
+def get_altitude_from_AT(AT_h,ty):
+    # below AT center, ty > 0
+    # above AT center, ty < 0
+    altitude = AT_h - ty
+    return altitude
+
+
 def test_function():
     url = 'http://192.168.1.118/cam-hi.jpg'
     tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
@@ -494,6 +555,7 @@ def test_function():
     print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
     print("rx,ry,rz:{},{},{}".format(rx,ry,rz))
 
+
 # ===== Main Function =====
 if __name__ == '__main__':
     # =========== SET UP ============