twokmclosing

//Closing on Space Station from 2 km
clearscreen.
set pi to 3.1415926535897932384626433832795028841971693993751058209749445923.
set mu to 3.986*(10^14).
ag1 off.
set target to "LIESS".
set lateraloffset   to 0.
set verticaloffset to 0.
rcs on.
sas off.
gear off.
lock steering to prograde + r(0,0,180).

set maneuverSpeed to 1.

//functions
function lateraldist {
	set uct to vcrs(ship:up:vector,target:position). //up cross target
	set ev to vcrs(ship:body:position,ship:north:vector).

	if vang(uct,ship:north:vector) > 90 {
		set thdg to vang(uct,ev).
	} else {
		set thdg to 360-vang(uct,ev). //heading of the target vector
	}.
  if thdg > 360 {
    set thdg to thdg-360.
  }.
  set ucv to vcrs(ship:up:vector,ship:velocity:orbit).
  if vang(ucv,ship:north:vector) > 90 {
		set vhdg to vang(ucv,ev).
	} else {
		set vhdg to 360-vang(ucv,ev). //heading of the velocity vector
	}

  if vhdg-thdg > 180{
    set hdgdiff to vhdg-thdg-360.
  }. else {
    set hdgdiff to vhdg-thdg.
  }.
  set latdist to sin(hdgdiff)*target:position:mag.
	return latdist.
}.
function verticaldist {
  set velangle to vang(ship:velocity:orbit,ship:up:vector).
  set targangle to vang(target:position,ship:up:vector).
  set pitchdiff to velangle-(targangle-verticaloffset).
  set vdist to sin(pitchdiff)*target:position:mag.
	return vdist.
}.
function depthdist {
	set targpos to (target:position+ship:body:position).
	set ang to vang(targpos,ship:body:position)*pi/180.
	set ddist to ang*ship:body:position:mag.
	return ddist.
}
function lateralinputFunction {
	if abs(lateraldistance2) > 10 {
		set vlat to (lateralDistance2-lateralDistance1)/(dt).
		if lateralDistance2 > 0 {
			set ms to -maneuverSpeed.
		}. else {
			set ms to  maneuverSpeed.
		}
		set vlatError to ms-vlat.
		set latinput  to kpv*vlatError.
	}. else {
		set slatError to lateralDistance2.
		set vlat to (lateralDistance2-lateralDistance1)/(dt).
		set latinput to -((kps*lateralDistance2)+(kds*vlat)).
	}.
	return latinput.
}
function verticalinputFunction {
	if abs(verticalDistance2+extraH) > 10 {
		set vver to (verticalDistance2-verticalDistance1)/(dt).
		if verticaldistance2+extraH > 0 {
			set ms to -maneuverSpeed.
		}. else {
			set ms to  maneuverSpeed.
		}
		set vverError to ms-vver.
		set vertinput  to kpv*vverError.
    ///////////////////////////////////////////
	}. else {
		set sverError to (verticaldistance2+extraH).
		set vver to (verticalDistance2-verticalDistance1)/(dt).
		set vertinput to -((kps*sverError)+(kds*vver)).
	}.
	return vertinput.
}.
function depthinputFunction {
	if abs(depthDistance2-depthoffset) > 10 {
		set vdep to (depthDistance2-depthDistance1)/(dt).
		if depthdistance2-depthoffset > 0 {
			set ms to -maneuverSpeed.
		}. else {
			set ms to  maneuverSpeed.
		}
		set vdepError to ms-vdep.
		set depinput  to -kpv*vdepError.
	///////////////////////////////////////////
	}. else {
		set sdepError to depthdistance2-depthoffset.
		set vdep to ((depthDistance2)-(depthDistance1))/(dt).
		set depinput to ((kps*sdepError)+(kds*vdep)).
	}.
	return depinput.
}
////////////////////////////////////////////////////////////////////////////////
//defining values and gains
set kpv to 1.
set kps to .1.
set kds to 2.

set latinput to 0.
set verticalinput to 0.
set depthinput to 0.
set extraH to 0.
set vver to 0.
set depthoffset    to depthdist().

//matching planes
set lateraldistance2 to 4123.

until lateralDistance2<200 {
	if 2 > 1 {
		set lateralDistance1  to lateraldist().
		set verticalDistance1 to verticaldist().
		set depthDistance1    to depthdist().
		set t1                to time:seconds.
		print "Lateral Difference  "+round(lateralDistance1)+" m".
		print "Vertical Difference "+round(verticalDistance1)+" m".
		print "Depth Difference    "+round(depthDistance1)+" m".
		print " ".
		print "Lateral Input  "+round(100*latinput).
		print "Vertical Input "+round(100*verticalinput).
		print "Depth Input "+round(100*depthinput).
		print " ".
		print "Depth Offset "+round(depthoffset)+" m".
		wait 0.2.
		clearscreen.
		set lateralDistance2  to lateraldist().
		set verticalDistance2 to verticaldist().
		set depthDistance2    to depthdist().
		set t2                to time:seconds.
		set dt                to t2-t1.
	}. //this is just so i can hide it easily
	////////////////////////////////////////////
	set lateralinput to 0.
	set verticalinput to verticalinputfunction().
	set depthinput to depthinputfunction().
	///////////////////////////////////////////
	set ship:control:starboard to lateralinput.
	set ship:control:top       to verticalinput.
	set ship:control:fore 		 to depthinput.

	if sas =true {
		unlock steering.
	}. else {
		lock steering to prograde+r(0,0,180).
	}.

}.

lock steering to prograde+r(0,0,180).

//Two KM station keeping
until ag1 {
	if 2 > 1 {
		set lateralDistance1  to lateraldist().
		set verticalDistance1 to verticaldist().
		set depthDistance1    to depthdist().
		set t1                to time:seconds.
		print "Lateral Difference  "+round(lateralDistance1)+" m".
		print "Vertical Difference "+round(verticalDistance1)+" m".
		print "Depth Difference    "+round(depthDistance1)+" m".
		print " ".
		print "Lateral Input  "+round(100*latinput).
		print "Vertical Input "+round(100*verticalinput).
		print "Depth Input "+round(100*depthinput).
		print " ".
		print "Depth Offset "+round(depthoffset)+" m".
		wait 0.2.
		clearscreen.
		set lateralDistance2  to lateraldist().
		set verticalDistance2 to verticaldist().
		set depthDistance2    to depthdist().
		set t2                to time:seconds.
		set dt                to t2-t1.
	}. //this is just so i can hide it easily
	////////////////////////////////////////////
	set lateralinput to lateralinputFunction().
	set verticalinput to verticalinputfunction().
	set depthinput to depthinputfunction().
	///////////////////////////////////////////
	set ship:control:starboard to lateralinput.
	set ship:control:top       to verticalinput.
	set ship:control:fore      to depthinput.
}.



set extrah to -1000. //initiates downward maneuver
ag1 off.


//downward maneuver begins
until ag1 {
	if 2 > 1 {
		set lateralDistance1  to lateraldist().
		set verticalDistance1 to verticaldist().
		set depthDistance1    to depthdist().
		set t1                to time:seconds.
		print "Lateral Difference  "+round(lateralDistance1)+" m".
		print "Vertical Difference "+round(verticalDistance1)+" m".
		print "Depth Difference    "+round(depthDistance1)+" m".
		print " ".
		print "Lateral Input  "+round(100*latinput).
		print "Vertical Input "+round(100*verticalinput).
		print "Depth Input "+round(100*depthinput).
		print " ".
		print "Depth Offset "+round(depthoffset)+" m".
		wait 0.2.
		clearscreen.
		set lateralDistance2  to lateraldist().
		set verticalDistance2 to verticaldist().
		set depthDistance2    to depthdist().
		set t2                to time:seconds.
		set dt                to t2-t1.
	}. //this is just so i can hide it easily
	////////////////////////////////////////////
	set lateralinput to lateralinputFunction().
	set verticalinput to verticalinputfunction().
	set depthinput to depthinputfunction().
	///////////////////////////////////////////
	set ship:control:starboard to lateralinput.
	set ship:control:top       to verticalinput.
	set ship:control:fore      to depthinput.
}.
//I found a short boost foreward works well. this is about 1 m/s dv prograde
//foreward acceleration averages .4 m/s2
set ship:control:neutralize to true.
print "Executing foreward boost maneuver".
set t0 to time:seconds.
set ship:control:fore to 1.
wait until time:seconds>t0+2.5.
set ship:control:neutralize to true.

//coasting
set extrah to 0.
clearscreen.
set ship:control:neutralize to true.

lock steering to prograde + r(0,0,180).
ag1 off.
unlock steering.
sas on.
wait until ship:altitude<(target:altitude-21).
until ship:altitude>(target:altitude-20) {
	print "Coasting One".
	print "Altitude Difference: "+round(abs(target:altitude-ship:altitude))+" m".
	wait 0.1.
	clearscreen.
}.
set warp to 0.
sas off.
lock steering to prograde + r(0,0,180).
print "Preparing for Stationkeeping".
wait until ship:altitude>target:altitude.
set depthoffset    to depthdist().


//stationkeeping at approximately 1km
until ag1 {
	if 2 > 1 {
		set lateralDistance1  to lateraldist().
		set verticalDistance1 to verticaldist().
		set depthDistance1    to depthdist().
		set t1                to time:seconds.
		print "Lateral Difference  "+round(lateralDistance1)+" m".
		print "Vertical Difference "+round(verticalDistance1)+" m".
		print "Depth Difference    "+round(depthDistance1)+" m".
		print " ".
		print "Lateral Input  "+round(100*latinput).
		print "Vertical Input "+round(100*verticalinput).
		print "Depth Input "+round(100*depthinput).
		print " ".
		print "Depth Offset "+round(depthoffset)+" m".
		wait 0.2.
		clearscreen.
		set lateralDistance2  to lateraldist().
		set verticalDistance2 to verticaldist().
		set depthDistance2    to depthdist().
		set t2                to time:seconds.
		set dt                to t2-t1.
	}. //this is just so i can hide it easily
	////////////////////////////////////////////
	set lateralinput to lateralinputFunction().
	set verticalinput to verticalinputfunction().
	set depthinput to depthinputfunction().
	///////////////////////////////////////////
	set ship:control:starboard to lateralinput.
	set ship:control:top       to verticalinput.
	set ship:control:fore      to depthinput.
}.
ag1 off.

//initiates second closing maneuver
set maneuverspeed to 0.5.
set extrah to -1000.
ag1 off.


until ag1 {
	if 2 > 1 {
		set lateralDistance1  to lateraldist().
		set verticalDistance1 to verticaldist().
		set depthDistance1    to depthdist().
		set t1                to time:seconds.
		print "Lateral Difference  "+round(lateralDistance1)+" m".
		print "Vertical Difference "+round(verticalDistance1)+" m".
		print "Depth Difference    "+round(depthDistance1)+" m".
		print " ".
		print "Lateral Input  "+round(100*latinput).
		print "Vertical Input "+round(100*verticalinput).
		print "Depth Input "+round(100*depthinput).
		print " ".
		print "Depth Offset "+round(depthoffset)+" m".
		wait 0.2.
		clearscreen.
		set lateralDistance2  to lateraldist().
		set verticalDistance2 to verticaldist().
		set depthDistance2    to depthdist().
		set t2                to time:seconds.
		set dt                to t2-t1.
	}. //this is just so i can hide it easily
	////////////////////////////////////////////
	set lateralinput to lateralinputFunction().
	set verticalinput to verticalinputfunction().
	set depthinput to depthinputfunction().
	///////////////////////////////////////////
	set ship:control:starboard to lateralinput.
	set ship:control:top       to verticalinput.
	set ship:control:fore      to depthinput.
}.

set ship:control:neutralize to true.
print "Executing foreward boost maneuver".
set t0 to time:seconds.
set ship:control:fore to 1.
wait until time:seconds>t0+1.25. //average acceleration 0.4. this is 0.5 m/s dv
set ship:control:neutralize to true.

set extrah to 0.
clearscreen.
set ship:control:neutralize to true.
lock steering to prograde + r(0,0,180).
ag1 off.
unlock steering.
sas on.
wait until ship:altitude<(target:altitude-21).
until ship:altitude>(target:altitude-20) {
	print "Coasting".
	print "Altitude Difference: "+round(abs(target:altitude-ship:altitude))+" m".
	wait 0.1.
	clearscreen.
}.
set warp to 0.
sas off.
lock steering to prograde + r(0,0,180).
print "Preparing for Stationkeeping".
wait until ship:altitude>target:altitude.
set depthoffset    to depthdist().


//stationkeeping at approximately 500m
until ag1 {
	if 2 > 1 {
		set lateralDistance1  to lateraldist().
		set verticalDistance1 to verticaldist().
		set depthDistance1    to depthdist().
		set t1                to time:seconds.
		print "Lateral Difference  "+round(lateralDistance1)+" m".
		print "Vertical Difference "+round(verticalDistance1)+" m".
		print "Depth Difference    "+round(depthDistance1)+" m".
		print " ".
		print "Lateral Input  "+round(100*latinput).
		print "Vertical Input "+round(100*verticalinput).
		print "Depth Input "+round(100*depthinput).
		print " ".
		print "Depth Offset "+round(depthoffset)+" m".
		wait 0.2.
		clearscreen.
		set lateralDistance2  to lateraldist().
		set verticalDistance2 to verticaldist().
		set depthDistance2    to depthdist().
		set t2                to time:seconds.
		set dt                to t2-t1.
	}. //this is just so i can hide it easily
	////////////////////////////////////////////
	set lateralinput to lateralinputFunction().
	set verticalinput to verticalinputfunction().
	set depthinput to depthinputfunction().
	///////////////////////////////////////////
	set ship:control:starboard to lateralinput.
	set ship:control:top       to verticalinput.
	set ship:control:fore      to depthinput.

}.
ag1 off.

//initiates third closing maneuver
set maneuverspeed to 0.25.
set extrah to -1000.
ag1 off.


until ag1 {
	if 2 > 1 {
		set lateralDistance1  to lateraldist().
		set verticalDistance1 to verticaldist().
		set depthDistance1    to depthdist().
		set t1                to time:seconds.
		print "Lateral Difference  "+round(lateralDistance1)+" m".
		print "Vertical Difference "+round(verticalDistance1)+" m".
		print "Depth Difference    "+round(depthDistance1)+" m".
		print " ".
		print "Lateral Input  "+round(100*latinput).
		print "Vertical Input "+round(100*verticalinput).
		print "Depth Input "+round(100*depthinput).
		print " ".
		wait 0.2.
		clearscreen.
		set lateralDistance2  to lateraldist().
		set verticalDistance2 to verticaldist().
		set depthDistance2    to depthdist().
		set t2                to time:seconds.
		set dt                to t2-t1.
	}. //this is just so i can hide it easily
	////////////////////////////////////////////
	set lateralinput to lateralinputFunction().
	set verticalinput to verticalinputfunction().
	set depthinput to depthinputfunction().
	///////////////////////////////////////////
	set ship:control:starboard to lateralinput.
	set ship:control:top       to verticalinput.
	set ship:control:fore      to depthinput.
}.

set ship:control:neutralize to true.
print "Executing foreward boost maneuver".
set t0 to time:seconds.
set ship:control:fore to 1.
wait until time:seconds>t0+.5. //average acceleration 0.4. this is 0.25 m/s dv
set ship:control:neutralize to true.

set extrah to 0.
clearscreen.
set ship:control:neutralize to true.
lock steering to prograde + r(0,0,180).
ag1 off.
print "Coasting".
unlock steering.
sas on.
wait until ship:altitude<(target:altitude-11).
until ship:altitude>(target:altitude-10) {
	print "Coasting".
	print "Altitude Difference: "+round(abs(target:altitude-ship:altitude))+" m".
	wait 0.1.
	clearscreen.
}.
set warp to 0.
sas off.
lock steering to prograde + r(0,0,180).
print "Preparing for Stationkeeping".
wait until ship:altitude>target:altitude.
set depthoffset    to 175.

//stationkeeping at approximately 175m
set maneuverSpeed to 1.
until ag1 {
	if 2 > 1 {
		set lateralDistance1  to lateraldist().
		set verticalDistance1 to verticaldist().
		set depthDistance1    to target:position:mag.
		set t1                to time:seconds.
		print "Lateral Difference  "+round(lateralDistance1)+" m".
		print "Vertical Difference "+round(verticalDistance1)+" m".
		print "Depth Difference    "+round(depthDistance1)+" m".
		print " ".
		print "Lateral Input  "+round(100*latinput).
		print "Vertical Input "+round(100*verticalinput).
		print "Depth Input "+round(100*depthinput).
		print " ".
		print "Depth Offset "+round(depthoffset)+" m".
		wait 0.2.
		clearscreen.
		set lateralDistance2  to lateraldist().
		set verticalDistance2 to verticaldist().
		set depthDistance2    to target:position:mag.
		set t2                to time:seconds.
		set dt                to t2-t1.
	}. //this is just so i can hide it easily
	////////////////////////////////////////////
	set lateralinput to lateralinputFunction().
	set verticalinput to verticalinputfunction().
	set depthinput to depthinputfunction().
	///////////////////////////////////////////
	set ship:control:starboard to lateralinput.
	set ship:control:top       to verticalinput.
	set ship:control:fore      to depthinput.

}.
set ship:control:neutralize to true.
sas on.
ag1 off.