first creation of PID controller + adjustments to pumpingstation
This commit is contained in:
znetsixe
279
src/pid/PIDController.js
Normal file
279
src/pid/PIDController.js
Normal file
@@ -0,0 +1,279 @@
|
||||
'use strict';
|
||||
|
||||
/**
|
||||
* Discrete PID controller with optional derivative filtering and integral limits.
|
||||
* Sample times are expressed in milliseconds to align with Node.js timestamps.
|
||||
*/
|
||||
class PIDController {
|
||||
constructor(options = {}) {
|
||||
const {
|
||||
kp = 1,
|
||||
ki = 0,
|
||||
kd = 0,
|
||||
sampleTime = 1000,
|
||||
derivativeFilter = 0.15,
|
||||
outputMin = Number.NEGATIVE_INFINITY,
|
||||
outputMax = Number.POSITIVE_INFINITY,
|
||||
integralMin = null,
|
||||
integralMax = null,
|
||||
derivativeOnMeasurement = true,
|
||||
autoMode = true
|
||||
} = options;
|
||||
|
||||
this.kp = 0;
|
||||
this.ki = 0;
|
||||
this.kd = 0;
|
||||
|
||||
this.setTunings({ kp, ki, kd });
|
||||
this.setSampleTime(sampleTime);
|
||||
this.setOutputLimits(outputMin, outputMax);
|
||||
this.setIntegralLimits(integralMin, integralMax);
|
||||
this.setDerivativeFilter(derivativeFilter);
|
||||
|
||||
this.derivativeOnMeasurement = Boolean(derivativeOnMeasurement);
|
||||
this.autoMode = Boolean(autoMode);
|
||||
|
||||
this.reset();
|
||||
}
|
||||
|
||||
/**
|
||||
* Update controller gains at runtime.
|
||||
* Accepts partial objects, e.g. setTunings({ kp: 2.0 }).
|
||||
*/
|
||||
setTunings({ kp = this.kp, ki = this.ki, kd = this.kd } = {}) {
|
||||
[kp, ki, kd].forEach((gain, index) => {
|
||||
if (!Number.isFinite(gain)) {
|
||||
const label = ['kp', 'ki', 'kd'][index];
|
||||
throw new TypeError(`${label} must be a finite number`);
|
||||
}
|
||||
});
|
||||
|
||||
this.kp = kp;
|
||||
this.ki = ki;
|
||||
this.kd = kd;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the controller execution interval in milliseconds.
|
||||
*/
|
||||
setSampleTime(sampleTimeMs = this.sampleTime) {
|
||||
if (!Number.isFinite(sampleTimeMs) || sampleTimeMs <= 0) {
|
||||
throw new RangeError('sampleTime must be a positive number of milliseconds');
|
||||
}
|
||||
|
||||
this.sampleTime = sampleTimeMs;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constrain controller output.
|
||||
*/
|
||||
setOutputLimits(min = this.outputMin, max = this.outputMax) {
|
||||
if (!Number.isFinite(min) && min !== Number.NEGATIVE_INFINITY) {
|
||||
throw new TypeError('outputMin must be finite or -Infinity');
|
||||
}
|
||||
if (!Number.isFinite(max) && max !== Number.POSITIVE_INFINITY) {
|
||||
throw new TypeError('outputMax must be finite or Infinity');
|
||||
}
|
||||
if (min >= max) {
|
||||
throw new RangeError('outputMin must be smaller than outputMax');
|
||||
}
|
||||
|
||||
this.outputMin = min;
|
||||
this.outputMax = max;
|
||||
this.lastOutput = this._clamp(this.lastOutput ?? 0, this.outputMin, this.outputMax);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constrain the accumulated integral term.
|
||||
*/
|
||||
setIntegralLimits(min = this.integralMin ?? null, max = this.integralMax ?? null) {
|
||||
if (min !== null && !Number.isFinite(min)) {
|
||||
throw new TypeError('integralMin must be null or a finite number');
|
||||
}
|
||||
if (max !== null && !Number.isFinite(max)) {
|
||||
throw new TypeError('integralMax must be null or a finite number');
|
||||
}
|
||||
if (min !== null && max !== null && min > max) {
|
||||
throw new RangeError('integralMin must be smaller than integralMax');
|
||||
}
|
||||
|
||||
this.integralMin = min;
|
||||
this.integralMax = max;
|
||||
this.integral = this._applyIntegralLimits(this.integral ?? 0);
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Configure exponential filter applied to the derivative term.
|
||||
* Value 0 disables filtering, 1 keeps the previous derivative entirely.
|
||||
*/
|
||||
setDerivativeFilter(value = this.derivativeFilter ?? 0) {
|
||||
if (!Number.isFinite(value) || value < 0 || value > 1) {
|
||||
throw new RangeError('derivativeFilter must be between 0 and 1');
|
||||
}
|
||||
|
||||
this.derivativeFilter = value;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Switch between automatic (closed-loop) and manual mode.
|
||||
*/
|
||||
setMode(mode) {
|
||||
if (mode !== 'automatic' && mode !== 'manual') {
|
||||
throw new Error('mode must be either "automatic" or "manual"');
|
||||
}
|
||||
|
||||
this.autoMode = mode === 'automatic';
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Force a manual output (typically when in manual mode).
|
||||
*/
|
||||
setManualOutput(value) {
|
||||
this._assertNumeric('manual output', value);
|
||||
this.lastOutput = this._clamp(value, this.outputMin, this.outputMax);
|
||||
return this.lastOutput;
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset dynamic state (integral, derivative memory, timestamps).
|
||||
*/
|
||||
reset(state = {}) {
|
||||
const {
|
||||
integral = 0,
|
||||
lastOutput = 0,
|
||||
timestamp = null
|
||||
} = state;
|
||||
|
||||
this.integral = this._applyIntegralLimits(Number.isFinite(integral) ? integral : 0);
|
||||
this.prevError = null;
|
||||
this.prevMeasurement = null;
|
||||
this.lastOutput = this._clamp(
|
||||
Number.isFinite(lastOutput) ? lastOutput : 0,
|
||||
this.outputMin ?? Number.NEGATIVE_INFINITY,
|
||||
this.outputMax ?? Number.POSITIVE_INFINITY
|
||||
);
|
||||
this.lastTimestamp = Number.isFinite(timestamp) ? timestamp : null;
|
||||
this.derivativeState = 0;
|
||||
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* Execute one control loop iteration.
|
||||
*/
|
||||
update(setpoint, measurement, timestamp = Date.now()) {
|
||||
this._assertNumeric('setpoint', setpoint);
|
||||
this._assertNumeric('measurement', measurement);
|
||||
this._assertNumeric('timestamp', timestamp);
|
||||
|
||||
if (!this.autoMode) {
|
||||
this.prevError = setpoint - measurement;
|
||||
this.prevMeasurement = measurement;
|
||||
this.lastTimestamp = timestamp;
|
||||
return this.lastOutput;
|
||||
}
|
||||
|
||||
if (this.lastTimestamp !== null && (timestamp - this.lastTimestamp) < this.sampleTime) {
|
||||
return this.lastOutput;
|
||||
}
|
||||
|
||||
const elapsedMs = this.lastTimestamp === null ? this.sampleTime : (timestamp - this.lastTimestamp);
|
||||
const dtSeconds = Math.max(elapsedMs / 1000, Number.EPSILON);
|
||||
|
||||
const error = setpoint - measurement;
|
||||
this.integral = this._applyIntegralLimits(this.integral + error * dtSeconds);
|
||||
|
||||
const derivative = this._computeDerivative({ error, measurement, dtSeconds });
|
||||
this.derivativeState = this.derivativeFilter === 0
|
||||
? derivative
|
||||
: this.derivativeState + (derivative - this.derivativeState) * (1 - this.derivativeFilter);
|
||||
|
||||
const output = (this.kp * error) + (this.ki * this.integral) + (this.kd * this.derivativeState);
|
||||
this.lastOutput = this._clamp(output, this.outputMin, this.outputMax);
|
||||
|
||||
this.prevError = error;
|
||||
this.prevMeasurement = measurement;
|
||||
this.lastTimestamp = timestamp;
|
||||
|
||||
return this.lastOutput;
|
||||
}
|
||||
|
||||
/**
|
||||
* Inspect controller state for diagnostics or persistence.
|
||||
*/
|
||||
getState() {
|
||||
return {
|
||||
kp: this.kp,
|
||||
ki: this.ki,
|
||||
kd: this.kd,
|
||||
sampleTime: this.sampleTime,
|
||||
outputLimits: { min: this.outputMin, max: this.outputMax },
|
||||
integralLimits: { min: this.integralMin, max: this.integralMax },
|
||||
derivativeFilter: this.derivativeFilter,
|
||||
derivativeOnMeasurement: this.derivativeOnMeasurement,
|
||||
autoMode: this.autoMode,
|
||||
integral: this.integral,
|
||||
lastOutput: this.lastOutput,
|
||||
lastTimestamp: this.lastTimestamp
|
||||
};
|
||||
}
|
||||
|
||||
getLastOutput() {
|
||||
return this.lastOutput;
|
||||
}
|
||||
|
||||
_computeDerivative({ error, measurement, dtSeconds }) {
|
||||
if (!(dtSeconds > 0) || !Number.isFinite(dtSeconds)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (this.derivativeOnMeasurement && this.prevMeasurement !== null) {
|
||||
return -(measurement - this.prevMeasurement) / dtSeconds;
|
||||
}
|
||||
|
||||
if (this.prevError === null) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return (error - this.prevError) / dtSeconds;
|
||||
}
|
||||
|
||||
_applyIntegralLimits(value) {
|
||||
if (!Number.isFinite(value)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
let result = value;
|
||||
if (this.integralMin !== null && result < this.integralMin) {
|
||||
result = this.integralMin;
|
||||
}
|
||||
if (this.integralMax !== null && result > this.integralMax) {
|
||||
result = this.integralMax;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
_assertNumeric(label, value) {
|
||||
if (!Number.isFinite(value)) {
|
||||
throw new TypeError(`${label} must be a finite number`);
|
||||
}
|
||||
}
|
||||
|
||||
_clamp(value, min, max) {
|
||||
if (value < min) {
|
||||
return min;
|
||||
}
|
||||
if (value > max) {
|
||||
return max;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
}
|
||||
|
||||
module.exports = PIDController;
|
||||
Reference in New Issue
Block a user