Rendezvous

@clobberbuiltins on.
clearscreen.
set T to vessel("ISS").
set accel to 1.1.
set steeringManager:maxstoppingtime to 0.05.
lock steering to prograde.
set missionstatus to "Burn 1".
print "Press 9 to come into next step".
until ag9 {
    printing().
}

on ag10 {
    abort().
}

                         /// here should be b1() but it is skipped for testing purpose
ag9 off.
lock steering to prograde.
checkan().
until tn < -1000000000 { /// that is for testing purpose so cycle would never stop
    clearscreen.
    checkan().
    print("Angle to AN: " + ANang + " Degrees").
    print("Angle to DN: " + DNang + " Degrees").
    print("True anomaly of the ship: " + ta + " Degrees").
    print("Eccentric anomaly of the ship: " + E + " Degrees").
    print("Mean anomaly of the ship: " + M + " Degrees").
    print("True anomaly of the AN: " + ANTA + " Degrees").
    print("Eccentric anomaly of the AN: " + ANE + " Degrees").
    print("Mean anomaly of the AN: " + ANM + " Degrees").
    print("Time till ascending node: " + tn + " Seconds").
    wait 1.
}

b2().
until checkang {
    clearscreen.
    print "Mission status: " + missionstatus.
    print "Angle: " + (cur_angle - t_angle) + " Degrees".
    print "Angle: " + targetang + " Degrees".
    print "Press 10 to abort current program".
}

b3().
run DragonApproach.


function checkan {
    set targetorbitnormal to vcrs(T:orbit:velocity:orbit:normalized,(T:body:position-T:position):normalized):normalized.
    set shiporbitnormal to vcrs(ship:orbit:velocity:orbit:normalized,(ship:body:position-ship:position):normalized):normalized.
    set vectoan to vcrs(shiporbitnormal,targetorbitnormal):normalized.
    set vectoship to (ship:position-ship:body:position):normalized.
    set ANang to vang(vectoship,vectoan).
    set DNang to -ANang.
    set shipvelxcl to vxcl((ship:body:position-ship:position), ship:orbit:velocity:orbit):normalized.
    set signang to vdot(vectoan,shipvelxcl).
    if signang < 0 {
        set ANang to 360 - ANang.
    }

    set e to orbit:eccentricity.
    set ta to mod(orbit:trueanomaly, 360).
    set n to (2 * constant:pi / orbit:period) * constant:radtodeg().
    set ANTA to mod(ANang + ta,360).
    set DNTA to 180 + ANTA.
    if DNTA > 360 {
        set DNTA to DNTA - 360.
    }

    set ANE to arctan2(sqrt(1 - e^2) * sin(ANTA), e + cos(ANTA)).
    set DNE to arctan2(sqrt(1 - e^2) * sin(DNTA), e + cos(DNTA)).
    set ANMi to mod((ANE - (e*sin(ANE) * constant:radtodeg())) + 360, 360).
    set ANM to mod(ANMi + 360, 360).
    set DNMi to mod((DNE - (e*sin(DNE) * constant:radtodeg())) + 360, 360).
    set DNM to mod(DNMi + 360, 360).
    set E to arctan2(sqrt(1 - e^2) * sin(ta), e + cos(ta)).
    set Mi to (E - (e*sin(E) * constant:radtodeg())).
    set M to mod(Mi + 360,360).
    set tShip to M / n.
    set tAN to ANM / n.
    set tn to tAN - tShip.
}


function abort {
    clearscreen.
    print "Program aborted".
    wait 2.
    print 3/0.
}


function printing {
    clearscreen.
    print "Mission status: " + missionstatus.
    print "Press 10 to abort current program".
    wait 1.
}


function b1 {
    set T to vessel("ISS").
    set TargetAp to ship:apoapsis + body:radius.
    lock ap to ship:apoapsis + body:radius.
    set smja to (ap + TargetAp) / 2.
    set targetvel to sqrt(body:mu*((2/ap)-(1/smja))).
    print targetvel.
    set maneuvertime to time + eta:apoapsis.
    set apvel to velocityat(ship, maneuvertime):orbit:mag.
    print apvel.
    set needvel to targetvel - apvel.
    print needvel.
    set nd to node(maneuvertime, 0, 0, needvel).
    add nd.
    burn_exec().
    set missionstatus to "Burn 2".
}

function b2 {
    set T to vessel("ISS").
    set maneuvertime to tn+time:seconds.
    set velocityatnode to velocityat(ship, maneuvertime):orbit.
    set targetorbitnormal to vcrs(T:velocity:orbit:normalized,(T:body:position-T:position):normalized):normalized.
    set shiporbitnormal to vcrs(ship:velocity:orbit:normalized,(ship:body:position-ship:position):normalized):normalized.
    set vectortobodynorm to (body:position-positionat(ship,maneuvertime)):normalized.
    set inclination to vang(shiporbitnormal,targetorbitnormal).
    if vdot(targetorbitnormal,velocityatnode) > 0 {
        set inc to inclination.
    } else {
        set inc to -inclination.
    }

    set targetvelvector to angleaxis(inc,vectortobodynorm) * velocityatnode.
    set burnvec to targetvelvector-velocityatnode.
    set vectornodenormal to vcrs(velocityatnode,positionat(ship,maneuvertime)-ship:body:position).
    set vectornoderadial to vcrs(vectornodenormal,velocityatnode).
    set nodeprog to vdot(burnvec,velocityatnode:normalized).
    set nodenormal to vdot(burnvec,vectornodenormal:normalized).
    set noderadial to vdot(burnvec,vectornoderadial:normalized).
    set nd to node(maneuvertime, noderadial, nodenormal, nodeprog).
    add nd.
    burn_exec().
    set missionstatus to "Burn 3".
}

function b3 {
    set T to vessel("ISS").
    set TargetAp to T:apoapsis + body:radius+1000.
    set ap to ship:apoapsis + body:radius.
    set smja to (ap + TargetAp) / 2.
    set targetvel to sqrt(body:mu*((2/ap)-(1/smja))).
    print targetvel.
    set now to time:seconds + 50.
    set maneuvertime to now.
    set apvel to velocityat(ship, maneuvertime):orbit:mag.
    print apvel.
    set needvel to targetvel - apvel.
    print needvel.
    set nd to node(maneuvertime, 0, 0, needvel).
    add nd.
    burn_exec().
}


function burn_exec {
    set max_acc to accel.
    set burn_duration to nd:deltav:mag/max_acc.
    print "Burn duration: " + burn_duration + "s".
    lock steering to prograde.
    wait 5.
    until nd:eta <= (burn_duration / 2 + 60) {
        printing().
    }

    set np to nd:deltav.
    lock steering to np.
    until vang(np, ship:facing:vector) < 0.5 {
        printing().
    }

    print("Vehicle is aligned with node").
    wait 4.
    until nd:eta <= (burn_duration / 2) {
        printing().
    }

    set ship:control:fore to 0.
    set done to False.
    set dv0 to nd:deltav.
    until done
    {
        set max_acc to accel.
        set ship:control:fore to min(nd:deltav:mag+0.1 / max_acc, 1).
        if vdot(dv0, nd:deltav) < 0
        {
            print "Burn has been ended, dv " + nd:deltav:mag + "m/s".
            set ship:control:fore to 0.
            break.
        }

        if nd:deltav:mag < 0.1
        {
            print "Finishing burn, dv " + nd:deltav:mag + "m/s".
            wait until vdot(dv0, nd:deltav) < 0.5.
            set ship:control:fore to 0.
            print "Burn has been ended, dv " + nd:deltav:mag + "m/s".
            set done to True.
        }
    }

    unlock steering.
    unlock throttle.
    remove nd.
}

function calcangle {
    set Sa to (2*body("Kerbin"):radius + T:altitude + ship:altitude)/2.
    set Ta to body("Kerbin"):radius + T:altitude.
    return 180*(1-(Sa/Ta)^1.5).
}

function checkang {
    set Tap to T:apoapsis+body:radius.
    set Sap to (T:apoapsis+ship:altitude+2*body:radius)/2.
    set VecS to ship:position - body("Kerbin"):position.
    set VecT to T:position - body("Kerbin"):position.
    set VecHV to vxcl(ship:up:vector, ship:velocity:orbit).
    set VecST to T:position - ship:position.
    set t_angle to calcangle.
    set cur_angle to vang(VecT, VecS).
    set targetang to 1 - sqrt(Sap^3 / Tap^3).
    if vang(Vect, VecS)>90 {
        set cur_angle to -cur_angle.
    }

    return (cur_angle - t_angle) < abs(targetang) + 0.1 and (cur_angle - t_angle) > abs(targetang) - 0.1.
}