277 lines
8.2 KiB
Python
277 lines
8.2 KiB
Python
from time import time, sleep
|
|
import math
|
|
|
|
import numpy as np
|
|
|
|
from krpc.services.spacecenter import SASMode
|
|
|
|
from connector import get_connexion
|
|
|
|
|
|
|
|
def magnitude(vector):
|
|
return np.linalg.norm(vector)
|
|
|
|
|
|
def node_thrust_time(vessel, node):
|
|
return (node.delta_v * vessel.mass) / vessel.available_thrust
|
|
|
|
|
|
def unitary(vector):
|
|
return np.array(vector) / magnitude(vector)
|
|
|
|
|
|
THROTTLE = .1
|
|
|
|
|
|
def kill_relative_velocity(conn, vessel, reference_frame):
|
|
mj = conn.mech_jeb
|
|
sa = mj.smart_ass
|
|
|
|
vessel.control.throttle = 0
|
|
print("Killing relative velocity")
|
|
while magnitude(vessel.velocity(reference_frame)) > .05:
|
|
sa.autopilot_mode = mj.SmartASSAutopilotMode.relative_minus
|
|
sa.update(False)
|
|
while magnitude(vessel.angular_velocity(reference_frame)) > .1:
|
|
sleep(.1)
|
|
|
|
vessel.control.throttle = THROTTLE if magnitude(vessel.velocity(reference_frame)) > 1 else THROTTLE / 10
|
|
current_speed = magnitude(vessel.velocity(reference_frame))
|
|
previous_speed = current_speed
|
|
while current_speed <= previous_speed:
|
|
sleep(.1)
|
|
previous_speed = current_speed
|
|
current_speed = magnitude(vessel.velocity(reference_frame))
|
|
|
|
vessel.control.throttle = 0
|
|
|
|
print("Relative velocity killed")
|
|
sa.autopilot_mode = mj.SmartASSAutopilotMode.off
|
|
sa.update(False)
|
|
|
|
|
|
def correct_course(conn, vessel, waypoint, reference_frame):
|
|
waypoint = np.array(conn.space_center.transform_position(tuple(waypoint), reference_frame, vessel.reference_frame))
|
|
|
|
waypoint_x = round(waypoint[0], 0)
|
|
if waypoint_x < 0:
|
|
vessel.control.right = -.1
|
|
elif waypoint_x > 0:
|
|
vessel.control.right = .1
|
|
else:
|
|
vessel.control.right = 0
|
|
|
|
waypoint_z = round(waypoint[2], 0)
|
|
if waypoint_z < 0:
|
|
vessel.control.up = .1
|
|
elif waypoint_z > 0:
|
|
vessel.control.up = -.1
|
|
else:
|
|
vessel.control.up = 0
|
|
|
|
|
|
def correct_course_to_target(vessel, target):
|
|
target_position = target.position(vessel.reference_frame)
|
|
|
|
angle_x = math.atan(target_position[0]/target_position[1])
|
|
if math.isclose(angle_x, 0, abs_tol=0.05):
|
|
vessel.control.right = 0
|
|
elif angle_x < 0:
|
|
vessel.control.right = -.1
|
|
elif angle_x > 0:
|
|
vessel.control.right = .1
|
|
|
|
angle_z = math.atan(target_position[2]/target_position[1])
|
|
if math.isclose(angle_z, 0, abs_tol=0.05):
|
|
vessel.control.up = 0
|
|
elif angle_z < 0:
|
|
vessel.control.up = .1
|
|
elif angle_z > 0:
|
|
vessel.control.up = -.1
|
|
|
|
|
|
def kill_relative_velocity_rcs(vessel, target):
|
|
print("Killing RCS velocity")
|
|
|
|
vessel.control.sas = True
|
|
vessel.control.rcs = True
|
|
|
|
velocity = target.velocity(vessel.reference_frame)
|
|
while any(abs(component) >= .1 for component in velocity):
|
|
thrust = get_required_rcs_thrust(vessel, velocity)
|
|
vessel.control.right = thrust[0] if abs(velocity[0]) >= .1 else 0
|
|
vessel.control.forward = thrust[1] if abs(velocity[1]) >= .1 else 0
|
|
vessel.control.up = - thrust[2] if abs(velocity[2]) >= .1 else 0
|
|
|
|
print(target.velocity(vessel.reference_frame))
|
|
print((thrust[0], thrust[1], thrust[2]))
|
|
print((vessel.control.right, vessel.control.forward, vessel.control.up))
|
|
|
|
sleep(.1)
|
|
velocity = target.velocity(vessel.reference_frame)
|
|
|
|
continue
|
|
if abs(velocity[0]) > .05:
|
|
sign = velocity[0] / abs(velocity[0])
|
|
if abs(velocity[0]) > 1:
|
|
vessel.control.right = 1 * sign
|
|
elif abs(velocity[0]) > .1:
|
|
vessel.control.right = .1 * sign
|
|
else:
|
|
vessel.control.right = 0
|
|
|
|
if abs(velocity[1]) > .05:
|
|
sign = velocity[1] / abs(velocity[1])
|
|
if abs(velocity[1]) > 1:
|
|
vessel.control.forward = 1 * sign
|
|
elif abs(velocity[1]) > .1:
|
|
vessel.control.forward = .1 * sign
|
|
else:
|
|
vessel.control.forward = 0
|
|
|
|
if abs(velocity[2]) > .05:
|
|
sign = - velocity[2] / abs(velocity[2])
|
|
if abs(velocity[2]) > 1:
|
|
vessel.control.up = 1 * sign
|
|
elif abs(velocity[2]) > .1:
|
|
vessel.control.up = .1 * sign
|
|
else:
|
|
vessel.control.up = 0
|
|
sleep(.1)
|
|
velocity = target.velocity(vessel.reference_frame)
|
|
vessel.control.rcs = False
|
|
vessel.control.sas = False
|
|
print("RCS velocity killed")
|
|
|
|
|
|
def get_required_rcs_thrust(vessel, delta_v, polling=.1):
|
|
acceleration = np.array(vessel.available_rcs_force) / vessel.mass
|
|
thrust = [0, 0, 0]
|
|
for i in range(3):
|
|
if delta_v[i] >= 0:
|
|
thrust[i] = max(min(delta_v[i] / acceleration[0][i]*polling, 1), .051)
|
|
else:
|
|
thrust[i] = min(max(-delta_v[i] / acceleration[1][i]*polling, -1), -.051)
|
|
|
|
return thrust
|
|
|
|
|
|
def get_safety_radius(vessel):
|
|
bbox = vessel.bounding_box(vessel.reference_frame)
|
|
return max(magnitude(bbox[0]), magnitude(bbox[1]))
|
|
|
|
|
|
def point_toward_direction(vessel, direction, reference_frame):
|
|
ap = vessel.auto_pilot
|
|
ap.reference_frame = reference_frame
|
|
ap.target_direction = unitary(direction)
|
|
ap.target_roll = 0
|
|
ap.rcs = False
|
|
ap.sas = False
|
|
ap.engage()
|
|
sleep(1)
|
|
ap.wait()
|
|
|
|
ap.disengage()
|
|
ap.sas_mode = SASMode.stability_assist
|
|
ap.sas = True
|
|
|
|
|
|
def point_toward_target(conn, vessel, target, force_roll=False):
|
|
sa = conn.mech_jeb.smart_ass
|
|
sa.autopilot_mode = conn.mech_jeb.SmartASSAutopilotMode.target_plus
|
|
sa.force_roll = force_roll
|
|
sa.update(False)
|
|
while magnitude(vessel.angular_velocity(target.reference_frame)) > .002:
|
|
sleep(.1)
|
|
|
|
|
|
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 set_attitude_and_roll(conn, vessel, 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 align_horizontally(conn, vessel, reference_frame):
|
|
conn.drawing.add_direction((1, 0, 0), vessel.reference_frame)
|
|
target = conn.space_center.target_vessel
|
|
|
|
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_relative_velocity_rcs(vessel, target)
|
|
print("Vertical alignment done!")
|