SHOW:
|
|
- or go back to the newest paste.
| 1 | set places to readjson("0:/kerbin.json").
| |
| 2 | ||
| 3 | clearVecDraws(). | |
| 4 | ||
| 5 | set mode to "initial". | |
| 6 | print "mode initial". | |
| 7 | ||
| 8 | // Hack for ground operations, I think. | |
| 9 | set craftHeight to 0. | |
| 10 | ||
| 11 | - | set circRadius to 600. |
| 11 | + | set circRadius to 2500. |
| 12 | set drawVecs to true. | |
| 13 | set landLoop to true. | |
| 14 | ||
| 15 | - | set finalEnd to places["ksc 09"]. |
| 15 | + | set finalEnd to places["runway west"]. |
| 16 | - | set finalStart to ship:body:geopositionof(finalEnd:position + (heading(-90, 0):forevector * 1500)). |
| 16 | + | set finalStart to ship:body:geopositionof(finalEnd:altitudeposition(altitude) + (heading(-90, 0):forevector * 3600)). |
| 17 | - | set vecCircleCenter to finalStart:position + V(0, circRadius, 0). |
| 17 | + | set vecCircleCenter to finalstart:altitudeposition(altitude) + heading(0, 0):forevector * circRadius. |
| 18 | set geoCircleCenter to ship:body:geopositionof(vecCircleCenter). | |
| 19 | //set vecCircleCenter:x to vecCircleCenter:x + craftHeight. | |
| 20 | set vecNorth to V(0,1,0). | |
| 21 | ||
| 22 | - | //set arrCircleCenterDir to vecdraw(V(0,0,0), geoCircleCenter:position, red, "Circle", 1, true, 0.5, true). |
| 22 | + | //set arrCircleCenterDir to vecdraw(V(0,0,0), geoCircleCenter:altitudeposition(altitude), red, "Circle", 1, true, 0.5, true). |
| 23 | //set arrCircleCenterDir to vecdraw(V(0,0,0), vecCircleCenter, yellow, "Circle", 1, true, 0.5, true). | |
| 24 | // set arrFinal to vecdraw(finalStart:position, finalEnd:position - finalStart:position, blue, "Final", 1, true, 0.5, true). | |
| 25 | ||
| 26 | - | set angCraftToTangent to arccos(circRadius/vecCircleCenter:mag). |
| 26 | + | |
| 27 | set angCraftToTangent to arccos(circRadius/geoCircleCenter:altitudeposition(altitude):mag). | |
| 28 | set angNorthToTangent to angNorthToCraft - angCraftToTangent. | |
| 29 | ||
| 30 | set vecCircleCenterToTangent to heading(angNorthtoTangent, 0, 0):forevector * circRadius. | |
| 31 | set vecCraftToTangent to vecCircleCenter + vecCircleCenterToTangent. | |
| 32 | //set vecCraftToTangent:x to craftHeight. | |
| 33 | ||
| 34 | //when drawVecs then {
| |
| 35 | ||
| 36 | set vecCraftToTangent to geoCircleCenter:position + vecCircleCenterToTangent. | |
| 37 | //set vecCraftToTangent:x to craftHeight. | |
| 38 | set geoCraftToTangent to ship:body:geopositionof(vecCraftToTangent). | |
| 39 | ||
| 40 | //wait 0. | |
| 41 | ||
| 42 | //return false. | |
| 43 | ||
| 44 | //} | |
| 45 | ||
| 46 | print "distance to circle: " + vecCircleCenter:mag. | |
| 47 | print "distance to circle (alt): " + geoCircleCenter:distance. | |
| 48 | print "circle radius: " + circRadius. | |
| 49 | ||
| 50 | print "angNorthToCraft " + angNorthToCraft. | |
| 51 | print "angNorthToCraft (alt) " + (geoCircleCenter:heading - 180). | |
| 52 | print "angCraftToTangent " + angCraftToTangent. | |
| 53 | print "angNorthToTangent " + angNorthToTangent. | |
| 54 | ||
| 55 | print "vecCircleCenterToTangent " + vecCircleCenterToTangent. | |
| 56 | print "vecCraftToTangent " + vecCraftToTangent. | |
| 57 | print "geoCraftToTangent " + geoCraftToTangent. | |
| 58 | print "distance to tangent " + geoCraftToTangent:distance. | |
| 59 | print "angle diff " + (geoCircleCenter:heading - geoCraftToTangent:heading). | |
| 60 | ||
| 61 | - | lock wheelsteering to targetHeading. |
| 61 | + | |
| 62 | - | lock wheelthrottle to 0.20. |
| 62 | + | set hdg to targetHeading. |
| 63 | - | brakes off. |
| 63 | + | //lock wheelsteering to targetHeading. |
| 64 | //lock wheelthrottle to 0.20. | |
| 65 | //brakes off. | |
| 66 | ||
| 67 | print "before vecs". | |
| 68 | //set arrTargetDir to vecdraw(V(0,0,0), { return geoCraftToTangent:position. }, red, "Heading", 1, true, 0.5, true).
| |
| 69 | //set arrTangent to vecdraw({ return geoCircleCenter:position. }, { return vecCircleCenterToTangent. }, yellow, "Tangent", 1, true, 0.5, true).
| |
| 70 | print "after vecs". | |
| 71 | ||
| 72 | when landLoop = true then {
| |
| 73 | - | set angCraftToTangent to arccos(circRadius/vecCircleCenter:mag). |
| 73 | + | |
| 74 | set angNorthToCraft to (geoCircleCenter:heading - 180). | |
| 75 | set angCraftToTangent to arccos(circRadius/geoCircleCenter:altitudeposition(altitude):mag). | |
| 76 | set angNorthToTangent to angNorthToCraft - angCraftToTangent. | |
| 77 | - | set vecCraftToTangent to vecCircleCenter + vecCircleCenterToTangent. |
| 77 | + | |
| 78 | set vecCircleCenterToTangent to heading(angNorthtoTangent, 0, 0):forevector * circRadius. | |
| 79 | set vecCraftToTangent to geoCircleCenter:altitudeposition(altitude) + vecCircleCenterToTangent. | |
| 80 | set geoCraftToTangent to ship:body:geopositionof(vecCraftToTangent). | |
| 81 | ||
| 82 | - | print targetHeading. |
| 82 | + | |
| 83 | set targetHeading to geoCraftToTangent:heading. | |
| 84 | - | if vang(ship:facing:forevector, geoCraftToTangent:position) < 5 {
|
| 84 | + | set hdg to targetHeading. |
| 85 | print round(targetHeading, 2) + " " + round(abs(geoCraftToTangent:bearing), 2). | |
| 86 | ||
| 87 | if abs(geoCraftToTangent:bearing) < 5 {
| |
| 88 | set mode to "locked". | |
| 89 | print "mode locked". | |
| 90 | } | |
| 91 | - | set vecCraftToTangent to vecCircleCenter + vecCircleCenterToTangent. |
| 91 | + | |
| 92 | if mode = "locked" {
| |
| 93 | set vecCircleCenterToTangent to heading(angNorthtoTangent, 0, 0):forevector * circRadius. | |
| 94 | - | //print targetHeading + " " + geoCraftToTangent:distance. |
| 94 | + | set vecCraftToTangent to geoCircleCenter:altitudeposition(altitude) + vecCircleCenterToTangent. |
| 95 | ||
| 96 | set targetHeading to geoCraftToTangent:heading. | |
| 97 | - | if vecCraftToTangent:mag < 20 or vang(ship:facing:forevector, geoCraftToTangent:position) > 45 {
|
| 97 | + | set hdg to targetHeading. |
| 98 | //print round(vecCraftToTangent:mag) + " " + round(geoCraftToTangent:distance) + " " + round(geoCraftToTangent:bearing, 2). | |
| 99 | ||
| 100 | // Close or gone past it. | |
| 101 | if vecCraftToTangent:mag < 20 or abs(geoCraftToTangent:bearing) > 45 {
| |
| 102 | set mode to "turning". | |
| 103 | print "mode turning". | |
| 104 | set lastupdate to time:seconds. | |
| 105 | //set arrTargetDir:show to false. | |
| 106 | ||
| 107 | //set arrTangent to vecdraw({ return geoCircleCenter:position. }, { return heading(geoCircleCenter:heading - 180, 0, 0):forevector * circRadius. }, yellow, "Tangent", 1, true, 0.5, true, false).
| |
| 108 | ||
| 109 | //SET Kp TO 0.5. | |
| 110 | //SET Ki TO 0.6. | |
| 111 | //SET Kd TO 0.6. | |
| 112 | //SET PID TO PIDLOOP(Kp, Ki, Kd). | |
| 113 | //SET PID:SETPOINT TO circRadius. | |
| 114 | - | if lastUpdate < time:seconds + 5 {
|
| 114 | + | |
| 115 | } | |
| 116 | - | set targetHeading to (geoCircleCenter:heading + 90) - 0.30. |
| 116 | + | |
| 117 | if mode = "turning" {
| |
| 118 | if lastUpdate < time:seconds + 3 {
| |
| 119 | - | print geoCircleCenter:distance + " >? " + circRadius + " " + targetHeading + " heading " + geoCircleCenter:heading + " => " + (geoCircleCenter:heading + 90). |
| 119 | + | |
| 120 | set targetHeading to (geoCircleCenter:heading + 90) - 15. | |
| 121 | set hdg to targetHeading. | |
| 122 | ||
| 123 | } | |
| 124 | if geoCircleCenter:distance > (circRadius*1.05) {
| |
| 125 | set targetHeading to (geoCircleCenter:heading + 90) - 18. | |
| 126 | set hdg to targetHeading. | |
| 127 | ||
| 128 | } | |
| 129 | if geoCircleCenter:distance > (circRadius * 1.25) {
| |
| 130 | set targetHeading to (geoCircleCenter:heading + 90) - 25. | |
| 131 | set hdg to targetHeading. | |
| 132 | ||
| 133 | } | |
| 134 | if geoCircleCenter:distance > (circRadius * 1.5) {
| |
| 135 | set targetHeading to (geoCircleCenter:heading + 90) - 35. | |
| 136 | set hdg to targetHeading. | |
| 137 | ||
| 138 | } | |
| 139 | set lastupdate to time:seconds. | |
| 140 | print round(geoCircleCenter:distance, 2) + " >? " + round(circRadius, 2) + " " + round(targetHeading, 2) + " hdg " + round(geoCircleCenter:heading, 2) + " => " + round(geoCircleCenter:heading + 90, 2). | |
| 141 | } | |
| 142 | //SET targetHeading TO targetHeading + PID:UPDATE(TIME:SECONDS, geoCircleCenter:position:mag). | |
| 143 | ||
| 144 | if finalStart:distance < 10 or (finalStart:distance < (circRadius / 4) and abs(finalStart:BEARING) > 120) {
| |
| 145 | set mode to "final". | |
| 146 | print "mode final". | |
| 147 | print finalstart:distance. | |
| 148 | print finalstart:bearing. | |
| 149 | //set arrTangent:show to false. | |
| 150 | //set arrTargetDir to vecdraw(V(0,0,0), { return finalEnd:position. }, red, "Heading", 1, true, 0.5, true, false).
| |
| 151 | when alt:radar < 15 then { hs(0). } when alt:radar < 10 then { hv(10). } hs(80). hv(-6). on ship:status { brakes on. hs(0). for p in ship:partsnamedpattern("wheel") { p:getModule("ModuleWheelBrakes"):SetField("brakes", 200). } }
| |
| 152 | } | |
| 153 | } | |
| 154 | if mode = "final" {
| |
| 155 | set targetHeading to finalEnd:heading. | |
| 156 | set hdg to targetHeading. | |
| 157 | ||
| 158 | if finalEnd:distance < 10 or (finalEnd:distance < (circRadius / 4) and abs(finalEnd:BEARING) > 120) {
| |
| 159 | set mode to "landing". | |
| 160 | print "mode landing". | |
| 161 | print finalend:distance. | |
| 162 | print finalend:bearing. | |
| 163 | brakes on. | |
| 164 | } | |
| 165 | } | |
| 166 | if mode = "landing" {
| |
| 167 | set hdg to places["runway west"]:heading. | |
| 168 | ||
| 169 | } | |
| 170 | ||
| 171 | return landLoop. | |
| 172 | } | |
| 173 | ||
| 174 | //wait until false. | |
| 175 |