Implement docking.

dock.ks

@@ -0,0 +1,124 @@
+@LAZYGLOBAL off.
+
+run once "lib/rendezvous".
+run once "lib/rocket".
+run once "lib/vectors".
+
+
+function translate {
+    //
+    // Translate the ship's position by the given vector using RCS thrusters.
+    //
+    parameter translation_vector_delegate.
+    parameter max_speed.
+
+    local lock translation_vector to translation_vector_delegate().
+    // Find available RCS acceleration by Newton's second law: (F=mg). Note that the acceleration is conservatively divided
+    // by 6 since thrusters might not be aligned with thrust vector (but assumed evenly distributed on the 6 axes).
+    // Furthermore, acceleration is limited to 0.05m/s^2 since small ships with low mass are uncontrollable if allowed to accelerate wildly.
+    local acceleration is min(0.05, (rcs_maxthrust() / SHIP:MASS) / 6).
+    // Time t to travel distance d under constant acceleration a is t = sqrt(2d/a) (https://en.wikipedia.org/wiki/Equations_for_a_falling_body).
+    // Multiply this by the available acceleration to get speed (s * m/s^2 = m/s) required to decelerate to 0 by the time d=0.
+    local lock desired_speed to min(max_speed, sqrt(2*translation_vector:mag / acceleration) * acceleration).
+    local lock desired_velocity to translation_vector:normalized * desired_speed.
+
+    // The proportional gain factor is set to the square root of the ship's mass. Why? It was empirically shown to work.
+    // The idea is that small ships are more sensitive to adjustments, and so small error corrections will do more harm than good.
+    local pid_x is PIDLOOP(sqrt(SHIP:MASS), 0.01, 0.001, -1, 1).  // kp, ki, kd, minoutput, maxoutput
+    local pid_y is PIDLOOP(sqrt(SHIP:MASS), 0.01, 0.001, -1, 1).
+    local pid_z is PIDLOOP(sqrt(SHIP:MASS), 0.01, 0.001, -1, 1).
+
+    //vdraw(SHIP:controlpart:position, translation_vector@, WHITE).
+    //vdraw(SHIP:controlpart:position, relative_velocity@, GREEN).
+
+    RCS on.
+    until translation_vector:mag < 0.05 and relative_velocity():mag < 0.01 {
+        set pid_x:setpoint to desired_velocity:x.
+        set pid_y:setpoint to desired_velocity:y.
+        set pid_z:setpoint to desired_velocity:z.
+
+        local x is pid_x:update(TIME:seconds, relative_velocity():x).
+        local y is pid_y:update(TIME:seconds, relative_velocity():y).
+        local z is pid_z:update(TIME:seconds, relative_velocity():z).
+
+        set SHIP:CONTROL:TRANSLATION to ship_raw_to_ship_control(V(x,y,z)).
+    }
+}
+
+
+function dock {
+    //
+    // Dock the ship with another vessel.
+    // Prerequisites:
+    //  Must be controlling from a local docking port (right click -> control from here).
+    //  TARGET must be a docking port (right click -> set as target).
+    //
+    parameter max_translation_speed is 1.0.
+    parameter final_docking_speed is 0.1.
+
+    print "==================== DOCKING =====================".
+
+    // Target port vectors
+    local lock target_port to TARGET:nodeposition.
+    local lock target_port_facing to TARGET:portfacing:vector.
+    //vdraw(target_port@, target_port_facing@, CYAN).
+
+    // Local port vectors
+    local lock local_port to SHIP:controlpart:nodeposition.
+    local lock local_port_facing to SHIP:controlpart:portfacing:vector.
+    //vdraw(local_port@, local_port_facing@, CYAN).
+
+    SAS off.
+    set NAVMODE to "TARGET".
+
+    print "==> Aligning with target port".
+    local lock alignment_direction to lookdirup(-target_port_facing, TARGET:ship:facing:vector).  // align dock-on-dock but with UP the same direction
+    lock STEERING to alignment_direction.
+    wait until vang(SHIP:facing:vector, alignment_direction:vector) <= 1.
+
+    print "==> Translating".
+    // TODO: Translate "around" the target vessel on different axes first so we dont fail if behind the target docking port.
+    translate({return (target_port + target_port_facing) - (local_port + local_port_facing).}, max_translation_speed).
+
+    print "==> Docking".
+    // TARGET will be unset the moment we dock, causing many of the calculations and local locks to cause errors.
+    // Therefore, the final docking will be done using the information available to us now, without any error corrections.
+    lock STEERING to SHIP:facing.
+    set SHIP:CONTROL:TRANSLATION to ship_raw_to_ship_control(target_port - local_port).
+    wait until relative_velocity():mag >= final_docking_speed.
+    unlock_control().
+
+    wait until not HASTARGET.
+    print "==> DOCKING COMPLETE".
+}
+
+
+dock().
+
+
+
+
+// TODO: Make GUI with dropdown of target:dockingports and SHIP:dockingports so user can't fuck it up.
+//if not HASTARGET {
+//    print "Please select a target".
+//    return.
+//}
+//function find_docking_port {
+//    parameter target.  // SHIP or TARGET.
+//    parameter target_name is target:name.
+//
+//    if target:istype("DockingPort") {
+//        return target.
+//    }
+//
+//    local target_ports is target:dockingports.
+//    if target_ports:empty {
+//        print "No docking ports on " + target_name.
+//        return.
+//    }
+//    if target_ports:length <> 1 {
+//        print "Multiple docking ports on " + target_name + ", please select one and try again".
+//        return.
+//    }
+//    return target_ports[0]
+//}

kOS.xml

@@ -83,7 +83,7 @@
     <keywords3 keywords="vecdraw;vecdrawargs;hudtext;clearvecdraws" />
     
     <!-- Various Functions -->
-    <keywords3 keywords="vessel;NODE" />
+    <keywords3 keywords="vessel;NODE;PIDLOOP" />
     
     <!-- Global Structures -->
     <keywords3 keywords="LEXICON;LIST;QUEUE;RANGE;STACK;UNIQUESET" />

lib/node.ks

@@ -12,13 +12,24 @@ function estimated_burn_duration {
     //  https://space.stackexchange.com/questions/27375/how-do-i-calculate-a-rockets-burn-time-from-required-velocity
     //
     parameter burn.
+    parameter isp is isp_sum().
+    parameter maxthrust is SHIP:maxthrust.
 
     if burn:istype("Node") {
         set burn to burn:deltav.
     }
 
-    local exhaust_velocity is isp_sum() * (CONSTANT:G * KERBIN:mass).
+    local exhaust_velocity is isp * (CONSTANT:G * KERBIN:mass).
-    return ((SHIP:mass * exhaust_velocity) / SHIP:maxthrust) * (1 - CONSTANT:E^(-burn:mag/exhaust_velocity)).
+    return ((SHIP:mass * exhaust_velocity) / maxthrust) * (1 - CONSTANT:E^(-burn:mag/exhaust_velocity)).
+}
+
+
+function estimated_rcs_burn_duration {
+    //
+    // Calculate estimated burn duration of a vector or node using RCS thrusters instead of main engines.
+    //
+    parameter burn.
+    return estimated_burn_duration(burn, rcs_isp_sum(), rcs_maxthrust()).
 }
 
 

lib/rendezvous.ks

@@ -3,12 +3,26 @@
 run once "lib/node".
 
 
+function relative_velocity {
+    //
+    // Return the relative orbital velocity between the ship and our target in the target's inertial frame of reference.
+    //
+    if not HASTARGET {
+        return V(0,0,0).  // avoids 'has no target' errors upon docking
+    }
+
+    local t is TARGET.
+    if TARGET:istype("Part") {  // e.g. if selected docking port as target
+        set t to TARGET:ship.
+    }
+    return SHIP:velocity:orbit - t:velocity:orbit.
+}
+
+
 function kill_relative_velocity {
-    parameter target is TARGET.
     parameter tolerance is 0.1.
 
-    local relative_velocity is target:velocity:orbit - SHIP:velocity:orbit.
+    if relative_velocity():mag > tolerance {
-    if relative_velocity:mag > tolerance {
+        execute_burn(-relative_velocity()).
-        execute_burn(relative_velocity).
     }
 }

lib/rocket.ks

@@ -51,6 +51,41 @@ function isp_sum {
 }
 
 
+function rcs_isp_sum {
+    //
+    // Return the sum of ISP for RCS thrusters.
+    //
+    local sum is 0.
+    for thruster in SHIP:modulesnamed("ModuleRCSFX") {
+        set sum to sum + thruster:getfield("rcs isp").
+    }
+    return sum.
+}
+
+
+function rcs_maxthrust {
+    // There's no way to get thrust of RCS dynamically in kOS; gonna have to hard-code it :/
+    // Source: https://wiki.kerbalspaceprogram.com/wiki/Reaction_Control_System#Thrusters
+    local rcs_thrusts is LEXICON(  // in kN
+        "RCSBlock", 1.0,  // RV-105 RCS Thruster Block
+        "linearRcs", 2.0,  // Place-Anywhere 7 Linear RCS Port
+        "vernierEngine", 12.0  // Vernor Engine
+    ).
+
+    local thrust_sum is 0.
+    for module in SHIP:modulesnamed("ModuleRCSFX") {
+        local part_name is module:part:name.
+        if rcs_thrusts:haskey(part_name) {
+            set thrust_sum to thrust_sum + rcs_thrusts[part_name].
+        } else {  // if non-stock part
+            print "WARNING: Unknown RCS Thruster '" + part_name + "'; using default thrust of 1.0 kN".
+            set thrust_sum to thrust_sum + 1.0.
+        }
+    }
+    return thrust_sum.
+}
+
+
 function deploy_fairings {
     print "Deploying fairings".
     for module in SHIP:modulesnamed("ModuleProceduralFairing") {

lib/vectors.ks

@@ -16,23 +16,40 @@ set_actual_prograde().
 
 
 function vdraw {
+    //
+    // Wrapper for the VECDRAW-function that doesn't hide the vector by default.
+    //
     parameter start.  // vector or function. For example, pass {return UP:vector.} to automatically update the draw
     parameter vector.  // same as start
     parameter color is RED.
     parameter label is "".
+    parameter scale is 1.0.
+    parameter width is 0.2.
 
-    vecdraw(
+    return vecdraw(
       start,
       vector,
       color,
       label,
-      1.0,  // scale
+      scale,
-      true,  // show? defaults to false in original vecdraw
+      true,  // show
-      0.2  // width
+      width
     ).
 }
 
 
+function ship_raw_to_ship_control {
+    //
+    // Translate a vector in SHIP-RAW coordinates to one in SHIP-CONTROL (STARBOARD, TOP, FORE) coordinates.
+    //
+    parameter vec.
+
+    return V(vdot(vec, FACING:starvector),  // projection is correct because SHIP's FACING vectors are normalised
+             vdot(vec, FACING:topvector),
+             vdot(vec, FACING:forevector)).  // Same order as SHIP:CONTROL:TRANSLATION's input
+}
+
+
 // ANGLES
 lock actual_prograde_pitch to vang(actual_prograde:vector, UP:vector).