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!")