17 lines
889 B
Plaintext
17 lines
889 B
Plaintext
s=10 Kp=0.11 Ki=0.00 Kd=0.8 t=3 m=1000 g=1/m p=0 goto1+:Approach>0
|
|
r=:Distance :Range=1 :Turtle=:TurtleRate :Avoidance=0 :Cruise=10
|
|
e=r-s d=e-p i+=e/(d-g) i/=2 o=Kp*e+Ki*i+Kd*d p=e r*=r<m
|
|
z/=:Approach*r*(d*d>g+e*e>4) :FCUForward=o/t :FcuBackward=-o*t goto2
|
|
:Approach=0 :FCUForward=0 :FcuBackward=0 goto1
|
|
|
|
// PID approach uses a proportional-integal-derivative controller to
|
|
// output FCU values appropriate to the distance and current speed.
|
|
// It has several tunables:
|
|
// s (setpoint) the distance we want to get to
|
|
// Kp (proportional) how much we factor in our current distance
|
|
// Ki (integral) factor of how long we've been at our distance
|
|
// Kd (derivative) factor of prediction of where we'll be in future
|
|
// t (thrust diff) account for disparity in forward/rev thrust power
|
|
// CREDIT TO WHITESTRAKE
|
|
// github.com/Archaegeo/Starbase/blob/main/Approach/ApproachPID.yolol
|