SG Hot Rod Control

for i in range(terminal:height-3) Print "":padright(terminal:width).


set steeringmanager:rollcontrolanglerange to 180.



set proprad to 5.8.
set rotormods to ship:modulesnamed("ModuleRoboticServoRotor").
set rotors to list().
for rm in rotormods {
    local rotor to lexicon().
    set rotor["part"] to rm:part.
    set rotor["job"] to choose "push" if rm:part:parent:tag:contains("push") else "vtol".
    set rotor["mod"] to rm.
	set rotor["mode"] to "flight".
    set rotor["blades"] to list().
    for bm in rm:part:ModulesNamed("ModuleControlSurface") {
        local b to bm:part.

        local blade to lexicon().
        set blade["mod"] to bm.
        set blade["offset"] to vdot(-b:Facing:starvector, b:position - b:parent:position).
        set blade["proprad"] to proprad.
        set blade["angleratio"] to 2 * constant:pi * (blade:proprad + blade:offset)/60.

	    bm:setfield("deploy", true).
        bm:setfield("deploy angle", 5).

        rotor["blades"]:add(blade).
    }

    rotors:add(rotor).
    rm:setfield("torque limit(%)", 100).
}


//set aoatopressure to 9.

//4.387475597 / body:atm:altitudepressure(4997).
// set aoatopressure to 3.33333 / body:atm:altitudepressure(12e3).
// lock taraoa to body:atm:altitudepressure(altitude) * aoatopressure.
set taralt to alt:radar + 10.
set taraoa to 4.25.
set deploycopy to 9.
set speedPID to pidloop(1.5, 0, 0, -5, 5).
// set vtolPID to pidloop(1.5, 0, 0, -2, 5).
//set vtolPID:setpoint to taralt.
lock vtolrpmtarget to min(460,max(0,((taralt-alt:radar)-verticalspeed) * 460)).
lock pushrpmtarget to min(460,max(0,throttle * 460)).
list engines in es.
lock x to 90.
lock p to 0.
lock sl to 0.
toggle ag1.

function gojets {
    for rm in rotors {
        if rm:job = "push" set rm:mode to "flight".
    }
	set taralt to 20e3.
	lock vtolrpmtarget to min(460,max(0,((taralt-altitude)-verticalspeed) * 460)).

	toggle ag1.
	lock p to min(45,max(-15, -verticalSpeed + (taralt - altitude)/10)).
	lock steering to heading(x, p).
	steeringmanager:resetpids().
	lock throttle to ship:electriccharge-100.
	for en in es en:activate().
}

function goprops {
    for rm in rotors {
        if rm:job = "push" set rm:mode to "flight".
    }
	set taralt to alt:radar + 100.
	lock vtolrpmtarget to min(460,max(0,((taralt-alt:radar)-verticalspeed) * 460)).

	toggle ag1.
	lock p to min(15,max(-15, -verticalSpeed*1.7 + (taralt - alt:radar)/2.5)).
	lock steering to heading(x, p).
	steeringmanager:resetpids().
	lock throttle to ship:electriccharge-100.
	for en in es en:shutdown().
}
function vtol {

	lock vtolrpmtarget to min(460,max(0,((taralt-alt:radar)-verticalspeed) * 460)).
	if airspeed >= 10 {
        toggle ag1.
		lock x2 to body:geopositionof(ship:facing:vector):heading.
		lock p2 to min(15,max(-15, -verticalSpeed*1.7 + (taralt - alt:radar)/2.5)).
		lock steering to heading(x2,p2).
	}
    for rm in rotors {
        if rm:job = "push" set rm:mode to "vtol".
    }
	when airspeed < 10 and sl = 0 then {
		toggle ag2.
    	lock steering to lookdirup(up:vector*10-vxcl(ship:facing:topvector,ship:velocity:surface*0.1), heading(x + 180, 0):vector).
	}

}

function stop {
	lock throttle to 0.
	if airspeed >= 10 {
        toggle ag1.
		lock x2 to body:geopositionof(ship:facing:vector):heading.
		lock p2 to min(15,max(-15, -verticalSpeed*1.7 + (taralt - alt:radar)/2.5)).
		lock steering to heading(x2,p2).
	}
    lock sl to 0.
    for rm in rotors {
        if rm:job = "push" set rm:mode to "stopping".
    }
	when airspeed < 10 then {
		toggle ag2.
    	lock steering to lookdirup(up:vector*10-ship:velocity:surface*0.1, heading(x + 180, 0):vector).
	}


}

function land {
	stop().
	when airspeed < 1 then 	lock vtolrpmtarget to min(460,max(0,(-verticalspeed - 2) * 460)).
	on status brakes on.
}

function vtoltotarget {
    parameter tgt.
    lock x to tgt:heading.

    for rm in rotors {
        if rm:job = "push" { set rm:mode to "vtol". rm:mod:setfield("torque limit(%)",0).}
    }

    when tgt:distance < 1000 then set x to tgt:heading.
    set vtoltargeting to true.
    set talt to tgt:terrainheight + 20.
    lock vtolrpmtarget to ((-verticalspeed*1.7 + max(-10,min(20,talt-altitude)))/5)*460.

    lock sl to 5.
    lock tv to tgt:position:normalized * min(sl,(tgt:distance/6) ^ 0.85).

    set relVelVec to vxcl(up:vector, tv - velocity:surface) / 3.5.
    set relVelVec:mag to min(sl,relVelVec:mag).
    set st to lookdirup(up:vector * 10 + relVelVec, heading(x+180,0):vector).
    lock steering to st.

    when true then {
        set relVelVec to vxcl(up:vector, tv - velocity:surface) / 3.5.
        set relVelVec:mag to min(sl,relVelVec:mag).
        set st to lookdirup(up:vector * 10 + relVelVec, heading(x+180,0):vector).
        return vtoltargeting.
    }

    when (ship:geoposition:position - tgt:position):mag < 0.1 and ship:groundspeed < 0.1 then {
        lock vtolrpmtarget to (-verticalspeed - 1)*460.
        wait 1.
        When ship:verticalspeed > 0 then lock vtolrpmtarget to 0.
    }

}









when true then {
    set vtolrpmcopy to vtolrpmtarget.
    set pushrpmcopy to pushrpmtarget.
    set speedCopy to speedPID:update(time:seconds, vdot(ship:velocity:surface, ship:partstagged("PusherPropBay")[0]:facing:vector) - sl).

    //set vtolCopy to vtolPID:update(time:seconds, alt:radar).

    for rotor in rotors {
        if rotor["job"] = "vtol"
        {
            rotor:mod:setfield("rpm limit", vtolrpmcopy ).
        }

        if rotor["job"] = "push" {
			if rotor["mode"] = "flight" {
            	rotor:mod:setfield("rpm limit", pushrpmtarget ).
            	for blade in rotor:blades {
                	set calcAirspeed to  blade["angleratio"] * pushrpmcopy.
                	set deploycopy to max(3, taraoa + arctan2(airspeed, calcAirspeed)).
                	blade:mod:setfield("deploy angle", deploycopy).
            	}
			}
			if rotor["mode"] = "stopping" or rotor["mode"] = "vtol" {
            	rotor:mod:setfield("rpm limit", 460 ).
            	for blade in rotor:blades {
                	set calcAirspeed to blade["angleratio"] * 460.
                	set deploycopy to speedCopy + arctan2(airspeed, calcAirspeed).
                	blade:mod:setfield("deploy angle", deploycopy).
            	}
			}
        }
    }

    return true.
}



if (ship:Status = "Landed" or ship:status = "Splashed" or ship:velocity:surface:mag < 10) {
	stop().
} else {
    gojets().
}