-- Basic integration. [k] is a gain; [x0] is the initial position; -- [u] the input to integrate. hybrid #pragma kcg expand #end int(k, x0, u:'a) returns (x:'a last=x0) where 'a float der x = k * u; -- the same but with [x0] and [u], two floatting point vectors of size [n] hybrid #pragma kcg expand #end int_vect<>(x0, u:'a^n) returns (x:'a^n) where 'a float x = (map int <>)(1.0^n, x0, u); hybrid reset_int(k, x0, u:'a; res:zero) returns (x:'a) where 'a float x = (restart int every res)(k, x0, u); type saturation = enum { Between, Lower, Upper }; hybrid #pragma kcg expand #end xup(x, offset:'a) returns (z:zero) where 'a float z = up(x - offset); hybrid #pragma kcg expand #end xdown(x, offset:'a) returns (z:zero) where 'a float z = up(- x + offset); -- integrator with initial condition [x0] with threshold [lower] and [upper] hybrid #pragma kcg expand #end limit_int(k, x0, upper, lower, u:'a) returns (x:'a default=x0; s:saturation) where 'a float let automaton if x0 >= upper state Upper elsif x0 <= lower state Lower else state Between end; state Between -- regular mode. Integrate the signal unless if xup(x, upper) do x = upper; restart Upper; if xdown(x, lower) do x = lower; restart Lower; let x = int(k, x0, u); s = Between; tel state Upper -- when the speed becomes negative, go to the regular mode *) unless if down u restart Between; s = Upper; state Lower -- when the speed becomes positive, go to the regular mode *) unless if up u restart Between; s = Lower; returns ..; tel hybrid filter(n, k, u:'a) returns (udot:'a) where 'a float var f:'a; let udot = n * (u - f); f = int(k, 0.0, udot); tel