diff --git a/boot/helicopter_debug.ks b/boot/helicopter_debug.ks index b680705..fd7c96d 100644 --- a/boot/helicopter_debug.ks +++ b/boot/helicopter_debug.ks @@ -1,6 +1,6 @@ -copypath("0:/helicopter/init", "1:/init"). +copypath("0:/lib/control", "1:/lib/control"). copypath("0:/helicopter/hover", "1:/hover"). -copypath("0:/debug", "1:/debug"). +copypath("0:/helicopter/init", "1:/init"). CORE:PART:GETMODULE("kOSProcessor"):DOEVENT("Open Terminal"). run "1:/init". diff --git a/debug/vecrender.ks b/debug/vecrender.ks index 4b46ed8..122ec87 100644 --- a/debug/vecrender.ks +++ b/debug/vecrender.ks @@ -1,25 +1,83 @@ +// requires a global list called draws, apparently @lazyglobal off. -parameter vec is false. +parameter vec is V(0,0,0). -local draws is list(). - -function DrawVectorWithComponents { +function RenderVectorWithComponents { parameter vec. + parameter prefix is "". + if prefix <> "" { + set prefix to prefix + " ". + } + + local dv is 0. + local dx is 0. + local dy is 0. + local dz is 0. + set dv to VecDraw(V(0,0,0), vec, WHITE, prefix + "v", 1.0, true). + draws:add(dv). + set dx to VecDraw(V(0,0,0), V(vec:X,0,0), RED, prefix + "x", 1.0, true). + draws:add(dx). + set dy to VecDraw(V(0,0,0), V(0,vec:Y,0), GREEN, prefix + "y", 1.0, true). + draws:add(dy). + set dz to VecDraw(V(0,0,0), V(0,0,vec:Z), BLUE, prefix + "z", 1.0, true). + draws:add(dz). +} + +function RenderVectorShipRelative { + parameter vec. + parameter prefix is "". + + if prefix <> "" { + set prefix to prefix + " ". + } + + // this seems backwards, but velocity vectors at least seem to work this way. + local axes is 0. lock axes to lookdirup(SHIP:FACING:FOREVECTOR, SHIP:UP:FOREVECTOR). + local dv is 0. + local dx is 0. + local dy is 0. + local dz is 0. + + set dv to VecDraw(V(0,0,0), vec, WHITE, prefix + "v", 1.0, true). + draws:add(dv). + + set dx to VecDraw(V(0,0,0), V(vec:X,0,0), RED, prefix + "x", 1.0, true). + draws:add(dx). + + set dy to VecDraw(V(0,0,0), V(0,vec:Y,0), GREEN, prefix + "y", 1.0, true). + draws:add(dy). + + set dz to VecDraw(V(0,0,0), V(0,0,vec:Z), BLUE, prefix + "z", 1.0, true). + draws:add(dz). +} + +function RenderDirAxes { + parameter dir. + parameter prefix is "". + + if prefix <> "" { + set prefix to prefix + " ". + } + + local dx is 0. + local dy is 0. + local dz is 0. + set dx to VecDraw(V(0,0,0), dir:RIGHTVECTOR * 100, RED, prefix + "x", 1.0, true). + draws:add(dx). + set dy to VecDraw(V(0,0,0), dir:TOPVECTOR * 100, GREEN, prefix + "y", 1.0, true). + draws:add(dy). + set dz to VecDraw(V(0,0,0), dir:FOREVECTOR * 100, BLUE, prefix + "z", 1.0, true). + draws:add(dz). +} + +if vec <> V(0,0,0) { + // RenderVectorShipRelative(vec). + RenderDirAxes(vec). +} + +function ClearRenders { set draws to list(). - local d is 0. - - set d to VecDraw(V(0,0,0), vec, WHITE, "v", 1.0, true). - list:add(d). - set d to VecDraw(V(0,0,0), V(vec:X,0,0), RED, "x", 1.0, true). - list:add(d). - set d to VecDraw(V(0,0,0), V(0,vec:Y,0), GREEN, "y", 1.0, true). - list:add(d). - set d to VecDraw(V(0,0,0), V(0,0,vec:Z), BLUE, "z", 1.0, true). - list:add(d). -} - -if vec <> false { - DrawVectorWithComponents(vec). + ClearVecDraws(). } diff --git a/helicopter/hover.ks b/helicopter/hover.ks index 22e3a42..7009d7f 100644 --- a/helicopter/hover.ks +++ b/helicopter/hover.ks @@ -1,48 +1,4 @@ // This script assumes you have bound collective (aka deploy angle) to main throttle. -runoncepath("/debug/vecrender"). // debug +runoncepath("lib/control"). -// SAS off. // debug: re-enable when adjusting heading -local pitchPID is PIDLoop(0.01, 0.001, 0.001, -45, 45). -set pitchPID:SETPOINT to 0. -local rollPID is PIDLoop(0.01, 0.001, 0.001, -70, 70). -set rollPID:SETPOINT to 0. -local collectivePID is PIDLoop(0.1, 0.1, 0.001, 0, 1). -set collectivePID:SETPOINT to 0. - - -// local x is 0. lock x to SHIP:FACING:FOREVECTOR * SHIP:VELOCITY:SURFACE. -// local y is 0. lock y to (SHIP:UP:FOREVECTOR * SHIP:VELOCITY:SURFACE) / (SHIP:VELOCITY:SURFACE:MAG * cos(VectorAngle(SHIP:UP:FOREVECTOR, SHIP:VELOCITY:SURFACE))). -// local z is 0. lock z to SHIP:FACING:RIGHTVECTOR * SHIP:VELOCITY:SURFACE. -// local spd is 0. lock spd to SHIP:VELOCITY:SURFACE:MAG. - -local done is false. -on AG9 { - set done to true. -} - -until done { - // local newPitch is pitchPID:Update(TIME:SECONDS, spd). // was x - // local newRoll is rollPid:Update(TIME:SECONDS, spd). // was z - local newThrot is collectivePID:Update(TIME:SECONDS, SHIP:VELOCITY:SURFACE:Y). // was y - - // debug - print "Yaw = " + mod(360 - SHIP:BEARING, 360). - print "Pitch = " + newPitch. - print "Roll = " + newRoll. - print "Throt = " + newThrot. - print "Vertical Speed = " + SHIP:VELOCITY:SURFACE:Y. - DrawVectorWithComponents(SHIP:VELOCITY:SURFACE). - - set SHIP:CONTROL:PILOTMAINTHROTTLE to newThrot. - // lock STEERING to Heading( - // mod(360 - SHIP:BEARING, 360), - // newPitch, - // newRoll - // ). - wait 0.001. -} - -// Because helicopters can hover stably, once we achieve a hover we should be able to turn SAS on and call it good. -unlock STEERING. -SAS on. -print "Hover operation canceled. Returning control.". +Stabilize(). diff --git a/helicopter/init.ks b/helicopter/init.ks index 91b714e..32d96b6 100644 --- a/helicopter/init.ks +++ b/helicopter/init.ks @@ -1,3 +1,7 @@ +runoncepath("lib/control"). + +global done is false. + // Main UI. local interface is gui(250, 300). set interface:X to 200. @@ -8,9 +12,19 @@ local y is interface:AddVLayout(). local x is y:AddHLayout(). set x:AddButton("HOVER"):onClick to { - run "hover". + Stabilize(). }. +set x:AddButton("LAND"):onClick to { + Stabilize(-2). +}. + + +on AG9 { + set done to true. + return true. +} + interface:show(). wait until false. diff --git a/lib/control.ks b/lib/control.ks new file mode 100644 index 0000000..79ee629 --- /dev/null +++ b/lib/control.ks @@ -0,0 +1,55 @@ +// functions that execute comprehensive control schemes. +// +// These typically assume a global variable 'done' that will be +// set externally when they should return. + + +// Hover, ascend, or descend at a fixed rate. +// Adjusts the throttle to control ascent or descent. +// Adjusts pitch and roll to maintain zero lateral velocity. +function Stabilize { + parameter vertSpeed is 0.0. + + set done to false. + SAS off. + + // set up PID controllers + local pitchPID is PIDLoop(5, 0.1, 0.01, -45, 45). + set pitchPID:SETPOINT to 0. + + local rollPID is PIDLoop(5, 0.1, 0.01, -70, 70). + set rollPID:SETPOINT to 0. + + local collectivePID is PIDLoop(0.1, 0.1, 0.001, 0, 1). + set collectivePID:SETPOINT to vertSpeed. + + // keep the craft facing a single direction so vectoring is consistent + local yaw is mod(360 - SHIP:BEARING, 360). + + // TOPVECTOR (y) is forward + // FOREVECTOR (z) is up + // RIGHTVECTOR (x) is left + lock axes to lookdirup(SHIP:UP:VECTOR, SHIP:FACING:VECTOR). + + until done { + local newPitch is pitchPID:Update(TIME:SECONDS, SHIP:VELOCITY:SURFACE * axes:TOPVECTOR). + local newRoll is rollPID:Update(TIME:SECONDS, SHIP:VELOCITY:SURFACE * axes:RIGHTVECTOR). + local newThrot is collectivePID:Update(TIME:SECONDS, SHIP:VERTICALSPEED). + + print "Forward speed: " + SHIP:VELOCITY:SURFACE * axes:TOPVECTOR. + print "Lateral speed: " + SHIP:VELOCITY:SURFACE * axes:RIGHTVECTOR. + + set SHIP:CONTROL:PILOTMAINTHROTTLE to newThrot. + lock STEERING to Heading( + yaw, + -newPitch, + newRoll + ). + wait 0.001. + } + + unlock STEERING. + SAS on. + set done to false. + print "Stabilized operation ended. Returning control to pilot.". +}