kOS TK

//TK.ks
//Hauling/Testing mission pilot

//Phase 1: Liftoff
CLEARSCREEN.
SET SHIP:CONTROL:PILOTMAINTHROTTLE TO 0.

RUN lib_pid.

SET myTh TO 0.

LOCK THROTTLE TO myTh.

SET TKPID TO PID_init( 0.05, 0.01, 0.1, 0, 1 ).

PARAMETER haul, minAlt, maxAlt, minSpd, maxSpd.

LOCAL haulboolean is FALSE.

IF haul = 1 {
	SET haulboolean TO TRUE.
} ELSE {
	SET haulboolean TO FALSE.
}

LOCAL tgtSpd is minSpd + maxSpd.
SET tgtSpd to tgtSpd/2.
PRINT "TARGET SPEED: " + tgtSpd.
PRINT "TARGET ALTITUDE: " + minAlt.
IF haulboolean = TRUE {
	PRINT "MISSION: HAUL".
} ELSE {
	PRINT "MISSION: TEST".
}


LOCAL xPos IS 17.
LOCAL zPos1 IS 15.
LOCAL zPos2 IS zPos1 + 1.
LOCAL zPos3 IS zPos1 + 2.

WHEN SHIP:ALTITUDE < minAlt THEN {
	PRINT "|---------------|" AT(xPos,zPos1).
	PRINT "|ALTITUDE:      |" AT(xPos,zPos2).
	PRINT "|---------------|" AT(xPos,zPos3).
	PRINT ROUND(SHIP:ALTITUDE) AT(xPos+11,zPos2).
	PRESERVE.
}

IF SAS {
	SAS OFF.
}


FUNCTION STG {
	STAGE.
	SET CSTG TO CSTG + 1.
}

GLOBAL CSTG IS 0.


PRINT "LAUNCHING".



LOCK STEERING TO UP.

IF CSTG = 0 AND ALT:RADAR < 500 {
	STG().
}

WHEN ALT:RADAR > 50 THEN {
	GEAR OFF.
}


UNTIL ALT:RADAR > minAlt {
	SET myTh TO PID_seek( TKPID, tgtSpd, SHIP:VELOCITY:SURFACE:MAG ).
}


WHEN SHIP:ALTITUDE > minAlt AND SHIP:ALTITUDE < maxAlt AND SHIP:VELOCITY:SURFACE:MAG > minSpd AND SHIP:VELOCITY:SURFACE:MAG < maxSpd THEN {
	PRINT "MISSION REQUIRMENTS MET".
	LOCK THROTTLE TO 0.
	IF haul = 0 {
		STG().
	}
	LOCK THROTTLE TO 0.
	WAIT 0.
	REBOOT.
}



WAIT UNTIL FALSE.