Basic mission vessel tunnel

maneuvers/approach.py

@@ -3,11 +3,53 @@ from krpc.services.spacecenter import SASMode
 import numpy as np
 from time import time, sleep
 
-from utils import magnitude, kill_relative_velocity, correct_course
+from .utils import magnitude, unitary, kill_relative_velocity, correct_course
+
+from . import Maneuver
 
 
-def unitary(vector):
-    return np.array(vector) / magnitude(vector)
+class ApproachManeuver(Maneuver):
+
+    def __init__(self, conn, vessel_id, reference_frame):
+        super().__init__(conn, vessel_id)
+        self.reference_frame = reference_frame
+
+    def start(self):
+        sc = self.conn.space_center
+        vessel = sc.active_vessel
+        target = sc.target_vessel
+
+        kill_relative_velocity(self.conn, vessel, self.reference_frame)
+
+        self.conn.drawing.add_direction((0, 1, 0), self.reference_frame)
+
+        vessel.control.rcs = False
+
+        pv = vessel.position(self.reference_frame)
+
+        safety_radius = get_safety_radius(vessel) + get_safety_radius(target) + SAFETY_RADIUS_MARGIN
+
+        # if under and inside safety cylinder's circle
+        if pv[1] < safety_radius and pow(pv[0], 2) + pow(pv[2], 2) <= pow(safety_radius, 2):
+            print("We're under the target and inside the safety cylinder, getting out")
+            # get out of the cylinder
+            plane_move_vector = unitary(tuple((pv[0], pv[2]))) * (safety_radius - magnitude(tuple((pv[0], pv[2]))))
+
+            pv = vessel.position(self.reference_frame)
+            move_vector = np.array((plane_move_vector[0], 0, plane_move_vector[1]))
+            move_to_waypoint(self.conn, vessel, pv + move_vector, self.reference_frame)
+
+        print("We're outside of the safety cylinder, setting vertical distance")
+        pv = vessel.position(self.reference_frame)
+        move_to_waypoint(self.conn, vessel, (pv[0], safety_radius, pv[2]), self.reference_frame)
+
+        # should be above and outside => get inside
+        print("We're at the right vertical distance to the target, setting horizontal position")
+        move_to_waypoint(self.conn, vessel, (0, safety_radius, 0), self.reference_frame)
+
+        point_toward_direction(vessel, - np.array(vessel.position(self.reference_frame)), self.reference_frame)
+
+        return True
 
 
 def get_safety_radius(vessel):
@@ -106,8 +148,6 @@ def move_to_waypoint(conn, vessel, waypoint, reference_frame):
     thrust(vessel, -velocity, reference_frame)
     print("Ship decelerated")
 
-    #do positition correction
-
     sa.autopilot_mode = mj.SmartASSAutopilotMode.off
     sa.update(True)
 
@@ -118,6 +158,7 @@ def move_to_waypoint(conn, vessel, waypoint, reference_frame):
 SAFETY_RADIUS_MARGIN = 10
 
 
+# DEPRECATED
 def maneuver_to_approach(conn, reference_frame):
     print("Handling approach")
     sc = conn.space_center
@@ -158,6 +199,7 @@ def maneuver_to_approach(conn, reference_frame):
 TARGET_VELOCITY = 2
 
 
+# DEPRECATED
 def move_with_vector(conn, vessel, vector, reference_frame):
     mj = conn.mech_jeb
     sa = mj.smart_ass