Skip to content

Instantly share code, notes, and snippets.

@HerrCraziDev
Last active July 19, 2021 22:30
Show Gist options
  • Save HerrCraziDev/a98349df55923b354424677834aa16b8 to your computer and use it in GitHub Desktop.
Save HerrCraziDev/a98349df55923b354424677834aa16b8 to your computer and use it in GitHub Desktop.
Auto-docking script for KSP, using kOS. It doesn't performs orbital rendez-vous, hence you should be in the vicinity of your target when executing it.
//***********************************/
//* Auto-docking system */
//* - */
//* © FAITO Aerospace Inc. - 2059 */
//***********************************/
// Author : HerrCrazi - License : GNU GPL 3.0
// This program is distributed by FAITO Aerospace and the Feguan Open Source Foundation under a GNU GLP 3.0 license,
// as part of the National Open-source Initiative of 2096.
// GNU Fegelnix, DRACO OS and the FRS program belongs to their respective licensers (University of Huturoa, Dragon Space Ltd. and FAITO Aerospace)
// v1.3.7
parameter debug is false. //Set this to true to see some vectors on the screen showing the ship's speed & position
function vproj
{
parameter a.
parameter b.
return vdot(a, b) / b:mag.
}
function getSignedMag
{
parameter vect.
parameter baseVect.
local vangle is vang(vect, baseVect).
if ( vangle > 90 and vangle < 270 )
{
return vect:mag.
} else {
return -vect:mag.
}
}
clearvecdraws().
clearscreen.
print "***********************************".
print "* Auto-docking system *".
print "* - *".
print "* © FAITO Aerospace Inc. - 2059 *".
print "***********************************".
set PID_y to pidloop(10,0,5,-1,1).
set PID_y:setpoint to 0.
set PID_z to pidloop(10,0,5,-1,1).
set PID_z:setpoint to 0.
set PID_x to pidloop(10,0,5,-1,1).
set PID_x:setpoint to 0.
rcs on.
sas off.
if defined target and target:targetable
{
set trgt to target.
print "Docking to : " + trgt:name.
//Lock steering to the same orientation than the target's port, but in the opposite direction (so the ship is facing the target port)
lock steering to lookdirup(-trgt:portfacing:forevector, trgt:portfacing:upvector).
wait until vang(ship:facing:forevector, -trgt:portfacing:forevector) < 1 and vang(ship:facing:upvector, trgt:portfacing:upvector) < 5.
//Target-centered base, with the x axis pointing forward out of the target
lock vtx to trgt:portfacing:forevector.
lock vty to trgt:portfacing:upvector. //y axis is straight up out of the target
lock vtz to trgt:portfacing:rightvector. //z axis is starboard out of the target
//Ship and target position in SHIP-RAW coordinates
lock spos to ship:position.
lock tpos to trgt:position.
lock targetVel to trgt:ship:velocity:orbit - ship:velocity:orbit. //Target-relative velocity
//Declared here for the when statement below
declare trgt_y is 1.
declare trgt_z is 1.
declare Kx is 0.
set stop to false.
set continue to true.
// Modes:
// 3 - Plane matching (if we're behind the docking port, move to the other side to avoid docking from BEHIND a docking port which is NOT recommended)
// 2 - Alignment
// 1 - Approach (keep alignment and control forward speed)
set runmode to 3.
// AG1 toggles ALL correction burns
on AG1
{
toggle stop.
}
// ABORT terminates the program and releases the controls
on ABORT
{
set continue to false.
print "Docking aborted, controls released.".
}
//At this point we're aligned with the target, it's time to burn forward
when ( abs(trgt_z) < 0.1 and abs(trgt_y) < 0.1 ) then
{
set Kx to 0.4.
set runmode to 1.
return false.
}
// Once we're ahead of the plane of the target, start to align the two ports
when ( trgt_x > 2 ) then
{
set runmode to 2.
return false.
}
until ( continue = false and trgt:state <> "Ready" and trgt:state <> "PreAttached" )//Press AG1 to stop at any moment
{
set v_ShipToTarget to tpos - spos. //A vector going from the ship to the target
//Convert the position vector of the ship relative to the target from SHIP-RAW to target-relative coordinates
set trgt_x to getSignedMag(vtx * vproj(v_ShipToTarget, vtx), vtx).
set trgt_y to getSignedMag(vty * vproj(v_ShipToTarget, vty), vty).
set trgt_z to getSignedMag(vtz * vproj(v_ShipToTarget, vtz), vtz).
//Same for their relative velocity
set targetVel_x to -getSignedMag(vtx * vproj(targetVel, vtx), vtx).
set targetVel_y to -getSignedMag(vty * vproj(targetVel, vty), vty).
set targetVel_z to -getSignedMag(vtz * vproj(targetVel, vtz), vtz).
//Draw some fancy vectors
if debug
{
SET vdt TO VECDRAW(trgt:position, (-trgt:portfacing:forevector) * 5, yellow, "Trgt", 1, true).
SET vds TO VECDRAW(V(0,0,0), (ship:facing:forevector) * 5, yellow, "Ship", 1, true).
set x to VECDRAW( V(0,0,0),-vtx * trgt_x , red, "x", 1, true, 0.1).
set y to VECDRAW( -vtx * trgt_x, -vty * trgt_y , green, "y", 1, true, 0.1).
set z to VECDRAW( -vtx * trgt_x - vty * trgt_y, -vtz * trgt_z , blue, "z", 1, true, 0.1).
set tx to VECDRAW( V(0,0,0), -vtx * targetVel_x * 10, red, "Tx", 1, true, 0.05).
set ty to VECDRAW( V(0,0,0), -vty * targetVel_y * 10, green, "Ty", 1, true, 0.05).
set tz to VECDRAW( V(0,0,0), -vtz * targetVel_z * 10, blue, "Tz", 1, true, 0.05).
}
//Apply PID corrections
if runmode <> 3 { // Apply corrections
set Ky to min(max(trgt_y/5,-1),1).
set Kz to min(max(trgt_z/5,-1),1).
} else { // Not yet ahead of the target plane
set Ky to 0.
set Kz to 0.
}
if runmode = 3 and trgt_x < 2 {
set Kx to -1.
} else if runmode = 2 {
set Kx to 0.
} else if runmode = 1 {
set Kx to 0.3.
}
if stop {
set ship:control:top to 0.
set ship:control:starboard to 0.
set ship:control:fore to 0.
set ship:control:neutralize to true.
} else {
set PID_y:setpoint to Ky.
set ship:control:top to -PID_y:update(time:seconds, targetVel_y).
set PID_z:setpoint to Kz.
set ship:control:starboard to PID_z:update(time:seconds, targetVel_z).
set PID_x:setpoint to Kx.
set ship:control:fore to PID_x:update(time:seconds, targetVel_x).
}
//Print stuff
if stop {
print "Mode : " + runmode + " [STOP]" at (0, terminal:height - 11).
} else {
print "Mode : " + runmode + " " at (0, terminal:height - 11).
}
print "Corr. X : " + round(PID_x:output,3) + " (" + round(Kx,2) + ") " at (0, terminal:height - 9).
print "Corr. Y : " + round(PID_y:output,3) + " (" + round(Ky,2) + ") " at (0, terminal:height - 8).
print "Corr. Z : " + round(PID_z:output,3) + " (" + round(Kz,2) + ") " at (0, terminal:height - 7).
print "Vel. X : " + round(targetVel_x, 3) + "m/s " at (terminal:width / 2, terminal:height - 9).
print "Vel. Y : " + round(targetVel_y, 3) + "m/s " at (terminal:width / 2, terminal:height - 8).
print "Vel. Z : " + round(targetVel_z, 3) + "m/s " at (terminal:width / 2, terminal:height - 7).
print "Dist. : " + round(trgt_x, 3) + "m " at (0, terminal:height - 5).
print "Right : " + round(trgt_z, 3) + "m " at (0, terminal:height - 4).
print "Up : " + round(trgt_y, 3) + "m " at (0, terminal:height - 3).
print "Rel. ang : "+round(vang( (-trgt:portfacing:forevector), ship:facing:forevector ), 2) at (0, terminal:height - 1).
wait 0.1.
}
unlock all.
set ship:control:neutralize to true.
sas on.
clearvecdraws().
} else {
print "No dockable target selected.".
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment