Ozin

VTOL hover experimental

Sep 16th, 2020 (edited)
587
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. //VTOL hover script with side translation - Ozin
  2. // !runscript xeNy4xmv
  3.  
  4. set bnds to ship:bounds.
  5. HUDTEXT("Running script: VTOL hover for jets",20,2,22,white,false).
  6. set steeringmanager:rollcontrolanglerange to 180.
  7. set steeringmanager:rollpid:kp to 0.4.
  8. set steeringmanager:rollpid:ki to 0.04.
  9. set steeringmanager:rollpid:kd to 0.1.
  10. set st to  lookdirup(up:vector, facing:topvector).
  11. lock steering to st.
  12. lock hdg to 90. lock sl to 0.
  13. set relVelVec to v(0,0,0).
  14.  
  15. set servos to ship:modulesnamed("ModuleRoboticRotationServo").
  16. for s in servos { s:setfield("damping", 80). s:setfield("traverse rate", 40). }
  17. set rightHinges to list().
  18. set leftHinges to list().
  19. for p in ship:partstagged("rightHinge") rightHinges:add(p:getmodule("ModuleRoboticServoHinge")).
  20. for p in ship:partstagged("leftHinge") leftHinges:add(p:getmodule("ModuleRoboticServoHinge")).
  21. for s in rightHinges { s:setfield("damping", 30). s:setfield("traverse rate", 40). }
  22. for s in leftHinges { s:setfield("damping", 30). s:setfield("traverse rate", 40). }
  23.  
  24. set twoaxistilt to (rightHinges:length > 0 and leftHinges:length > 0).
  25. if twoaxistilt HUDTEXT("2-axis engine tilting enabled. Toggle 'twoaxistilt' to disable",20,2,22,yellow,false).
  26.  
  27. list engines in engs.
  28. function vertThrust {
  29.   set vth to 0.
  30.   for e in engs if e:ignition set vth to vth + vdot(up:vector, e:facing:vector * (choose -1 if e:tag = "reverse" else 1)) * e:availablethrust.
  31.   return max(1,vth).
  32. }
  33. function norm {
  34.     parameter p0 is v(0,0,0).
  35.     set u to (p0-body:position):normalized.
  36.     set p1 to vxcl(u,facing:vector):normalized * (bnds:size:mag + 5).
  37.     set g2 to body:geopositionof(angleaxis(120, u) * p1 + p0). set p2 to choose g2:position if g2:terrainheight > 0 else g2:altitudeposition(0).
  38.     set g3 to body:geopositionof(angleaxis(-120, u) * p1 + p0). set p3 to choose g3:position if g3:terrainheight > 0 else g3:altitudeposition(0).
  39.     set g1 to body:geopositionof(p1 + p0). set p1 to choose g1:position if g1:terrainheight > 0 else g1:altitudeposition(0).
  40.     return vcrs(p1-p2,p1-p3):normalized.
  41. }
  42. function getSlope {
  43.     parameter pos, vec.
  44.     local g1 is body:geopositionof(pos). set p1 to choose g1:position if g1:terrainheight > 0 else g1:altitudeposition(0).
  45.     local g2 is body:geopositionof(pos+vec). set p2 to choose g2:position if g2:terrainheight > 0 else g2:altitudeposition(0).
  46.     return 90 - vang(up:vector, p2-p1).
  47. }
  48. set jets to false.
  49. for e in engs if e:sealevelisp > 5000 jets on.
  50.  
  51. //PIDS
  52. if jets {
  53.     set tpid to pidloop(8,0.2, 9, -0.5, 4).
  54. } else {
  55.     set tpid to pidloop(5,0.2, 0.5, -1, 4).
  56. }
  57. set vsPID to pidloop(1,1,1,-50,100).
  58. set tltpid to pidloop(20, 2, 2, -45, 65).
  59. set tltStarpid to pidloop(20, 2, 2, -35, 35).
  60.  
  61.  
  62. lock tAltOverride to 0.
  63. set terSlope to 0.
  64. lock slopeVS to choose 0 if tAltOverride else tan(terSlope) * groundspeed.
  65.  
  66. set twr to 1.
  67. set grav to 9.
  68. set acc to 10.
  69. lock height to 20.
  70.  
  71. lock terAlt to max(0,max(ship:geoposition:terrainheight, body:geopositionof(velocity:surface * 1):terrainheight)).
  72. lock talt to choose tAltOverride if tAltOverride <> 0 else height + terAlt.
  73.  
  74.  
  75. lock vsOverride to 0.
  76. set vsMin to -100.
  77. set vsMax to 200.
  78. set vs to 0.
  79. set vsErr to 0.
  80.  
  81. //slow trigger
  82. on round(time:seconds * 5) {
  83.     set grav to body:mu/body:position:sqrmagnitude.
  84.     set curThrust to vertThrust().
  85.     set twr to (curThrust/mass)/grav.
  86.     set acc to max(0.1, curThrust/mass - grav).
  87.     set tpid:maxoutput to twr.
  88.     set tpid:minoutput to (choose -0.5 if altitude - talt > -1 else -1).
  89.     return true.
  90. }
  91.  
  92.  
  93. //fast trigger
  94. when true then {
  95.     set vs to choose vsOverride if vsOverride <> 0 else max(vsMin,min(vsMax,      choose (-1 * (1 * acc * max(altitude - talt,0.0001))^0.4) if altitude > talt else sqrt(2 * (grav/2) * max(talt - altitude - verticalspeed * 0.2,0.0001))         )) + (choose slopeVS if slopeVS > 0 else slopeVS / 2).
  96.     set vsErr to verticalspeed - vs.
  97.     if vsErr < -10 tPid:reset().
  98.     return not(abort).
  99. }
  100. lock mul to 1 + tpid:update(time:seconds,vsErr).
  101. lock throttle to mul/twr.
  102.  
  103. set tiltOutwards to 5.
  104. if ship:partsnamedpattern("grap"):length > 0 {
  105.     set localport to ship:partsnamedpattern("grap")[0].
  106. } else if ship:dockingports:length > 0 {
  107.     set localport to ship:dockingports[0].
  108. } else {
  109.     set localport to false.
  110. }
  111.  
  112.  
  113. function tiltStar {
  114.     parameter tlt is 0.
  115.     if vsErr < -1 set tlt to tlt + tlt * max(-1, vsErr / 15).
  116.     set tlt to tlt + vang(up:vector,  facing:starvector) - 90.  
  117.     for t in leftHinges t:setfield("target angle", -tlt + tiltOutwards).
  118.     for t in rightHinges t:setfield("target angle", tlt + tiltOutwards).
  119. }
  120. function tilt {
  121.     parameter tlt is 0. if vsErr < -1  and vdot(velocity:surface:normalized, relVelVec) > -2 set tlt to tlt + tlt * max(-1, vsErr / 20).
  122.     set tlt to tlt + vang(up:vector,  facing:topvector) - 90.
  123.     for s in servos s:setfield("target angle", tlt).
  124. }
  125.  
  126.  
  127. set mode to "Heading".
  128. set tgt to false.
  129. function goToTarget {
  130.     if localport:istype("part") {
  131.         lock tv to choose (vxcl(up:vector,target:position - localport:position):normalized * min(min(airspeed + 5,sl),(vxcl(up:vector,target:position - localport:position):mag/3)^0.70)) if hastarget else v(0,0,0).
  132.        
  133.     } else {
  134.         lock tv to choose (vxcl(up:vector,target:position):normalized * min(min(airspeed + 5,sl),(vxcl(up:vector,target:position):mag/3)^0.70)) if hastarget else v(0,0,0).
  135.     }
  136.     set mode to "Target".
  137.     if sl < 1 { set sl to 5. HUDTEXT("SL (speedlimit) is low, setting SL to 5..",16,2,20,yellow,false). }
  138. }
  139.  
  140. function goToPosition {
  141.     parameter pos.
  142.     if pos:typename = "Vector" set pos to body:geopositionof(pos).
  143.     set tgt to pos.
  144.     lock tv to vxcl(up:vector,tgt:position):normalized * min(min(airspeed + 5,sl),(vxcl(up:vector,tgt:position):mag/3)^0.70) - (choose localport:position if localport:istype("part") else v(0,0,0)).
  145.     set mode to "GeoPos".
  146.     if sl < 1 { set sl to 5. HUDTEXT("SL (speedlimit) is low, setting SL to 5..",16,2,20,yellow,false). }
  147. }
  148.  
  149. function resetSteering { lock tv to heading(hdg,0):vector * sl. lock steering to st. set mode to "Heading". }
  150.  
  151.  
  152. set terI to 0. set terSteps to 10.
  153.  
  154.  
  155. set alignTerrainHeight to 10.
  156. lock tv to heading(hdg,0):vector * sl.
  157.  
  158. set sideTiltMult to choose 0.50 if twoaxistilt else 1.
  159. on round(time:seconds * 12) {
  160.     set tempSlope to getSlope(velocity:surface * terI, velocity:surface * 0.5).
  161.     set terSlope to choose tempSlope if tempSlope > terSlope else terSlope * 0.99 + 0.01 * tempSlope.
  162.     set terI to terI + 1.
  163.     if terI >= terSteps set terI to 0.
  164.    
  165.     set relVelVec to vxcl(up:vector, tv - velocity:surface) / 4.
  166.     set relVelVec:mag to min(5,relVelVec:mag).
  167.     tilt(tltpid:update(time:seconds, vdot(facing:topvector, relVelVec))).
  168.  
  169.     if twoaxistilt {
  170.         tiltStar(tltStarpid:update(time:seconds, vdot(facing:starvector, relVelVec))).
  171.         set st to lookdirup(velocity:surface / -500 + (choose norm() if alt:radar < alignTerrainHeight else up:vector) * 10 + vxcl(vxcl(up:vector,facing:topvector),relVelVec * sideTiltMult ), (choose tv * -1 if tv:mag > 2 else (choose -target:facing:topvector if hastarget else facing:topvector))).
  172.     } else {
  173.         set st to lookdirup(velocity:surface / -500 + up:vector * 10 + vxcl(vxcl(up:vector,facing:topvector),relVelVec * sideTiltMult ),(choose tv * -1 if tv:mag > 2 else (choose -target:facing:topvector if hastarget else facing:topvector))).
  174.     }
  175.     return not(abort).
  176. }
  177.  
  178.  
  179.  
  180. set aftEngs to ship:partstagged("aft").
  181. if aftEngs:length > 0 {
  182.     when true then {
  183.         for eng in aftEngs
  184.             set eng:thrustlimit to choose 0 if tv:mag < 10  else vdot(eng:facing:vector, tv-velocity:surface) * 30.
  185.         return true.
  186.     }
  187. }
  188.  
  189. if ship:availablethrust = 0 stage.
  190. if vang(up:vector,facing:vector) > 60 toggle ag2.
  191.  
  192.  
  193. set trackCam to true.
  194. set pitch to 12.
  195. lock co to v(0,0,0).
  196. set cam to addons:camera:flightcamera.
  197. when trackCam then {
  198.     set cam:position to cam:position * 0.9 + 0.1 * (co + angleaxis(pitch, facing:starvector) * (-velocity:surface / 50 + (-relVelVec/10 + facing:topvector):normalized * bnds:size:mag * 1.2)).
  199.     return lights.
  200. }
  201.  
  202. clearguis().
  203. set menu to gui(200, 30). Set menu:x to -30. set menu:y to 300. menu:show.
  204. menu:addlabel("<b><color=yellow>VTOL 2-axis script</color></b>").
  205. set m0 to menu:addlabel("Mode:").
  206. set m1 to menu:addlabel("SL:").
  207. set m2 to menu:addlabel("HDG:").
  208. set m3 to menu:addlabel("Height:").
  209. set m4 to menu:addlabel("tAltOverride:").
  210. set m5 to menu:addlabel("vsOverride:").
  211. set m6 to menu:addlabel("alt:radar:").
  212. set m7 to menu:addlabel("trackCam:").
  213. set m8 to menu:addlabel("alignTerrainHeight:").
  214. set m9 to menu:addlabel("tiltOutwards:").
  215. set m10 to menu:addlabel("Localport:").
  216. set mLast to menu:addlabel("Functions: <color=white>goToTarget(), goToPosition(<geopos or position>), resetSteering()</color>").
  217. set mLast2 to menu:addlabel("Additional vars: <color=white>minVS, maxVS, sideTiltMult</color>").
  218. set mLast3 to menu:addlabel("").
  219.  
  220. for txt in menu:widgets { set txt:style:fontsize to 10. set txt:style:padding:v to 1. }
  221. set m3:style:margin:top to 15.
  222. set m5:style:margin:bottom to 15.
  223. on time:second {
  224.     set m0:text to "Mode: <color=yellow>" + mode + "</color>".
  225.     set m1:text to "SL: <color=white>" + sl + "m/s</color>".
  226.     set m2:text to "HDG: <color=white>" + hdg + "</color>".
  227.    
  228.     set m3:text to (choose "<color=#222>Height: </color>" if vsOverride <> 0 or tAltOverride <> 0 else "Height: ") + "<color=white>" + height + "</color> (priority #3)".
  229.     set m4:text to (choose "<color=#222>tAltOverride: </color>" if vsOverride <> 0 or tAltOverride = 0 else "tAltOverride: ")+ "<color=white>" + tAltOverride + "</color> (priority #2 if <> 0)".
  230.     set m5:text to (choose "<color=#222>vsOverride: </color>" if vsOverride = 0  else "vsOverride: ") + "<color=white>" + vsOverride + "</color> (priority #1 if <> 0)".
  231.    
  232.     set m7:text to "TrackCam: <color=white>" + trackCam + "</color>".
  233.     set m8:text to "alignTerrainHeight: <color=white>" + alignTerrainHeight + "m</color> (match slope if height below this value)".
  234.     set m9:text to "tiltOutwards: <color=white>" + tiltOutwards + "</color>".
  235.     set m10:text to "Localport: <color=white>" + localport + "</color>".
  236.     return menu:visible.
  237. }
  238. when true then {
  239.     set m6:text to "Alt:radar: " + round(alt:radar,2) + "m".
  240.     set mLast3:text to "vs err: " + round(verticalspeed - vs,3) + ", cur: " + round(verticalspeed,3).
  241.     return menu:visible.
  242. }
  243.  
RAW Paste Data