0 THEN D := (meas[0] - meas[1]) * Kd / Tc; ELSE D := 0; END_IF P := E[0] * Kp; IF ABS(Ki) <= 1E-100 THEN I := 0.0; ELSE deltaI := E[0] * Ki * Tc; tmp := P + D + I + deltaI + Kv * TrajectoryVelocity + Ka * AHCAcceleration; IF tmp = LIMIT(-VelocityMax, tmp, VelocityMax) THEN I := I + deltaI; END_IF END_IF _out := LIMIT(-VelocityMax, P + D + I + deltaI + TrajectoryVelocity + Kv * AHCVelocity + Ka * AHCAcceleration, VelocityMax); ELSE _out := 0; END_IF; ]]>