limit2.comp

来自「CNC 的开放码,EMC2 V2.2.8版」· COMP 代码 · 共 30 行

component limit2 "Limit the output signal to fall between min and max and limit its slew rate to less than maxv per second.  When the signal is a position, this means that position and velocity are limited.";
pin in float in;
pin out float out;
param rw float min_=-1e20;
param rw float max_=1e20;
param rw float maxv=1e20;
option data limit2_data;
function _;
license "GPL";
;;

typedef struct { float old_out; } limit2_data;

#ifndef clamp
static inline double clamp(double v, double sub, double sup) {
    if(v < sub) return sub;
    if(v > sup) return sup;
    return v;
}
#endif

FUNCTION(_) {
    double tmp = in;
    double maxdelta = maxv * fperiod;
    tmp = clamp(tmp, min_, max_);
    tmp = clamp(tmp, data.old_out - maxdelta, data.old_out + maxdelta);
    data.old_out = tmp;
    out = tmp;
}