0zinDocking.ks

// 0zin's Docking Script, forked from - !runscript z1fNRuEf
// !runscript a0b2372aaabd528dfd99ec93de5ea170

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

    global 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).
    }
    global 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).
    }

    global function printError {
        parameter error.
        hudtext(error, 20, 2,32, yellow, false).
        print error.
        for i in range(6) { print char(7). wait .15. }
    }

    global function isInvalidTarget {
        return not(hastarget) OR not((target:istype("Vessel") AND target:status <> "LANDED" AND target:status <> "SPLASHED") OR target:istype("Dockingport")).
    }

    clearscreen.
    clearvecdraws().
    sas off.
    set dock to false.
    set menu to gui(300, 30).
    lock distanceBetweenPorts to 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).
    set cam to addons:camera:flightcamera.
    local checkTarget is false.
    local initialPartCount is ship:parts:length.
    if not(defined(localport)) { set localport to ship:controlpart. hudtext("Localport set to controlling part: " + localport:title, 10, 2, 24, yellow, false). }
    set camLex to lexicon("position", cam:position, "target", cam:target, "isActive", false, "isMoving", false).

    if isInvalidTarget()
    {
        printError("Target must be set to a non landed vessel or a dockingport!").
        checkTarget on.
    }
    else {
        set dock to true.
    }

    on isInvalidTarget() {
        if dock AND isInvalidTarget() AND not(ship:parts:length > initialPartCount) {
            printError("Target must be set to a non landed vessel or a dockingport!").
        }
        else if checkTarget {
            dock on.
            checkTarget off.
        }
        return dock or checkTarget.
    }
    if allNodes:length <> 0 { printError("Warning: Your vessel has one or more planned maneuvers!"). }

    when dock then {
        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.

        deadband(0).

        rcsCalc().
        on ship:controlpart { if not(ship:parts:length > initialPartCount) { 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).

        wait 0.

        set sBounds to ship:bounds.
        set sphere_radius to 3 + sBounds:extents:mag.
        set tBounds to 0.
        lock tVes to 0.
        on tVes { if not(tVes:isType("Vessel")) return dock. local tmpTves is tVes. when tVes:loaded then { if tVes = tmpTves set tBounds to choose tVes:bounds if tVes:loaded else 0.} return dock. }
        lock tVes to choose (choose target:ship if target:istype("Dockingport") else target) if hasTarget else "none".

        when true then {
            if isInvalidTarget() {
                set ship:control:translation to v(0,0,0).
                return dock.
            }
            
            if tVes:loaded {
                set sphere_radius to 3 + sBounds:extents:mag + (choose tBounds:extents:mag if tBounds:istype("Bounds") else 0).
            }
            if ship:parts:length < initialPartCount set initialPartCount to ship:parts:length.
            
            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 choose tBounds:abscenter if tBounds:istype("Bounds") else target:position.
                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.

        set portI to 0.
        local prt is 0.

        set trackCam to false.
        on trackCam { if trackCam { if not(camLex:isActive) { set camLex:position to cam:position. set camLex:target to cam:target. camLex:isActive on. } camLex:isMoving off. } else resetCam(). return dock. }
        when trackCam and hastarget then { set cam:position to -target:position:normalized * cam:distance * r(15,10,0). return dock. }

        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(), closestPort(), rcsMult(#), approach(), resetCam()</i>").
        for txt in menu:widgets set txt:style:fontsize to 18.

        on time:second { 
            if isInvalidTarget() return dock.
            
            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. 
        }
    }

    global 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). }
    }
    global function exitDock {
        rcs off. dock off. unlock steering. menu:dispose. set ship:control:translation to v(0,0,0). print "Exiting docking script.". clearvecdraws().
    }
    global function loadDist {
        parameter dist is 3000, ves is target.
        SET ves:loaddistance:ORBIT:UNLOAD TO max(dist + 100, ves:loaddistance:ORBIT:UNLOAD).
        SET ves:loaddistance:ORBIT:LOAD TO max(dist + 50, ves:loaddistance:ORBIT:LOAD).
        WAIT 0.
        SET ves:loaddistance:ORBIT:PACK TO max(dist + 90, ves:loaddistance:ORBIT:PACK).
        SET ves:loaddistance:ORBIT:UNPACK TO max(dist, ves:loaddistance:ORBIT:UNPACK).
        wait 0.
    }
    global function resetCam {
        trackcam off.
        camLex:isActive off.
        camLex:isMoving off.
        set cam:target to camLex:target. wait 0.
        set cam:position to camLex:position.
    }
    global 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 {
            portCam(prt).
            set target to prt. 
        }
    }
    global function closestPort {
        local bestPort is lex("port", 0, "dist", 0).
        if localport:typename <> "DockingPort" {
            printError("No Localport has been selected, please select a Localport first!").
            return.
        }
        for t in buildList("targets") {
            if t:loaded {
                for d in t:dockingports {
                    if (d:state = "Ready" and d:nodetype = localport:nodetype) {
                        if bestPort:port:istype("Scalar") OR d:nodeposition:mag < bestPort:dist {
                            set bestPort:port to d.
                            set bestPort:dist to d:nodeposition:mag.
                        }
                    }
                }
            }
        }
        if bestPort:port:istype("Scalar") {
            printError("No compatible open dockingports found!").
            return.
        }
        if not(bestPort:port:ship:unpacked) loadDist(bestPort:port:ship:distance + 200, bestPort:port:ship).
        portCam(bestPort:port).
        set target to bestPort:port.
    }

    function portCam {
        parameter prt.
        trackcam off.
        set camLex:isMoving to false.
        wait 0.
        set camVel to (cam:position- prt:position):normalized.
        if not(camLex:isActive) {
            set camLex:position to cam:position.
            set camLex:target to cam:target.
            set camLex:isActive to true.
            set camLex:isMoving to true.
        }

        set cam:target to prt. 
        wait 0. 
        //set cam:position to prt:portfacing:vector * 20 + prt:position.
        set cam:position to camLex:position.
        
        
        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.
            if (camTargetPosRel:mag > 1 AND camLex:isMoving) {
                return true.
            }
            else {
                set camLex:isMoving to false.
            }
        }
    }
}