initial commit

src/main.ts

@@ -0,0 +1,3 @@
+export * from "./multi-pid";
+export * from "./pid";
+export * from "./vec";

src/multi-pid.ts

@@ -0,0 +1,61 @@
+import { PID, PIDCallback, PIDParameters, PIDReport } from "./pid";
+
+export type MultiPIDCallback = (channel: number, report: PIDReport) => any;
+
+export class MultiPID {
+  private pids: PID[];
+  private channels: number;
+
+  public constructor(channels: number, params: PIDParameters) {
+    this.channels = channels;
+    this.pids = Array(channels)
+      .fill(0)
+      .map((_) => new PID(params));
+  }
+
+  public setSetpoint(channel: number, setpoint: number) {
+    if (channel > this.channels - 1)
+      throw new Error("Channel index out of range: " + channel);
+    this.pids[channel].setSetpoint(setpoint);
+  }
+
+  public setSetpoints(setpoints: (number | null)[]) {
+    if (setpoints.length != this.channels)
+      throw new Error("Setpoint length mismatch: " + setpoints.length);
+    setpoints
+      .filter((x) => x != null)
+      .forEach((x, i) => this.pids[i].setSetpoint(x));
+  }
+
+  public setParameter(channel: number, params: PIDParameters) {
+    if (channel > this.channels - 1)
+      throw new Error("Channel index out of range: " + channel);
+    this.pids[channel].setParameters(params);
+  }
+
+  public setParameters(params: (PIDParameters | null)[]) {
+    if (params.length != this.channels)
+      throw new Error("Setpoint length mismatch: " + params.length);
+    params
+      .filter((x) => x != null)
+      .forEach((x, i) => this.pids[i].setParameters(x));
+  }
+
+  public updateOne(channel: number, measuredValue: number) {
+    if (channel > this.channels - 1)
+      throw new Error("Channel index out of range: " + channel);
+    return this.pids[channel].update(measuredValue);
+  }
+
+  public updateAll(measuredValues: (number | null)[]) {
+    if (measuredValues.length != this.channels)
+      throw new Error("Values length mismatch: " + measuredValues.length);
+    return measuredValues
+      .filter((x) => x != null)
+      .map((x, i) => this.pids[i].update(x));
+  }
+
+  public setCallback(cb: MultiPIDCallback) {
+    this.pids.forEach((p, i) => p.setCallback((report) => cb(i, report)));
+  }
+}

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;
+  }
+}

src/vec.ts

@@ -0,0 +1,66 @@
+export class Vec3 {
+  public constructor(
+    public x: number,
+    public y: number,
+    public z: number,
+  ) { }
+
+  public dot(b: Vec3): number {
+    return this.x * b.x + this.y * b.y + this.z * b.z;
+  }
+
+  public cross(b: Vec3): Vec3 {
+    return new Vec3(
+      this.y * b.z - this.z * b.y,
+      this.z * b.x - this.x * b.z,
+      this.x * b.y - this.y * b.x,
+    );
+  }
+
+  public add(b: Vec3): Vec3 {
+    return new Vec3(this.x + b.x, this.y + b.y, this.z + b.z);
+  }
+
+  public sub(b: Vec3): Vec3 {
+    return new Vec3(this.x - b.x, this.y - b.y, this.z - b.z);
+  }
+
+  public mul(v: number): Vec3 {
+    return new Vec3(this.x * v, this.y * v, this.z * v);
+  }
+
+  public len(): number {
+    return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
+  }
+
+  public normalize(): Vec3 {
+    const len = this.len();
+    if (len === 0) return new Vec3(0, 0, 0);
+    return new Vec3(this.x / len, this.y / len, this.z / len);
+  }
+
+  public lerp(b: Vec3, t: number): Vec3 {
+    return this.add(b.sub(this).mul(t));
+  }
+
+  public slerp(b: Vec3, t: number): Vec3 {
+    const v0 = this.normalize();
+    const v1 = b.normalize();
+
+    const dot = math.min(math.max(v0.dot(v1), -1), 1);
+
+    const theta = math.acos(dot) * t;
+
+    if (math.abs(theta) < 1e-6) return this.lerp(b, t);
+
+    const relativeDir = b.sub(this.mul(dot)).normalize();
+
+    return this.mul(math.cos(theta)).add(relativeDir.mul(math.sin(theta)));
+  }
+}
+
+export interface Quaternion {
+  x: number;
+  y: number;
+  z: number;
+}