Add helicopter hovering code that works.
This commit is contained in:
parent
c5e26ab1ec
commit
6d9b8fc554
|
|
@ -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".
|
||||
|
|
|
|||
|
|
@ -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().
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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().
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
55
lib/control.ks
Normal file
55
lib/control.ks
Normal file
|
|
@ -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.".
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user