(* =====================================================================
   22_fb_level_control.st - FB_LEVEL_CTRL, section 4.3

   PI control, no derivative term.  Output is the common drive speed
   for every running unit.

   Direct acting: level above setpoint raises speed.

   The 38.0-50.0 Hz clamp is a hard physical limit, not a preference -
   below 38 Hz the 22 m static lift means no delivery at all.
   ===================================================================== *)

FUNCTION_BLOCK FB_LEVEL_CTRL
VAR_INPUT
    Level       : REAL;     (* m                                        *)
    Setpoint    : REAL;     (* m                                        *)
    Enable      : BOOL;
    MinSpeed    : REAL;     (* Hz, already clamped >= 38.0 by CONTROL   *)
    MaxSpeed    : REAL;     (* Hz                                       *)
END_VAR

VAR_OUTPUT
    Speed       : REAL;     (* Hz                                       *)
END_VAR

VAR CONSTANT
    SCAN_S  : REAL := 0.1;      (* must match TASK INTERVAL             *)
    KP      : REAL := 12.0;     (* Hz/m - starting gain, expect retune  *)
    TI      : REAL := 120.0;    (* s                                    *)
END_VAR

VAR
    Integ   : REAL := 38.0;     (* integrator carries the bias term     *)
    Err     : REAL;
    Raw     : REAL;
    Integrate : BOOL;
END_VAR

IF NOT Enable THEN
    (* Hold at minimum and reset the integrator, so that a restart does
       not inherit stale integral action. *)
    Integ := MinSpeed;
    Speed := MinSpeed;
ELSE
    Err := Level - Setpoint;
    Raw := KP * Err + Integ;

    (* Anti-windup: freeze the integrator whenever the output is
       clamped, except when the error would drive it back into range. *)
    Integrate := FALSE;
    IF (Raw > MinSpeed) AND (Raw < MaxSpeed) THEN
        Integrate := TRUE;
    ELSIF (Raw >= MaxSpeed) AND (Err < 0.0) THEN
        Integrate := TRUE;
    ELSIF (Raw <= MinSpeed) AND (Err > 0.0) THEN
        Integrate := TRUE;
    END_IF;

    IF Integrate THEN
        Integ := Integ + (KP / TI) * Err * SCAN_S;
    END_IF;

    Raw := KP * Err + Integ;

    IF Raw > MaxSpeed THEN
        Speed := MaxSpeed;
    ELSIF Raw < MinSpeed THEN
        Speed := MinSpeed;
    ELSE
        Speed := Raw;
    END_IF;
END_IF;

END_FUNCTION_BLOCK
