664 lines
21 KiB
JavaScript
664 lines
21 KiB
JavaScript
'use strict';
|
|
|
|
/**
|
|
* Production-focused discrete PID controller with modern control features:
|
|
* - auto/manual and bumpless transfer
|
|
* - freeze/unfreeze (hold output while optionally tracking process)
|
|
* - derivative filtering and derivative-on-measurement/error
|
|
* - anti-windup (clamp or back-calculation)
|
|
* - output and integral limits
|
|
* - output rate limiting
|
|
* - deadband
|
|
* - gain scheduling (array/function)
|
|
* - feedforward and dynamic tunings at runtime
|
|
*/
|
|
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,
|
|
setpointWeight = 1,
|
|
derivativeWeight = 0,
|
|
deadband = 0,
|
|
outputRateLimitUp = Number.POSITIVE_INFINITY,
|
|
outputRateLimitDown = Number.POSITIVE_INFINITY,
|
|
antiWindupMode = 'clamp',
|
|
backCalculationGain = 0,
|
|
gainSchedule = null,
|
|
autoMode = true,
|
|
trackOnManual = true,
|
|
frozen = false,
|
|
freezeTrackMeasurement = true,
|
|
freezeTrackError = false,
|
|
} = 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.setSetpointWeights({ beta: setpointWeight, gamma: derivativeWeight });
|
|
this.setDeadband(deadband);
|
|
this.setOutputRateLimits(outputRateLimitUp, outputRateLimitDown);
|
|
this.setAntiWindup({ mode: antiWindupMode, backCalculationGain });
|
|
this.setGainSchedule(gainSchedule);
|
|
|
|
this.derivativeOnMeasurement = Boolean(derivativeOnMeasurement);
|
|
this.autoMode = Boolean(autoMode);
|
|
this.trackOnManual = Boolean(trackOnManual);
|
|
|
|
this.frozen = Boolean(frozen);
|
|
this.freezeTrackMeasurement = Boolean(freezeTrackMeasurement);
|
|
this.freezeTrackError = Boolean(freezeTrackError);
|
|
|
|
this.reset();
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
setSetpointWeights({ beta = this.setpointWeight ?? 1, gamma = this.derivativeWeight ?? 0 } = {}) {
|
|
if (!Number.isFinite(beta) || !Number.isFinite(gamma)) {
|
|
throw new TypeError('setpoint and derivative weights must be finite numbers');
|
|
}
|
|
|
|
this.setpointWeight = beta;
|
|
this.derivativeWeight = gamma;
|
|
return this;
|
|
}
|
|
|
|
setDeadband(value = this.deadband ?? 0) {
|
|
if (!Number.isFinite(value) || value < 0) {
|
|
throw new RangeError('deadband must be a non-negative finite number');
|
|
}
|
|
|
|
this.deadband = value;
|
|
return this;
|
|
}
|
|
|
|
setOutputRateLimits(up = this.outputRateLimitUp, down = this.outputRateLimitDown) {
|
|
if (!Number.isFinite(up) && up !== Number.POSITIVE_INFINITY) {
|
|
throw new TypeError('outputRateLimitUp must be finite or Infinity');
|
|
}
|
|
if (!Number.isFinite(down) && down !== Number.POSITIVE_INFINITY) {
|
|
throw new TypeError('outputRateLimitDown must be finite or Infinity');
|
|
}
|
|
if (up <= 0 || down <= 0) {
|
|
throw new RangeError('output rate limits must be positive values');
|
|
}
|
|
|
|
this.outputRateLimitUp = up;
|
|
this.outputRateLimitDown = down;
|
|
return this;
|
|
}
|
|
|
|
setAntiWindup({ mode = this.antiWindupMode ?? 'clamp', backCalculationGain = this.backCalculationGain ?? 0 } = {}) {
|
|
const normalized = String(mode || 'clamp').trim().toLowerCase();
|
|
if (normalized !== 'clamp' && normalized !== 'backcalc') {
|
|
throw new RangeError('anti windup mode must be "clamp" or "backcalc"');
|
|
}
|
|
if (!Number.isFinite(backCalculationGain) || backCalculationGain < 0) {
|
|
throw new RangeError('backCalculationGain must be a non-negative finite number');
|
|
}
|
|
|
|
this.antiWindupMode = normalized;
|
|
this.backCalculationGain = backCalculationGain;
|
|
return this;
|
|
}
|
|
|
|
/**
|
|
* Gain schedule options:
|
|
* - null: disabled
|
|
* - function(input, state) => { kp, ki, kd }
|
|
* - array: [{ min, max, kp, ki, kd }, ...]
|
|
*/
|
|
setGainSchedule(schedule = null) {
|
|
if (schedule == null) {
|
|
this.gainSchedule = null;
|
|
return this;
|
|
}
|
|
|
|
if (typeof schedule === 'function') {
|
|
this.gainSchedule = schedule;
|
|
return this;
|
|
}
|
|
|
|
if (!Array.isArray(schedule)) {
|
|
throw new TypeError('gainSchedule must be null, a function, or an array');
|
|
}
|
|
|
|
schedule.forEach((entry, index) => {
|
|
if (!entry || typeof entry !== 'object') {
|
|
throw new TypeError(`gainSchedule[${index}] must be an object`);
|
|
}
|
|
const { min = Number.NEGATIVE_INFINITY, max = Number.POSITIVE_INFINITY, kp, ki, kd } = entry;
|
|
if (!Number.isFinite(min) && min !== Number.NEGATIVE_INFINITY) {
|
|
throw new TypeError(`gainSchedule[${index}].min must be finite or -Infinity`);
|
|
}
|
|
if (!Number.isFinite(max) && max !== Number.POSITIVE_INFINITY) {
|
|
throw new TypeError(`gainSchedule[${index}].max must be finite or Infinity`);
|
|
}
|
|
if (min >= max) {
|
|
throw new RangeError(`gainSchedule[${index}] min must be smaller than max`);
|
|
}
|
|
[kp, ki, kd].forEach((value, gainIndex) => {
|
|
const label = ['kp', 'ki', 'kd'][gainIndex];
|
|
if (!Number.isFinite(value)) {
|
|
throw new TypeError(`gainSchedule[${index}].${label} must be finite`);
|
|
}
|
|
});
|
|
});
|
|
|
|
this.gainSchedule = schedule;
|
|
return this;
|
|
}
|
|
|
|
setMode(mode, options = {}) {
|
|
if (mode !== 'automatic' && mode !== 'manual') {
|
|
throw new Error('mode must be either "automatic" or "manual"');
|
|
}
|
|
|
|
const nextAuto = mode === 'automatic';
|
|
const previousAuto = this.autoMode;
|
|
this.autoMode = nextAuto;
|
|
|
|
if (options && Number.isFinite(options.manualOutput)) {
|
|
this.setManualOutput(options.manualOutput);
|
|
}
|
|
|
|
if (!previousAuto && nextAuto) {
|
|
this._initializeForAuto(options);
|
|
}
|
|
|
|
return this;
|
|
}
|
|
|
|
freeze(options = {}) {
|
|
this.frozen = true;
|
|
this.freezeTrackMeasurement = options.trackMeasurement !== false;
|
|
this.freezeTrackError = Boolean(options.trackError);
|
|
|
|
if (Number.isFinite(options.output)) {
|
|
this.setManualOutput(options.output);
|
|
}
|
|
|
|
return this;
|
|
}
|
|
|
|
unfreeze() {
|
|
this.frozen = false;
|
|
return this;
|
|
}
|
|
|
|
isFrozen() {
|
|
return this.frozen;
|
|
}
|
|
|
|
setManualOutput(value) {
|
|
this._assertNumeric('manual output', value);
|
|
this.lastOutput = this._clamp(value, this.outputMin, this.outputMax);
|
|
return this.lastOutput;
|
|
}
|
|
|
|
reset(state = {}) {
|
|
const {
|
|
integral = 0,
|
|
lastOutput = 0,
|
|
timestamp = null,
|
|
prevMeasurement = null,
|
|
prevError = null,
|
|
prevDerivativeInput = null,
|
|
derivativeState = 0,
|
|
} = state;
|
|
|
|
this.integral = this._applyIntegralLimits(Number.isFinite(integral) ? integral : 0);
|
|
this.prevError = Number.isFinite(prevError) ? prevError : null;
|
|
this.prevMeasurement = Number.isFinite(prevMeasurement) ? prevMeasurement : null;
|
|
this.prevDerivativeInput = Number.isFinite(prevDerivativeInput) ? prevDerivativeInput : 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 = Number.isFinite(derivativeState) ? derivativeState : 0;
|
|
|
|
return this;
|
|
}
|
|
|
|
update(setpoint, measurement, timestamp = Date.now(), options = {}) {
|
|
if (timestamp && typeof timestamp === 'object' && options && Object.keys(options).length === 0) {
|
|
options = timestamp;
|
|
timestamp = Date.now();
|
|
}
|
|
|
|
this._assertNumeric('setpoint', setpoint);
|
|
this._assertNumeric('measurement', measurement);
|
|
this._assertNumeric('timestamp', timestamp);
|
|
|
|
const opts = options || {};
|
|
|
|
if (opts.tunings && typeof opts.tunings === 'object') {
|
|
this.setTunings(opts.tunings);
|
|
}
|
|
|
|
if (Number.isFinite(opts.gainInput)) {
|
|
this._applyGainSchedule(opts.gainInput, { setpoint, measurement, timestamp });
|
|
}
|
|
|
|
if (typeof opts.setMode === 'string') {
|
|
this.setMode(opts.setMode, opts);
|
|
}
|
|
|
|
if (opts.freeze === true) this.freeze(opts);
|
|
if (opts.unfreeze === true) this.unfreeze();
|
|
|
|
if (Number.isFinite(opts.manualOutput)) {
|
|
this.setManualOutput(opts.manualOutput);
|
|
}
|
|
|
|
const feedForward = Number.isFinite(opts.feedForward) ? opts.feedForward : 0;
|
|
const force = Boolean(opts.force);
|
|
|
|
const error = setpoint - measurement;
|
|
|
|
if (!this.autoMode) {
|
|
if (this.trackOnManual) {
|
|
this._trackProcessState(setpoint, measurement, error, timestamp);
|
|
}
|
|
return this.lastOutput;
|
|
}
|
|
|
|
if (this.frozen) {
|
|
if (this.freezeTrackMeasurement || this.freezeTrackError) {
|
|
this._trackProcessState(setpoint, measurement, error, timestamp, {
|
|
trackMeasurement: this.freezeTrackMeasurement,
|
|
trackError: this.freezeTrackError,
|
|
});
|
|
}
|
|
return this.lastOutput;
|
|
}
|
|
|
|
if (!force && 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 inDeadband = Math.abs(error) <= this.deadband;
|
|
if (inDeadband) {
|
|
this.prevError = error;
|
|
this.prevMeasurement = measurement;
|
|
this.prevDerivativeInput = this.derivativeOnMeasurement
|
|
? measurement
|
|
: ((this.derivativeWeight * setpoint) - measurement);
|
|
this.lastTimestamp = timestamp;
|
|
return this.lastOutput;
|
|
}
|
|
|
|
const effectiveError = error;
|
|
|
|
const pInput = (this.setpointWeight * setpoint) - measurement;
|
|
const pTerm = this.kp * pInput;
|
|
|
|
const derivativeRaw = this._computeDerivative({ setpoint, measurement, error, dtSeconds });
|
|
this.derivativeState = this.derivativeFilter === 0
|
|
? derivativeRaw
|
|
: this.derivativeState + (derivativeRaw - this.derivativeState) * (1 - this.derivativeFilter);
|
|
|
|
const dTerm = this.kd * this.derivativeState;
|
|
|
|
const nextIntegral = this._applyIntegralLimits(this.integral + (effectiveError * dtSeconds));
|
|
let unclampedOutput = pTerm + (this.ki * nextIntegral) + dTerm + feedForward;
|
|
let clampedOutput = this._clamp(unclampedOutput, this.outputMin, this.outputMax);
|
|
|
|
if (this.antiWindupMode === 'backcalc' && this.ki !== 0 && this.backCalculationGain > 0) {
|
|
const correctedIntegral = nextIntegral + ((clampedOutput - unclampedOutput) * this.backCalculationGain * dtSeconds);
|
|
this.integral = this._applyIntegralLimits(correctedIntegral);
|
|
} else {
|
|
const saturatingHigh = clampedOutput >= this.outputMax && effectiveError > 0;
|
|
const saturatingLow = clampedOutput <= this.outputMin && effectiveError < 0;
|
|
this.integral = (saturatingHigh || saturatingLow) ? this.integral : nextIntegral;
|
|
}
|
|
|
|
let output = pTerm + (this.ki * this.integral) + dTerm + feedForward;
|
|
output = this._clamp(output, this.outputMin, this.outputMax);
|
|
|
|
if (this.lastTimestamp !== null) {
|
|
output = this._applyRateLimit(output, this.lastOutput, dtSeconds);
|
|
}
|
|
|
|
if (Number.isFinite(opts.trackingOutput)) {
|
|
this._trackIntegralToOutput(opts.trackingOutput, { pTerm, dTerm, feedForward });
|
|
output = this._clamp(opts.trackingOutput, this.outputMin, this.outputMax);
|
|
}
|
|
|
|
this.lastOutput = output;
|
|
this.prevError = error;
|
|
this.prevMeasurement = measurement;
|
|
this.prevDerivativeInput = this.derivativeOnMeasurement
|
|
? measurement
|
|
: ((this.derivativeWeight * setpoint) - measurement);
|
|
this.lastTimestamp = timestamp;
|
|
|
|
return this.lastOutput;
|
|
}
|
|
|
|
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,
|
|
setpointWeight: this.setpointWeight,
|
|
derivativeWeight: this.derivativeWeight,
|
|
deadband: this.deadband,
|
|
outputRateLimits: { up: this.outputRateLimitUp, down: this.outputRateLimitDown },
|
|
antiWindupMode: this.antiWindupMode,
|
|
backCalculationGain: this.backCalculationGain,
|
|
autoMode: this.autoMode,
|
|
frozen: this.frozen,
|
|
integral: this.integral,
|
|
derivativeState: this.derivativeState,
|
|
lastOutput: this.lastOutput,
|
|
lastTimestamp: this.lastTimestamp,
|
|
};
|
|
}
|
|
|
|
getLastOutput() {
|
|
return this.lastOutput;
|
|
}
|
|
|
|
_initializeForAuto(options = {}) {
|
|
const setpoint = Number.isFinite(options.setpoint) ? options.setpoint : null;
|
|
const measurement = Number.isFinite(options.measurement) ? options.measurement : null;
|
|
const timestamp = Number.isFinite(options.timestamp) ? options.timestamp : Date.now();
|
|
|
|
if (measurement !== null) {
|
|
this.prevMeasurement = measurement;
|
|
}
|
|
if (setpoint !== null && measurement !== null) {
|
|
this.prevError = setpoint - measurement;
|
|
this.prevDerivativeInput = this.derivativeOnMeasurement
|
|
? measurement
|
|
: ((this.derivativeWeight * setpoint) - measurement);
|
|
}
|
|
|
|
this.lastTimestamp = timestamp;
|
|
|
|
if (this.ki !== 0 && setpoint !== null && measurement !== null) {
|
|
const pTerm = this.kp * ((this.setpointWeight * setpoint) - measurement);
|
|
const dTerm = this.kd * this.derivativeState;
|
|
const trackedIntegral = (this.lastOutput - pTerm - dTerm) / this.ki;
|
|
this.integral = this._applyIntegralLimits(Number.isFinite(trackedIntegral) ? trackedIntegral : this.integral);
|
|
}
|
|
}
|
|
|
|
_trackProcessState(setpoint, measurement, error, timestamp, tracking = {}) {
|
|
const trackMeasurement = tracking.trackMeasurement !== false;
|
|
const trackError = Boolean(tracking.trackError);
|
|
|
|
if (trackMeasurement) {
|
|
this.prevMeasurement = measurement;
|
|
this.prevDerivativeInput = this.derivativeOnMeasurement
|
|
? measurement
|
|
: ((this.derivativeWeight * setpoint) - measurement);
|
|
}
|
|
|
|
if (trackError) {
|
|
this.prevError = error;
|
|
}
|
|
|
|
this.lastTimestamp = timestamp;
|
|
}
|
|
|
|
_trackIntegralToOutput(trackingOutput, terms) {
|
|
if (this.ki === 0) return;
|
|
const { pTerm, dTerm, feedForward } = terms;
|
|
const targetIntegral = (trackingOutput - pTerm - dTerm - feedForward) / this.ki;
|
|
if (Number.isFinite(targetIntegral)) {
|
|
this.integral = this._applyIntegralLimits(targetIntegral);
|
|
}
|
|
}
|
|
|
|
_applyGainSchedule(input, state) {
|
|
if (!this.gainSchedule) return;
|
|
|
|
if (typeof this.gainSchedule === 'function') {
|
|
const tunings = this.gainSchedule(input, this.getState(), state);
|
|
if (tunings && typeof tunings === 'object') {
|
|
this.setTunings(tunings);
|
|
}
|
|
return;
|
|
}
|
|
|
|
const matched = this.gainSchedule.find((entry) => input >= entry.min && input < entry.max);
|
|
if (matched) {
|
|
this.setTunings({ kp: matched.kp, ki: matched.ki, kd: matched.kd });
|
|
}
|
|
}
|
|
|
|
_computeDerivative({ setpoint, measurement, error, dtSeconds }) {
|
|
if (!(dtSeconds > 0) || !Number.isFinite(dtSeconds)) {
|
|
return 0;
|
|
}
|
|
|
|
if (this.derivativeOnMeasurement) {
|
|
if (this.prevMeasurement === null) return 0;
|
|
return -(measurement - this.prevMeasurement) / dtSeconds;
|
|
}
|
|
|
|
const derivativeInput = (this.derivativeWeight * setpoint) - measurement;
|
|
if (this.prevDerivativeInput === null) return 0;
|
|
const derivativeFromInput = (derivativeInput - this.prevDerivativeInput) / dtSeconds;
|
|
|
|
if (Number.isFinite(derivativeFromInput)) {
|
|
return derivativeFromInput;
|
|
}
|
|
|
|
if (this.prevError === null) return 0;
|
|
return (error - this.prevError) / dtSeconds;
|
|
}
|
|
|
|
_applyRateLimit(nextOutput, previousOutput, dtSeconds) {
|
|
const maxRise = Number.isFinite(this.outputRateLimitUp)
|
|
? this.outputRateLimitUp * dtSeconds
|
|
: Number.POSITIVE_INFINITY;
|
|
const maxFall = Number.isFinite(this.outputRateLimitDown)
|
|
? this.outputRateLimitDown * dtSeconds
|
|
: Number.POSITIVE_INFINITY;
|
|
|
|
const lower = previousOutput - maxFall;
|
|
const upper = previousOutput + maxRise;
|
|
return this._clamp(nextOutput, lower, upper);
|
|
}
|
|
|
|
_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;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Cascade PID utility:
|
|
* - primary PID controls the outer variable
|
|
* - primary output becomes setpoint for secondary PID
|
|
*/
|
|
class CascadePIDController {
|
|
constructor(options = {}) {
|
|
const {
|
|
primary = {},
|
|
secondary = {},
|
|
} = options;
|
|
|
|
this.primary = primary instanceof PIDController ? primary : new PIDController(primary);
|
|
this.secondary = secondary instanceof PIDController ? secondary : new PIDController(secondary);
|
|
}
|
|
|
|
update({
|
|
setpoint,
|
|
primaryMeasurement,
|
|
secondaryMeasurement,
|
|
timestamp = Date.now(),
|
|
primaryOptions = {},
|
|
secondaryOptions = {},
|
|
} = {}) {
|
|
if (!Number.isFinite(setpoint)) {
|
|
throw new TypeError('setpoint must be a finite number');
|
|
}
|
|
if (!Number.isFinite(primaryMeasurement)) {
|
|
throw new TypeError('primaryMeasurement must be a finite number');
|
|
}
|
|
if (!Number.isFinite(secondaryMeasurement)) {
|
|
throw new TypeError('secondaryMeasurement must be a finite number');
|
|
}
|
|
|
|
const secondarySetpoint = this.primary.update(setpoint, primaryMeasurement, timestamp, primaryOptions);
|
|
const controlOutput = this.secondary.update(secondarySetpoint, secondaryMeasurement, timestamp, secondaryOptions);
|
|
|
|
return {
|
|
primaryOutput: secondarySetpoint,
|
|
secondaryOutput: controlOutput,
|
|
state: this.getState(),
|
|
};
|
|
}
|
|
|
|
setMode(mode, options = {}) {
|
|
this.primary.setMode(mode, options.primary || options);
|
|
this.secondary.setMode(mode, options.secondary || options);
|
|
return this;
|
|
}
|
|
|
|
freeze(options = {}) {
|
|
this.primary.freeze(options.primary || options);
|
|
this.secondary.freeze(options.secondary || options);
|
|
return this;
|
|
}
|
|
|
|
unfreeze() {
|
|
this.primary.unfreeze();
|
|
this.secondary.unfreeze();
|
|
return this;
|
|
}
|
|
|
|
reset(state = {}) {
|
|
this.primary.reset(state.primary || {});
|
|
this.secondary.reset(state.secondary || {});
|
|
return this;
|
|
}
|
|
|
|
getState() {
|
|
return {
|
|
primary: this.primary.getState(),
|
|
secondary: this.secondary.getState(),
|
|
};
|
|
}
|
|
}
|
|
|
|
module.exports = {
|
|
PIDController,
|
|
CascadePIDController,
|
|
};
|