Fighting with pid loops. WIP.

This commit is contained in:
Anna Rose Wiggins 2021-07-23 16:04:26 -04:00
parent a39f5fa328
commit dd6f3d528b
7 changed files with 83 additions and 13 deletions

View file

@ -1,17 +1,54 @@
// This script assumes you have bound collective (aka deploy angle) to main throttle.
SAS off.
local pitchPID is PIDLoop(0.01, 0.01, 0.01).
local rollPID is PIDLoop(0.01, 0.01, 0.01).
local collectivePID is PIDLoop(0.01, 0.01, 0.01).
// 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 yaw is SHIP:FACING:
until SHIP:VELOCITY:SURFACE:MAG < 0.01 {
local vel is 0. lock vel to SHIP:BODY:UP * SHIP:VELOCITY:SURFACE.
set vecDraw(V(0,0,0), vel):show to true. // debug
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 {
// debug
// print x + " " + y + " " + z.
print "Vertical Speed = " + y.
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, y). // was y
// debug
print "Yaw = " + mod(360 - SHIP:BEARING, 360).
print "Pitch = " + newPitch.
print "Roll = " + newRoll.
print "Throt = " + newThrot.
print "Spd = " + spd.
LOCK throttle to
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.".