166 lines
5.2 KiB
Python
166 lines
5.2 KiB
Python
from time import sleep
|
|
|
|
from .utils import kill_relative_velocity, correct_course, magnitude
|
|
|
|
from . import Maneuver
|
|
|
|
|
|
class DockingManeuver(Maneuver):
|
|
def __init__(self, conn, mission_control, docking_part, reference_frame):
|
|
super().__init__(conn, mission_control)
|
|
self.docking_part = docking_part
|
|
self.reference_frame = reference_frame
|
|
|
|
def start(self):
|
|
vessel = self.conn.space_center.active_vessel
|
|
self.conn.drawing.add_direction((0, 1, 0), self.reference_frame)
|
|
self.conn.drawing.add_direction((1, 0, 0), self.reference_frame)
|
|
vessel.parts.controlling = self.docking_part
|
|
|
|
kill_relative_velocity(self.conn, vessel, self.reference_frame)
|
|
set_attitude_and_roll(self.conn, vessel, self.reference_frame)
|
|
align_horizontally(self.conn, vessel, self.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(self.reference_frame)[1] > 0:
|
|
print(vessel.position(self.reference_frame)[1])
|
|
correct_course(self.conn, vessel, (0, 0, 0), self.reference_frame)
|
|
sleep(1)
|
|
except ValueError as e:
|
|
vessel = self.conn.space_center.active_vessel
|
|
finally:
|
|
vessel.control.rcs = False
|
|
|
|
return True
|
|
|
|
|
|
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!")
|