SG-22 Control

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


//////////////////// VARS //////////////////


set taralt to 100.

lock x to body:geopositionof(ship:facing:vector):heading.

lock p to 0.

lock sl to 0.

lock vtolrpmtarget to min(460,max(0,((taralt-alt:radar)-verticalspeed) * 460)).
lock pushrpmtarget to min(460,max(0,throttle * 460)).
set speedPID to pidloop(1.5, 0, 0, -5, 5).
set steeringmanager:rollcontrolanglerange to 180.


set servos to ship:modulesnamed("ModuleRoboticRotationServo").

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["mod"] to rm.
    set rotor["job"] to choose "push" if rm:part:parent:tag:contains("push") else "vtol".
	print "Found rotor module for job of " + rotor:job.
	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).

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

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

set aoatopressure to 9.

set taraoa to 4.68.
set deploycopy to 9.

set takeoff to true.
set flightmode to "vtol".

function SetAngle {
    parameter targetangle.
	if flightmode = "debug" print targetangle at (0,0).
    for s in servos if s:hasfield("target angle") s:setfield("target angle", targetangle).
}

function stop {
	print "stopping".
    lock vtolrpmtarget to throttle * 460.
    lock pushrpmtarget to 0.
	toggle ag1.
	set x2 to x.
	 lock p to -verticalspeed * 3.

	lock steering to heading(x2,p).
	lock throttle to choose 1 if verticalspeed > 0 else -verticalspeed*3.
	 set stopping to true.
	when stopping then {
		setangle(max(-90,min(90,-vdot(ship:velocity:surface,ship:facing:vector)))).
		return stopping.
	}


     when groundspeed < 10 then {
		set stopping to false.
		setangle(0).
    	ship:partsdubbedpattern("probo")[0]:controlfrom().
		lock steering to lookdirup(up:Vector*10-ship:Velocity:surface*0.1, heading(x+180,0):vector).
     	lock throttle to -verticalspeed - min(20,alt:Radar/10).
	}
}

function go {
	print "going".
    lock vtolrpmtarget to max(0,min(460,throttle * 460)).
    lock vtolrpmtarget to max(0,min(460,throttle * 460)).
	set taralt to alt:radar + 100.
	toggle ag1.
	lock p to min(15,max(-15, -verticalSpeed*1.7 + (max(taralt,alt:radar - body:geopositionof(ship:velocity:surface*10):terrainheight + taralt) - alt:radar)/2.5)).
	lock steering to heading(x, p).
	lock throttle to ship:electriccharge-100.
	set flightmode to "stol".

}

function save {
	print "waiting for valid save state".
  	when (ship:Status = "Landed" or ship:status = "Splashed") and ship:velocity:surface:mag < 0.1 then {
		print "saving".
		kuniverse:quicksaveto(ship:name).
		print "saved to '" + ship:name + "'".
	}
}

function vtoltotarget {
    parameter tgt is target:geoposition.
    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.
    lock talt to max(ship:geoposition:terrainheight + taralt, tgt:terrainheight + 20).
    SetAngle(0).
    lock vtolrpmtarget to ((-verticalspeed*1.7 + max(-10,min(20,talt-altitude)))/5)*460.
    lock pushrpmtarget to 0.

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

 	toggle ag2.
    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 max(0,min(460,vtolrpmtarget)).
    set pushrpmcopy to max(0,min(460,pushrpmtarget)).

	if ship:partstaggedpattern("Pusher"):length > 0 {
    	set speedCopy to speedPID:update(time:seconds, vdot(ship:velocity:surface, ship:partstaggedpattern("Pusher")[0]:facing:vector) - sl).
 	}
    for rotor in rotors {
        if rotor["job"] = "vtol"
        {
            rotor:mod:setfield("rpm limit", vtolrpmcopy ).
            	for blade in rotor:blades {
                    set calcAirspeed to blade["angleratio"] * vtolrpmcopy.
                    set deploycopy to max(3, taraoa + arctan2(vdot(rotor:part:parent:facing:vector,ship:Velocity:surface), calcAirspeed)).
                    blade:mod:setfield("deploy angle", deploycopy).
            	}
        }

        if rotor["job"] = "push" {
			if rotor["mode"] = "flight"  {
            	rotor:mod:setfield("rpm limit", pushrpmcopy ).
            	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).
            	}
			}
        }
    }

    if(flightmode = "stol")
    {
        SetAngle(max(0,min(90,(alt:radar/taralt)*90))).
    }


    return true.
}

when airspeed > 100 and flightmode = "stol" then {set flightmode to "plane". setangle(90). return true.}


if (ship:Status = "Landed" or ship:status = "Splashed" or ship:velocity:surface:mag < 10) {
	print "found landed or splashed, taking off".
    lock x to body:geopositionof(ship:facing:vector):heading.
    if hastarget lock x to target:heading.
    lock p to 0.
    toggle ag1.
    lock steering to heading(x,p).
    setangle(0).
    //lock throttle to (taralt-alt:radar)/10-verticalspeed.
}

if (ship:Status = "flying" and ship:velocity:surface:mag >= 10) {
	print "found flying".
	set flightmode to "plane".
	set currheading to body:geopositionof(ship:facing:vector):heading.
    lock x to currheading.
    lock p to -verticalspeed*3.
    toggle ag1.
    lock steering to heading(x,p).
    setangle(90).
    lock throttle to ship:electriccharge - 100.
	lock pushrpmtarget to min(460,max(0,throttle * 460)).
	lock vtolrpmtarget to min(460,max(0,throttle * 460)).
}