69 lines
1.9 KiB
Python
69 lines
1.9 KiB
Python
from time import time, sleep
|
|
|
|
import numpy as np
|
|
|
|
|
|
def execute_node(conn):
|
|
ne = conn.mech_jeb.node_executor
|
|
ne.execute_all_nodes()
|
|
|
|
with conn.stream(getattr, ne, "enabled") as enabled:
|
|
enabled.rate = 1
|
|
with enabled.condition:
|
|
while enabled():
|
|
enabled.wait()
|
|
|
|
|
|
def magnitude(vector):
|
|
return np.linalg.norm(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
|