126 lines
5.2 KiB
Markdown
126 lines
5.2 KiB
Markdown
|
```
|
||
|
|
#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;
|
||
|
|
|
||
|
|
|
||
|
|
```
|