Docking

//docking script - !runscript z1fNRuEf
clearscreen.
clearvecdraws().
sas off.

if not hastarget { hudtext("Target must be set to a vessel or a dockingport before running this script.", 10, 2, 24, yellow, false). }

set dock to true.
set cam to addons:camera:flightcamera.
if not(defined(localport)) { set localport to ship:controlpart. hudtext("Localport set to controlling part: " + localport:title, 10, 2, 24, yellow, false). }
on ship:controlpart { set localport to ship:controlpart. return dock. }
set sep to 5.
set safety to true.
set safetyAngle to 33.
if not ( defined sl ) set sl to 0.
set rotation to 0.
set acc to 0.1.
set fa to facing.
lock offset to v(0,0,0). //alter this to manually offset the target position.
if target:istype("Vessel") set tVes to target. else set tVes to target:ship.

function deadband {
    parameter db is 0.05.
    for rcsb in ship:rcs {
        set rcsb:deadband to db.
    }
}
deadband(0).



set axesT to lexicon("fore",0,"aft",0,"star", 0, "port", 0, "top", 0, "bottom", 0).
set axesAcc to lexicon("fore",0,"aft",0,"star", 0, "port", 0, "top", 0, "bottom", 0).

function rcsMult {
    parameter mult.
    set fpid to pidloop(4 * (mult/((axesAcc["fore"] + axesAcc["aft"])/2)),0,0,-1,1).
    set tpid to pidloop(4 * (mult/((axesAcc["top"] + axesAcc["bottom"])/2)),0,0,-1,1).
    set spid to pidloop(4 * (mult/((axesAcc["star"] + axesAcc["port"])/2)),0,0,-1,1).
}
function rcsCalc {
    for axisKey in axesT:keys set axesT[axisKey] to 0.
    for rp in ship:rcs {
        for tv in rp:thrustvectors {
            if (rp:foreenabled) {
                local vd is vdot(facing:vector, tv).
                if vd > 0 set axesT:fore to axesT:fore + rp:availablethrust * vd.
                else set axesT:aft to axesT:aft - rp:availablethrust * vd.
            }
            if (rp:STARBOARDENABLED) {
                local vd is vdot(facing:starvector, tv).
                if vd > 0 set axesT:star to axesT:star + rp:availablethrust * vd.
                else set axesT:port to axesT:port - rp:availablethrust * vd.
            }
            if (rp:TOPENABLED) {
                local vd is vdot(facing:topvector, tv).
                if vd > 0 set axesT:top to axesT:top + rp:availablethrust * vd.
                else set axesT:bottom to axesT:bottom - rp:availablethrust * vd.
            }
        }

    }
    print "RCS acc: ".
    set minAcc to 9999.
    for axisKey in axesT:keys {
        local axisThrust is axesT[axisKey].
        local axisAcc to axisThrust/mass.
        set minAcc to min(minAcc,axisAcc).
        print "  "+ axisKey + ": " + round(axisThrust/mass,3) + " m/s2".
        set axesAcc[axisKey] to axisAcc.
    }
    set acc to max(0.005,minAcc * 0.6).
    rcsMult(2).
}


rcsCalc().
on ship:controlpart { rcsCalc(). return dock. }

set vd_fore to vecdraw(v(0,0,0), v(0,0,0), yellow, "", 1, TRUE, 0.1).
set vd_star to vecdraw(v(0,0,0), v(0,0,0), magenta, "", 1, TRUE, 0.1).
set vd_top to vecdraw(v(0,0,0), v(0,0,0), cyan, "", 1, TRUE, 0.1).

set tBounds to tVes:bounds.
set sBounds to ship:bounds.
set sphere_radius to 3 + sBounds:extents:mag + tBounds:extents:mag.
when true then {
    set fa to choose target:portfacing if target:istype("Dockingport") else target:facing.
    set localPortPos to choose localport:nodeposition if localport:istype("Dockingport") else ship:position.
    set targetPortPos to choose target:nodeposition if target:istype("Dockingport") else target:position.
    //set rel_v to velocity:orbit - tVes:velocity:orbit.
    set rel_v to velocityat(ship,time:seconds):orbit - velocityat(tVes,time:seconds):orbit.

    set posAlignment to round(vang(fa:vector, localPortPos-targetPortPos),2).

    if safety and posAlignment > safetyAngle and (localPortPos-targetPortPos):mag > 3 {
        set tCent to tBounds:abscenter.
        set wv_pos to tCent + tCent:normalized * -sphere_radius + vxcl(tCent,targetPortPos + fa:vector * sphere_radius):normalized * max(0, 10 + min(0,tCent:mag - sphere_radius)).
    }
    else {
        set wv_pos to (targetPortPos + offset + fa:vector * sep) - localPortPos.
    }


    set wanted_v to wv_pos:normalized * min(sl , sqrt(2 * acc * max(wv_pos:mag,0.0001))).
    if wv_pos:mag < 1 set wanted_v:mag to min(wanted_v:mag, wv_pos:mag * 0.5).
    set wanted_v to wanted_v + vcrs(tves:angularvel, -tves:position).
    set ve to wanted_v - rel_v.

    set rcs_v to v(
        -spid:update(time:seconds, vdot(facing:starvector,ve)),
        -tpid:update(time:seconds,vdot(facing:topvector,ve)),
        -fpid:update(time:seconds,vdot(facing:vector,ve))
    ).
    set ship:control:translation to rcs_v.

    set vd_pos to cam:position:normalized * (cam:distance - 3).
    set vd_fore:start to vd_pos.
    set vd_star:start to vd_pos.
    set vd_top:start to vd_pos.

    set vd_fore:vec to facing:vector * rcs_v:z.
    set vd_star:vec to facing:starvector * rcs_v:x.
    set vd_top:vec to facing:topvector * rcs_v:y.

    return dock.
}

lock steering to lookdirup(-fa:vector,angleaxis(rotation, fa:vector) * fa:topvector).
lock distanceBetweenPorts to (targetPortPos-localPortPos):mag.

function approach {
    set sl to max(0.2, sl).
    hudtext("Approaching. Set SL to 0 to cancel", 10, 2, 36, yellow, false).
    when true then {  set sep to max(-0.1, distanceBetweenPorts-1). return sl > 0. }
    on ship:parts:length { exitDock(). hudtext("Docked? Or something fell off. Done", 10, 2, 24, yellow, false). }
}
function exitDock {
    rcs off. dock off. unlock steering. menu:dispose. set ship:control:translation to v(0,0,0). print "Exiting docking script.". clearvecdraws().
}
function loadDist {
    parameter dist is 3000.
    SET target:loaddistance:ORBIT:UNLOAD TO dist + 100.
    SET target:loaddistance:ORBIT:LOAD TO dist + 50.
    WAIT 0.
    SET target:loaddistance:ORBIT:PACK TO dist + 90.
    SET target:loaddistance:ORBIT:UNPACK TO dist.
    wait 0.
}

set portI to 0.
local prt is 0.
function nextPort {
    if not(hastarget) return.

    if target:typename = "Vessel" {
        if not(target:unpacked) loadDist(target:distance + 200).
        set target to target:dockingports[0].
    }
    local ports is target:ship:dockingports.
    local cnt to 0.
    local done is false.
    until done {
        set prt to ports[portI].
        set portI to portI + 1.
        if portI >= ports:length set portI to 0.
        if localport:typename = "DockingPort" and prt:state = "Ready" and prt:nodetype = localport:nodetype {
            done on.
        } else if localport:typename <> "DockingPort" and prt:state = "Ready" {
            done on.
        } else if cnt > ports:length {
            done on.
        }
        set cnt to cnt + 1.
    }
    if cnt <= ports:length + 1 {
        trackcam off.
        set camVel to (cam:position- target:position):normalized.
        set target to prt.
        set camOldPos to cam:position.
        set cam:target to prt.
        wait 0.
        //set cam:position to prt:portfacing:vector * 20 + prt:position.
        set cam:position to camOldPos.


        when true then {
            set camTargetPosRel to ((prt:portfacing:vector * 10 + prt:nodeposition) - cam:position).
            set camVel to camVel + camTargetPosRel:normalized * 0.08.
            set camVel:mag to min(1, camTargetPosRel:mag / 20).
            set cam:position to cam:position + camVel * 0.75.
            return camTargetPosRel:mag > 1.
        }
    }
}

set trackCam to false.
when trackCam and hastarget then { set cam:position to -target:position:normalized * 20 * r(15,10,0). return dock. }

function resetCam {
    trackcam off.
    set cam:target to localport. wait 0.
    set cam:position to (target:position:normalized * -20) * r(20,10,0).
}




set menu to gui(300, 30). Set menu:x to -60. set menu:y to 400. menu:show.
menu:addlabel("<b><color=yellow>Docking parameters</color></b>").
set m1 to menu:addlabel("SL:").
set mAcc to menu:addlabel("Acc:").
set m2 to menu:addlabel("SEP:").
set mRot to menu:addlabel("Extra Port Rotation:").
set m3 to menu:addlabel("Localport:").
set m4 to menu:addlabel("Target:").

set m8 to menu:addlabel("Steering Alignment: ").
set mPosAlignment to menu:addlabel("Positional alignment:").
set m5 to menu:addlabel("Distance fwd:").
set m6 to menu:addlabel("Distance side:").
set mTrack to menu:addlabel("TrackCam:").

menu:addlabel("Functions: <i>nextport(), rcsMult(#), approach(), resetCam()</i>").
for txt in menu:widgets set txt:style:fontsize to 18.

on time:second {
    set sideDist to round(vxcl(facing:vector, targetPortPos-localPortPos):mag,2).
    set angError to vang(-facing:vector, fa:vector).
    set m1:text to "SL: <color=white>" + round(sl,2) + "m/s</color>".
    set mAcc:text to "Acc: <color=white>" + round(acc,3) + "m/s2</color>".
    set m2:text to "SEP: <color=white>" + round(sep,2) + "m</color>".
    set mRot:text to "Rotation: <color=white>" + rotation + "</color>" + char(0176).

    if (localport:istype("Dockingport")) set m3:text to "Localport: <color=white>" + localport + "</color>".
    else set m3:text to "Localport: <color=orange>" + localport + "</color>".

    if (target:istype("Dockingport")) set m4:text to "Target: <color=white>" + target + "</color>".
    else set m4:text to "Target: <color=orange>" + target + "</color>".

    set m5:text to "Distance: " + round(distanceBetweenPorts,2) + "m".
    if sideDist <= 0.1 set m6:text to "Distance side: " + sideDist + "m".
    else set m6:text to "Distance side: <color=orange>" + sideDist + "m</color>".

    if angError <= 5 set m8:text to "Steering Alignment: " + round(angError,1) + char(0176).
    else set m8:text to "Steering Alignment: <color=orange>" + round(angError,1) + char(0176) + "</color>".


    if posAlignment <= 5 set mPosAlignment:text to "Positional alignment: " + posAlignment + char(0176).
    else set mPosAlignment:text to "Positional alignment: <color=orange>" + posAlignment + char(0176) + "</color>".

    set mTrack:text to "TrackCam: <color=white>" + trackCam + "</color>".
    return menu:visible.
}