[AARD-1766] More Simulation Support Pt.2 (#1103)

fission/src/Synthesis.tsx

@@ -47,9 +47,7 @@ import ChooseInputSchemePanel from "./ui/panels/configuring/ChooseInputSchemePan
 import ProgressNotifications from "./ui/components/ProgressNotification.tsx"
 import SceneOverlay from "./ui/components/SceneOverlay.tsx"
 
-import WPILibWSWorker from "@/systems/simulation/wpilib_brain/WPILibWSWorker.ts?worker"
 import WSViewPanel from "./ui/panels/WSViewPanel.tsx"
-import Lazy from "./util/Lazy.ts"
 
 import RCConfigPWMGroupModal from "@/modals/configuring/rio-config/RCConfigPWMGroupModal.tsx"
 import RCConfigCANGroupModal from "@/modals/configuring/rio-config/RCConfigCANGroupModal.tsx"
@@ -61,8 +59,6 @@ import PreferencesSystem from "./systems/preferences/PreferencesSystem.ts"
 import APSManagementModal from "./ui/modals/APSManagementModal.tsx"
 import ConfigurePanel from "./ui/panels/configuring/assembly-config/ConfigurePanel.tsx"
 
-const worker = new Lazy<Worker>(() => new WPILibWSWorker())
-
 function Synthesis() {
     const { openModal, closeModal, getActiveModalElement } = useModalManager(initialModals)
     const { openPanel, closePanel, closeAllPanels, getActivePanelElements } = usePanelManager(initialPanels)
@@ -93,8 +89,6 @@ function Synthesis() {
             setConsentPopupDisable(false)
         }
 
-        worker.getValue()
-
         let mainLoopHandle = 0
         const mainLoop = () => {
             mainLoopHandle = requestAnimationFrame(mainLoop)

fission/src/systems/simulation/wpilib_brain/SimInput.ts

@@ -1,20 +1,27 @@
 import World from "@/systems/World"
 import EncoderStimulus from "../stimulus/EncoderStimulus"
-import { SimCANEncoder, SimGyro } from "./WPILibBrain"
+import { SimCANEncoder, SimGyro, SimAccel, SimDIO, SimAI } from "./WPILibBrain"
 import Mechanism from "@/systems/physics/Mechanism"
 import Jolt from "@barclah/jolt-physics"
 import JOLT from "@/util/loading/JoltSyncLoader"
+import { JoltQuat_ThreeQuaternion, JoltVec3_ThreeVector3 } from "@/util/TypeConversions"
+import * as THREE from "three"
 
-export interface SimInput {
-    Update: (deltaT: number) => void
+export abstract class SimInput {
+    constructor(protected _device: string) {}
+
+    public abstract Update(deltaT: number): void
+
+    public get device(): string {
+        return this._device
+    }
 }
 
-export class SimEncoderInput implements SimInput {
-    private _device: string
+export class SimEncoderInput extends SimInput {
     private _stimulus: EncoderStimulus
 
     constructor(device: string, stimulus: EncoderStimulus) {
-        this._device = device
+        super(device)
         this._stimulus = stimulus
     }
 
@@ -24,8 +31,7 @@ export class SimEncoderInput implements SimInput {
     }
 }
 
-export class SimGyroInput implements SimInput {
-    private _device: string
+export class SimGyroInput extends SimInput {
     private _robot: Mechanism
     private _joltID?: Jolt.BodyID
     private _joltBody?: Jolt.Body
@@ -35,7 +41,7 @@ export class SimGyroInput implements SimInput {
     private static AXIS_Z: Jolt.Vec3 = new JOLT.Vec3(0, 0, 1)
 
     constructor(device: string, robot: Mechanism) {
-        this._device = device
+        super(device)
         this._robot = robot
         this._joltID = this._robot.nodeToBody.get(this._robot.rootBody)
 
@@ -58,12 +64,102 @@ export class SimGyroInput implements SimInput {
         return this.GetAxis(SimGyroInput.AXIS_Z)
     }
 
+    private GetAxisVelocity(axis: "x" | "y" | "z"): number {
+        const axes = this._joltBody?.GetAngularVelocity()
+        if (!axes) return 0
+
+        switch (axis) {
+            case "x":
+                return axes.GetX()
+            case "y":
+                return axes.GetY()
+            case "z":
+                return axes.GetZ()
+        }
+    }
+
     public Update(_deltaT: number) {
         const x = this.GetX()
         const y = this.GetY()
         const z = this.GetZ()
+
         SimGyro.SetAngleX(this._device, x)
         SimGyro.SetAngleY(this._device, y)
         SimGyro.SetAngleZ(this._device, z)
+        SimGyro.SetRateX(this._device, this.GetAxisVelocity("x"))
+        SimGyro.SetRateY(this._device, this.GetAxisVelocity("y"))
+        SimGyro.SetRateZ(this._device, this.GetAxisVelocity("z"))
+    }
+}
+
+export class SimAccelInput extends SimInput {
+    private _robot: Mechanism
+    private _joltID?: Jolt.BodyID
+    private _prevVel: THREE.Vector3
+
+    constructor(device: string, robot: Mechanism) {
+        super(device)
+        this._robot = robot
+        this._joltID = this._robot.nodeToBody.get(this._robot.rootBody)
+        this._prevVel = new THREE.Vector3(0, 0, 0)
+    }
+
+    public Update(deltaT: number) {
+        if (!this._joltID) return
+        const body = World.PhysicsSystem.GetBody(this._joltID)
+
+        const rot = JoltQuat_ThreeQuaternion(body.GetRotation())
+        const mat = new THREE.Matrix4().makeRotationFromQuaternion(rot).transpose()
+        const newVel = JoltVec3_ThreeVector3(body.GetLinearVelocity()).applyMatrix4(mat)
+
+        const x = (newVel.x - this._prevVel.x) / deltaT
+        const y = (newVel.y - this._prevVel.y) / deltaT
+        const z = (newVel.y - this._prevVel.y) / deltaT
+
+        SimAccel.SetX(this._device, x)
+        SimAccel.SetY(this._device, y)
+        SimAccel.SetZ(this._device, z)
+
+        this._prevVel = newVel
+    }
+}
+
+export class SimDigitalInput extends SimInput {
+    private _valueSupplier: () => boolean
+
+    /**
+     * Creates a Simulation Digital Input object.
+     *
+     * @param device Device ID
+     * @param valueSupplier Called each frame and returns what the value should be set to
+     */
+    constructor(device: string, valueSupplier: () => boolean) {
+        super(device)
+        this._valueSupplier = valueSupplier
+    }
+
+    private SetValue(value: boolean) {
+        SimDIO.SetValue(this._device, value)
+    }
+
+    public GetValue(): boolean {
+        return SimDIO.GetValue(this._device)
+    }
+
+    public Update(_deltaT: number) {
+        if (this._valueSupplier) this.SetValue(this._valueSupplier())
+    }
+}
+
+export class SimAnalogInput extends SimInput {
+    private _valueSupplier: () => number
+
+    constructor(device: string, valueSupplier: () => number) {
+        super(device)
+        this._valueSupplier = valueSupplier
+    }
+
+    public Update(_deltaT: number) {
+        SimAI.SetValue(this._device, this._valueSupplier())
     }
 }

fission/src/systems/simulation/wpilib_brain/SimOutput.ts

@@ -2,22 +2,25 @@ import Driver from "../driver/Driver"
 import HingeDriver from "../driver/HingeDriver"
 import SliderDriver from "../driver/SliderDriver"
 import WheelDriver from "../driver/WheelDriver"
-import { SimCAN, SimPWM, SimType } from "./WPILibBrain"
-
-// TODO: Averaging is probably not the right solution (if we want large output groups)
-// We can keep averaging, but we need a better ui for creating one to one (or just small) output groups
-// The issue is that if a drivetrain is one output group, then each driver is given the average of all the motors
-// We instead want a system where every driver gets (a) unique motor(s) that control it
-// That way a single driver might get the average of two motors or something, if it has two motors to control it
-// A system where motors a drivers are visually "linked" with "threads" in the UI would work well in my opinion
-export abstract class SimOutputGroup {
-    public name: string
+import { SimAO, SimCAN, SimDIO, SimPWM, SimType } from "./WPILibBrain"
+
+export abstract class SimOutput {
+    constructor(protected _name: string) {}
+
+    public abstract Update(deltaT: number): void
+
+    public get name(): string {
+        return this._name
+    }
+}
+
+export abstract class SimOutputGroup extends SimOutput {
     public ports: number[]
     public drivers: Driver[]
     public type: SimType
 
     public constructor(name: string, ports: number[], drivers: Driver[], type: SimType) {
-        this.name = name
+        super(name)
         this.ports = ports
         this.drivers = drivers
         this.type = type
@@ -35,7 +38,6 @@ export class PWMOutputGroup extends SimOutputGroup {
         const average =
             this.ports.reduce((sum, port) => {
                 const speed = SimPWM.GetSpeed(`${port}`) ?? 0
-                console.debug(port, speed)
                 return sum + speed
             }, 0) / this.ports.length
 
@@ -59,7 +61,7 @@ export class CANOutputGroup extends SimOutputGroup {
         const average =
             this.ports.reduce((sum, port) => {
                 const device = SimCAN.GetDeviceWithID(port, SimType.CANMotor)
-                return sum + (device?.get("<percentOutput") ?? 0)
+                return sum + ((device?.get("<percentOutput") as number | undefined) ?? 0)
             }, 0) / this.ports.length
 
         this.drivers.forEach(d => {
@@ -72,3 +74,36 @@ export class CANOutputGroup extends SimOutputGroup {
         })
     }
 }
+
+export class SimDigitalOutput extends SimOutput {
+    /**
+     * Creates a Simulation Digital Input/Output object.
+     *
+     * @param device Device ID
+     */
+    constructor(name: string) {
+        super(name)
+    }
+
+    public SetValue(value: boolean) {
+        SimDIO.SetValue(this._name, value)
+    }
+
+    public GetValue(): boolean {
+        return SimDIO.GetValue(this._name)
+    }
+
+    public Update(_deltaT: number) {}
+}
+
+export class SimAnalogOutput extends SimOutput {
+    public constructor(name: string) {
+        super(name)
+    }
+
+    public GetVoltage(): number {
+        return SimAO.GetVoltage(this._name)
+    }
+
+    public Update(_deltaT: number) {}
+}