227 lines
8.0 KiB
Python
227 lines
8.0 KiB
Python
from krpc.services.spacecenter import SASMode
|
|
|
|
import numpy as np
|
|
from time import time, sleep
|
|
|
|
from .utils import magnitude, unitary, kill_relative_velocity, correct_course, get_safety_radius, point_toward_direction
|
|
|
|
from . import Maneuver
|
|
|
|
|
|
class ApproachManeuver(Maneuver):
|
|
|
|
def __init__(self, conn, mission_control, reference_frame):
|
|
super().__init__(conn, mission_control)
|
|
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
|
|
|
|
|
|
THROTTLE = .1
|
|
VELOCITY_TOLERANCE = .1
|
|
|
|
|
|
def thrust(vessel, delta_v, reference_frame):
|
|
print("Starting velocity change")
|
|
starting_velocity = magnitude(vessel.velocity(reference_frame))
|
|
vessel.control.throttle = THROTTLE
|
|
|
|
if delta_v < 0:
|
|
while magnitude(vessel.velocity(reference_frame)) - starting_velocity > delta_v + VELOCITY_TOLERANCE:
|
|
while magnitude(vessel.velocity(reference_frame)) - starting_velocity > delta_v + .3:
|
|
sleep(.01)
|
|
vessel.control.throttle = THROTTLE / 10
|
|
sleep(.01)
|
|
else:
|
|
while magnitude(vessel.velocity(reference_frame)) - starting_velocity < delta_v - VELOCITY_TOLERANCE:
|
|
while magnitude(vessel.velocity(reference_frame)) - starting_velocity < delta_v - .3:
|
|
sleep(.01)
|
|
vessel.control.throttle = THROTTLE / 10
|
|
sleep(.01)
|
|
|
|
vessel.control.throttle = 0
|
|
print("Velocity change achieved")
|
|
|
|
|
|
def move_to_waypoint(conn, vessel, waypoint, reference_frame):
|
|
mj = conn.mech_jeb
|
|
sa = mj.smart_ass
|
|
|
|
kill_relative_velocity(conn, vessel, reference_frame)
|
|
|
|
conn.drawing.add_line(vessel.position(reference_frame), waypoint, reference_frame)
|
|
waypoint = np.array(waypoint)
|
|
|
|
distance = magnitude(waypoint - vessel.position(reference_frame))
|
|
if distance > 250:
|
|
velocity = 10
|
|
elif distance > 50:
|
|
velocity = 5
|
|
elif distance > 25:
|
|
velocity = 2
|
|
else:
|
|
velocity = 1
|
|
|
|
direction = waypoint - np.array(vessel.position(reference_frame))
|
|
point_toward_direction(vessel, direction, reference_frame)
|
|
|
|
start_position = np.array(vessel.position(reference_frame))
|
|
print("Starting acceleration")
|
|
thrust(vessel, velocity, reference_frame)
|
|
print("Target velocity achieved")
|
|
acceleration_distance = magnitude(np.array(vessel.position(reference_frame)) - start_position)
|
|
|
|
sa.autopilot_mode = mj.SmartASSAutopilotMode.relative_minus
|
|
sa.update(False)
|
|
|
|
while magnitude(vessel.angular_velocity(reference_frame)) > .1:
|
|
sleep(.1)
|
|
|
|
vessel.control.rcs = True
|
|
while magnitude(waypoint - vessel.position(reference_frame)) > acceleration_distance:
|
|
print(magnitude(waypoint - vessel.position(reference_frame)), " ", acceleration_distance)
|
|
sleep(.1)
|
|
correct_course(conn, vessel, waypoint, reference_frame)
|
|
|
|
vessel.control.rcs = False
|
|
vessel.control.up = 0
|
|
vessel.control.right = 0
|
|
|
|
print("Starting deceleration")
|
|
thrust(vessel, -velocity, reference_frame)
|
|
print("Ship decelerated")
|
|
|
|
sa.autopilot_mode = mj.SmartASSAutopilotMode.off
|
|
sa.update(True)
|
|
|
|
print("destination position: {}".format(waypoint))
|
|
print("end position: {}".format(np.array(vessel.position(reference_frame))))
|
|
|
|
|
|
SAFETY_RADIUS_MARGIN = 10
|
|
|
|
|
|
# DEPRECATED
|
|
def maneuver_to_approach(conn, reference_frame):
|
|
print("Handling approach")
|
|
sc = conn.space_center
|
|
vessel = sc.active_vessel
|
|
target = sc.target_vessel
|
|
kill_relative_velocity(conn, vessel, reference_frame)
|
|
conn.drawing.add_direction((0, 1, 0), reference_frame)
|
|
|
|
vessel.control.rcs = False
|
|
|
|
pv = vessel.position(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(reference_frame)
|
|
move_vector = np.array((plane_move_vector[0], 0, plane_move_vector[1]))
|
|
move_to_waypoint(conn, vessel, pv + move_vector, reference_frame)
|
|
|
|
print("We're outside of the safety cylinder, setting vertical distance")
|
|
pv = vessel.position(reference_frame)
|
|
move_to_waypoint(conn, vessel, (pv[0], safety_radius, pv[2]), 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(conn, vessel, (0, safety_radius, 0), reference_frame)
|
|
|
|
point_toward_direction(vessel, - np.array(vessel.position(reference_frame)), reference_frame)
|
|
|
|
print("Approach handled")
|
|
|
|
|
|
TARGET_VELOCITY = 2
|
|
|
|
|
|
# DEPRECATED
|
|
def move_with_vector(conn, vessel, vector, reference_frame):
|
|
mj = conn.mech_jeb
|
|
sa = mj.smart_ass
|
|
|
|
kill_relative_velocity(conn, vessel, reference_frame)
|
|
|
|
position = np.array(vessel.position(reference_frame))
|
|
vector = np.array(vector)
|
|
destination = position + vector
|
|
|
|
conn.drawing.add_line(vessel.position(reference_frame), destination, reference_frame)
|
|
|
|
print("Pointing to acceleration")
|
|
point_toward_direction(vessel, unitary(vector), reference_frame)
|
|
print("Pointed")
|
|
trip_duration = magnitude(vector) / TARGET_VELOCITY
|
|
acceleration_start = time()
|
|
print("Starting acceleration")
|
|
vessel.control.throttle = THROTTLE
|
|
while magnitude(vessel.velocity(reference_frame)) < TARGET_VELOCITY:
|
|
sleep(.01)
|
|
vessel.control.throttle = 0
|
|
print("Target velocity achieved")
|
|
|
|
acceleration_duration = time() - acceleration_start
|
|
|
|
sa.autopilot_mode = mj.SmartASSAutopilotMode.relative_minus
|
|
sa.update(False)
|
|
|
|
while acceleration_duration < trip_duration - (time() - acceleration_start):
|
|
sleep(.01)
|
|
|
|
print("Starting deceleration")
|
|
vessel.control.throttle = THROTTLE
|
|
|
|
deceleration_start = time()
|
|
while time() - deceleration_start < acceleration_duration:
|
|
sleep(.01)
|
|
vessel.control.throttle = 0
|
|
print("Deceleration done")
|
|
|
|
sa.autopilot_mode = mj.SmartASSAutopilotMode.off
|
|
sa.update(False)
|
|
|
|
print("starting position: {}".format(position))
|
|
print("destination position: {}".format(destination))
|
|
print("end position: {}".format(np.array(vessel.position(reference_frame)))) |