Obsidean_VM/04-InLavoro/22 - 9.3841 - Sidel - Tilting/Software/FC103 - FC Simple PID.md

#Sensor.Aux.#TimeFromRUN := TIME_TO_DINT(TIME_TCK());
IF #Sensor.Aux.#TimeFromRUN < #Sensor.Aux.#TimeRecorded THEN
    #Sensor.Aux.#TimeRecorded := #Sensor.Aux.#TimeFromRUN;
END_IF;

// Time from last call
#DeltaTime := #Sensor.Aux.#TimeFromRUN - #Sensor.Aux.#TimeRecorded;
#Sensor.Aux.#TimeRecorded := #Sensor.Aux.#TimeFromRUN;

IF #DeltaTime = 0 THEN
    RETURN;
END_IF;

// PID Logic    
IF #Sensor."Calculate Correction" THEN
    IF #Sensor.PID."Enable PID" THEN
        // Calculate Error
        IF #Sensor.PID."Type of Correction" = #type_Inverse THEN
            #Error := #Sensor.PID.Value - #Sensor.PID.SetPoint;
        ELSIF #Sensor.PID."Type of Correction" = #type_Positive THEN
            #Error := #Sensor.PID.SetPoint - #Sensor.PID.Value;
        END_IF;
        
        // Calculate the proportional component
        #Proportional := #Sensor.PID.Kp * #Error;
        
        // Calculate the integral component
        IF #Sensor.PID.Ki > 0 THEN
            // Update the integral sum with exponential decay
            IF #Sensor.PID."Integral Time" > 0 THEN
                #DecayFactor := EXP(-DINT_TO_REAL(#DeltaTime) / (#Sensor.PID."Integral Time" * 1000.0));
            ELSE
                #DecayFactor := 1.0;
            END_IF;
            
            #Sensor.Aux.#IntegralSum := #Sensor.Aux.#IntegralSum * #DecayFactor + #Error * (DINT_TO_REAL(#DeltaTime) / 1000.0);
            
            // Check the % on I Regulation
            #"Max I Regulation Scaled" := (#Sensor.PID."Out Scale"."In Max" - #Sensor.PID."Out Scale"."In Min") * #Sensor.PID."Max I Regulation %" / (100.0 * #Sensor.PID.Ki);
            
            IF #Sensor.Aux.#IntegralSum > #"Max I Regulation Scaled" THEN
                #Sensor.Aux.#IntegralSum := #"Max I Regulation Scaled";
            END_IF;
            
            IF #Sensor.Aux.#IntegralSum < -#"Max I Regulation Scaled" THEN
                #Sensor.Aux.#IntegralSum := -#"Max I Regulation Scaled";
            END_IF;
            
            IF NOT (#Sensor.Aux.#IntegralSum > 0 OR #Sensor.Aux.#IntegralSum = 0 OR #Sensor.Aux.#IntegralSum < 0) THEN
                // Reset Infinit or NaN
                #Sensor.Aux.#IntegralSum := 0;
            END_IF;
            
            // Integral Ki
            #Integral := #Sensor.PID.Ki * #Sensor.Aux.#IntegralSum;
        ELSE
            #Integral := 0;
        END_IF;
        
        // Calculate the derivative component
        // DeltaTime > 0 is checked before
        #Derivative := #Sensor.PID.Kd * (#Error - #Sensor.Aux.#PreviousError) / (DINT_TO_REAL(#DeltaTime) / 1000.0);
        
        // Calculate the output value
        #Sensor.PID.Output := (#Sensor.PID.SetPoint * #Sensor.PID.FFk) + #Sensor.PID.FFoffset + #Proportional + #Integral + #Derivative;
        
        // Update the previous error for Kd
        #Sensor.Aux.#PreviousError := #Error;
        
    ELSIF #Sensor.PID.SetOutputDisabled THEN
        #Sensor.Aux.#IntegralSum := 0;
        #Sensor.Aux.#PreviousError := 0;
        IF (#Sensor.PID."Output Disabled Ramp s" * 1000) * DINT_TO_REAL( #DeltaTime) = 0 THEN
            #Sensor.PID.Output := #Sensor.PID."Output Disabled";
        ELSE
            #UpdateFactor := (#Sensor.PID."Out Scale"."In Max" - #Sensor.PID."Out Scale"."In Min") / (#Sensor.PID."Output Disabled Ramp s" * 1000) * DINT_TO_REAL( #DeltaTime);
            IF ABS(#Sensor.PID."Output Disabled" - #Sensor.PID.Output) <= #UpdateFactor THEN
                #Sensor.PID.Output := #Sensor.PID."Output Disabled";
            ELSIF #Sensor.PID."Output Disabled" < #Sensor.PID.Output THEN
                #Sensor.PID.Output := #Sensor.PID.Output - #UpdateFactor;
            ELSE
                #Sensor.PID.Output := #Sensor.PID.Output + #UpdateFactor;
            END_IF;
        END_IF;
    END_IF;
    
    IF NOT (#Sensor.PID.Output < 0 OR #Sensor.PID.Output > 0 OR #Sensor.PID.Output = 0) THEN
        // Division by Zero
        #Sensor.PID.Output := 0;
    END_IF;
    
    IF #Sensor.PID."Actuator State" = #State_ManOpen THEN
        // Set the Manual Value
        "FC Scaling"(INPUT:=#Sensor.PID."Manual Output %",
                     IN_MIN:=0,
                     IN_MAX:=100,
                     OUT_MIN:=#Sensor.PID."Out Scale"."In Min",
                     OUT_MAX:=#Sensor.PID."Out Scale"."In Max",
                     OUTPUT=>#Sensor.PID."Output");
    ELSIF #Sensor.PID."Actuator State" = #State_ManClose THEN
        // Close the Actuator Manually
        #Sensor.PID."Output" := 0;
    END_IF;
  
    
    "FC Scale Real"(INPUT:=#Sensor.PID.Output,
                 IN_MIN:=#Sensor.PID."Out Scale"."In Min",
                 IN_MAX:=#Sensor.PID."Out Scale"."In Max",
                 OUT_MIN:=0,
                 OUT_MAX:=100,
                 OUTPUT=>#Sensor.PID."Output %");
    
     "FC Scale Real"(INPUT:=#Sensor.PID.Output,
                 IN_MIN:=#Sensor.PID."Out Scale"."In Min",
                 IN_MAX:=#Sensor.PID."Out Scale"."In Max",
                 OUT_MIN:=#Sensor.PID."Out Scale"."Out Min",
                 OUT_MAX:=#Sensor.PID."Out Scale"."Out Max",
                 OUTPUT=>#"Direct Scaled Value");
    
    #Sensor.PID."Output Scaled" := #"Direct Scaled Value" + #Sensor.PID."Out Scale"."Out Offset";
    
END_IF;