85 lines
2.0 KiB
TypeScript
85 lines
2.0 KiB
TypeScript
export interface PIDParameters {
|
|
kP: number;
|
|
kI: number;
|
|
kD: number;
|
|
calculateError?: (setpoint: number, measuredValue: number) => number;
|
|
}
|
|
|
|
export type PIDReport = {
|
|
timeMs: number;
|
|
measured: number;
|
|
setpoint: number;
|
|
output: number;
|
|
error: number;
|
|
};
|
|
|
|
export type PIDCallback = (report: PIDReport) => any;
|
|
|
|
export class PID {
|
|
private params: PIDParameters;
|
|
private setpoint: number;
|
|
private prevError: number = 0;
|
|
private integral: number = 0;
|
|
private lastTime: number | null = null;
|
|
private handler: PIDCallback | null = null;
|
|
|
|
constructor(params: PIDParameters, setpoint: number = 0) {
|
|
this.params = Object.assign({}, params);
|
|
this.setpoint = setpoint;
|
|
}
|
|
|
|
public setParameters(params: PIDParameters) {
|
|
this.params = Object.assign({}, params);
|
|
}
|
|
|
|
public setSetpoint(setpoint: number): void {
|
|
this.setpoint = setpoint;
|
|
this.prevError = 0;
|
|
this.integral = 0;
|
|
this.lastTime = null;
|
|
}
|
|
|
|
public setCallback(handler: PIDCallback) {
|
|
this.handler = handler;
|
|
}
|
|
|
|
public update(measuredValue: number): number {
|
|
const currentTime = os.epoch("utc");
|
|
|
|
const error = this.params.calculateError
|
|
? this.params.calculateError(this.setpoint, measuredValue)
|
|
: this.setpoint - measuredValue;
|
|
|
|
let deltaTime = 0;
|
|
if (this.lastTime !== null) {
|
|
deltaTime = (currentTime - this.lastTime) / 1000; // in seconds
|
|
}
|
|
|
|
this.integral += error * deltaTime;
|
|
const derivative = deltaTime > 0 ? (error - this.prevError) / deltaTime : 0;
|
|
|
|
const output =
|
|
this.params.kP * error +
|
|
this.params.kI * this.integral +
|
|
this.params.kD * derivative;
|
|
|
|
this.prevError = error;
|
|
this.lastTime = currentTime;
|
|
|
|
try {
|
|
if (this.handler != null)
|
|
this.handler({
|
|
timeMs: currentTime,
|
|
measured: measuredValue,
|
|
setpoint: this.setpoint,
|
|
output,
|
|
error,
|
|
});
|
|
} catch (e) {
|
|
print(e);
|
|
}
|
|
|
|
return output;
|
|
}
|
|
}
|