97 lines
3.6 KiB
Python
97 lines
3.6 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, kill_rcs_velocity, correct_course, get_safety_radius,\
|
|
point_toward_direction
|
|
|
|
from . import Maneuver
|
|
|
|
|
|
class ApproachRCSManeuver(Maneuver):
|
|
|
|
SAFETY_RADIUS_MARGIN = 10
|
|
|
|
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_rcs_velocity(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) + self.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
|
|
planar_move_vector = unitary((pv[0], pv[2])) * (safety_radius - magnitude((pv[0], pv[2])))
|
|
spacial_move_vector = np.array((planar_move_vector[0], 0, planar_move_vector[1]))
|
|
|
|
pv = vessel.position(self.reference_frame)
|
|
move_to_waypoint(self.conn, vessel, pv + spacial_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)
|
|
point_toward_direction(vessel, (0, -1, 0), self.reference_frame)
|
|
|
|
return True
|
|
|
|
|
|
def move_to_waypoint(conn, vessel, waypoint, reference_frame):
|
|
mj = conn.mech_jeb
|
|
|
|
kill_relative_velocity(conn, vessel, reference_frame)
|
|
|
|
conn.drawing.add_line(vessel.position(reference_frame), waypoint, reference_frame)
|
|
waypoint = np.array(waypoint)
|
|
|
|
start_position = np.array(vessel.position(reference_frame))
|
|
vector = waypoint - start_position
|
|
distance = magnitude(vector)
|
|
direction = unitary(vector)
|
|
acceleration_distance = distance / 4
|
|
|
|
point_toward_direction(vessel, direction, reference_frame)
|
|
|
|
print("Starting acceleration")
|
|
remaining_distance = distance
|
|
vessel.control.rcs = True
|
|
vessel.control.forward = 1
|
|
while remaining_distance > 3 * acceleration_distance:
|
|
sleep(.1)
|
|
remaining_distance = magnitude(waypoint - vessel.position(reference_frame))
|
|
vessel.control.forward = 0
|
|
print("Target velocity achieved")
|
|
|
|
while remaining_distance > acceleration_distance:
|
|
sleep(.1)
|
|
correct_course(conn, vessel, waypoint, reference_frame)
|
|
remaining_distance = magnitude(waypoint - vessel.position(reference_frame))
|
|
print(remaining_distance)
|
|
|
|
print("Starting deceleration")
|
|
kill_rcs_velocity(vessel, reference_frame)
|
|
print("Ship decelerated")
|
|
|
|
print("destination position: {}".format(waypoint))
|
|
print("end position: {}".format(np.array(vessel.position(reference_frame))))
|