Initial commit

maneuvers/docking.py

@@ -0,0 +1,156 @@
+from time import sleep
+
+from utils import kill_relative_velocity, correct_course, magnitude
+
+
+def set_attitude_and_roll(conn, vessel, reference_frame):
+    fl = vessel.flight(reference_frame)
+    vessel.control.rcs = False
+
+    ap = vessel.auto_pilot
+    ap.reference_frame = reference_frame
+    ap.target_direction = (0, -1, 0)
+    ap.target_roll = 0
+    ap.sas = False
+    ap.engage()
+    ap.wait()
+    ap.disengage()
+
+    mj = conn.mech_jeb
+    sa = mj.smart_ass
+
+    sa.autopilot_mode = mj.SmartASSAutopilotMode.target_plus
+    sa.update(False)
+
+    while magnitude(vessel.angular_velocity(reference_frame)) > .1:
+        sleep(.1)
+
+    print("Ship pointing to dock")
+
+
+def rcs_push(vessel, axis, duration):
+    vessel.control.rcs = True
+    if "x" in axis:
+        vessel.control.up = axis["x"]
+    elif "y" in axis:
+        vessel.control.forward = axis["y"]
+    elif "z" in axis:
+        vessel.control.right = axis["z"]
+    sleep(duration)
+    if "x" in axis:
+        vessel.control.up = 0
+    elif "y" in axis:
+        vessel.control.forward = 0
+    elif "z" in axis:
+        vessel.control.right = 0
+
+    vessel.control.rcs = False
+
+
+def kill_rcs_velocity(vessel, reference_frame):
+    print("Killing RCS velocity")
+    velo = vessel.velocity(reference_frame)
+    vessel.control.rcs = True
+    while any(abs(component) > .05 for component in velo) > .05:
+        if abs(velo[0]) > .05:
+            sign = -velo[0] / abs(velo[0])
+            if abs(velo[0]) > .1:
+                vessel.control.up = 1 * sign
+            elif abs(velo[0]) > .05:
+                vessel.control.up = .1 * sign
+            else:
+                vessel.control.up = 0
+
+        if abs(velo[1]) > .05:
+            sign = -velo[1] / abs(velo[1])
+            if abs(velo[1]) > .1:
+                vessel.control.forward = 1 * sign
+            elif abs(velo[1]) > .05:
+                vessel.control.forward = .1 * sign
+            else:
+                vessel.control.forward = 0
+
+        if abs(velo[2]) > .05:
+            sign = velo[2] / abs(velo[2])
+            if abs(velo[2]) > .1:
+                vessel.control.right = 1 * sign
+            elif abs(velo[2]) > .05:
+                vessel.control.right = .1 * sign
+            else:
+                vessel.control.right = 0
+        sleep(.1)
+        velo = vessel.velocity(reference_frame)
+    vessel.control.rcs = False
+    print("RCS velocity killed")
+
+
+def align_horizontally(conn, vessel, reference_frame):
+    conn.drawing.add_direction((1, 0, 0), vessel.reference_frame)
+
+    while abs(vessel.position(reference_frame)[0]) > .1 \
+            or abs(vessel.position(reference_frame)[2]) > .1:
+        # determine power requirements of each
+        sign_x = 1 if vessel.position(reference_frame)[0] > 0 else -1
+        if abs(vessel.position(reference_frame)[0]) > 1:
+            power_x = 1
+        elif abs(vessel.position(reference_frame)[0]) > .1:
+            power_x = .1
+        else:
+            power_x = 0
+            sign_x = 0
+
+        sign_z = 1 if vessel.position(reference_frame)[2] > 0 else -1
+        if abs(vessel.position(reference_frame)[2]) > 1:
+            power_z = 1
+        elif abs(vessel.position(reference_frame)[2]) > .1:
+            power_z = .1
+        else:
+            power_z = 0
+            sign_z = 0
+
+        axis = {}
+        if power_x > 0:
+            axis["x"] = -sign_x * power_x
+        if power_z > 0:
+            axis["z"] = sign_z * power_z
+
+        rcs_push(vessel, axis, 1)
+
+        while (sign_x > 0 and vessel.position(reference_frame)[0] > .1
+               or sign_x < 0 and vessel.position(reference_frame)[0] < -.1
+               or sign_x == 0) \
+                and (sign_z > 0 and vessel.position(reference_frame)[2] > .1
+                     or sign_z < 0 and vessel.position(reference_frame)[2] < -.1
+                     or sign_z == 0):
+            print(vessel.position(reference_frame))
+            sleep(.1)
+
+        kill_rcs_velocity(vessel, reference_frame)
+    print("Vertical alignment done!")
+
+
+def dock_ship(conn, vessel, docking_part, reference_frame):
+    conn.drawing.add_direction((0, 1, 0), reference_frame)
+    conn.drawing.add_direction((1, 0, 0), reference_frame)
+    vessel.parts.controlling = docking_part
+
+    kill_relative_velocity(conn, vessel, reference_frame)
+
+    set_attitude_and_roll(conn, vessel, reference_frame)
+
+    align_horizontally(conn, vessel, reference_frame)
+
+    print("Starting docking procedure")
+    vessel.control.set_action_group(0, True)
+    rcs_push(vessel, {"y": 1}, .5)
+
+    vessel.control.rcs = True
+    try:
+        while vessel.position(reference_frame)[1] > 0:
+            print(vessel.position(reference_frame)[1])
+            correct_course(conn, vessel, (0, 0, 0), reference_frame)
+            sleep(1)
+    except ValueError as e:
+        vessel = conn.space_center.active_vessel
+    finally:
+        vessel.control.rcs = False