initial commit

src/pid.ts

@@ -0,0 +1,80 @@
+export interface PIDParameters {
+  kP: number;
+  kI: number;
+  kD: 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.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;
+  }
+}