CAN Support

fission/src/systems/simulation/SimulationSystem.ts

@@ -87,19 +87,19 @@ class SimulationLayer {
                 const hinge = JOLT.castObject(x.constraint, JOLT.HingeConstraint)
                 const driver = new HingeDriver(hinge, x.maxVelocity, x.info)
                 this._drivers.push(driver)
-                const stim = new HingeStimulus(hinge)
+                const stim = new HingeStimulus(hinge, x.info)
                 this._stimuli.push(stim)
             } else if (x.constraint.GetSubType() == JOLT.EConstraintSubType_Vehicle) {
                 const vehicle = JOLT.castObject(x.constraint, JOLT.VehicleConstraint)
                 const driver = new WheelDriver(vehicle, x.maxVelocity, x.info)
                 this._drivers.push(driver)
-                const stim = new WheelRotationStimulus(vehicle.GetWheel(0))
+                const stim = new WheelRotationStimulus(vehicle.GetWheel(0), x.info)
                 this._stimuli.push(stim)
             } else if (x.constraint.GetSubType() == JOLT.EConstraintSubType_Slider) {
                 const slider = JOLT.castObject(x.constraint, JOLT.SliderConstraint)
                 const driver = new SliderDriver(slider, x.maxVelocity, x.info)
                 this._drivers.push(driver)
-                const stim = new SliderStimulus(slider)
+                const stim = new SliderStimulus(slider, x.info)
                 this._stimuli.push(stim)
             }
         })
@@ -117,9 +117,7 @@ class SimulationLayer {
 
         this._brain = brain
 
-        if (this._brain) {
-            this._brain.Enable()
-        }
+        if (this._brain) this._brain.Enable()
     }
 }
 

fission/src/systems/simulation/driver/WheelDriver.ts

@@ -19,7 +19,7 @@ class WheelDriver extends Driver {
     public maxVelocity = 30.0
     private _maxAcceleration = 1.5
 
-    private _targetVelocity = () => {
+    public _targetVelocity = () => {
         let vel = this.accelerationDirection * (this._reversed ? -1 : 1) * this.maxVelocity
 
         if (vel - this._prevVel < -this._maxAcceleration) vel = this._prevVel - this._maxAcceleration

fission/src/systems/simulation/stimulus/ChassisStimulus.ts

@@ -1,6 +1,7 @@
 import Jolt from "@barclah/jolt-physics"
 import Stimulus from "./Stimulus"
 import World from "@/systems/World"
+import { mirabuf } from "@/proto/mirabuf"
 
 class ChassisStimulus extends Stimulus {
     private _body: Jolt.Body
@@ -19,8 +20,8 @@ class ChassisStimulus extends Stimulus {
         return this._body.GetRotation().GetEulerAngles()
     }
 
-    public constructor(bodyId: Jolt.BodyID) {
-        super()
+    public constructor(bodyId: Jolt.BodyID, info?: mirabuf.IInfo) {
+        super(info)
 
         this._body = World.PhysicsSystem.GetBody(bodyId)
         this._mass = this._body.GetShape().GetMassProperties().mMass

fission/src/systems/simulation/stimulus/EncoderStimulus.ts

@@ -1,11 +1,12 @@
+import { mirabuf } from "@/proto/mirabuf"
 import Stimulus from "./Stimulus"
 
 abstract class EncoderStimulus extends Stimulus {
     public abstract get positionValue(): number
     public abstract get velocityValue(): number
 
-    protected constructor() {
-        super()
+    protected constructor(info?: mirabuf.IInfo) {
+        super(info)
     }
 
     public abstract Update(_: number): void

fission/src/systems/simulation/stimulus/HingeStimulus.ts

@@ -1,5 +1,6 @@
 import Jolt from "@barclah/jolt-physics"
 import EncoderStimulus from "./EncoderStimulus"
+import { mirabuf } from "@/proto/mirabuf"
 
 class HingeStimulus extends EncoderStimulus {
     private _accum: boolean = false
@@ -25,8 +26,8 @@ class HingeStimulus extends EncoderStimulus {
         this._accum = shouldAccum
     }
 
-    public constructor(hinge: Jolt.HingeConstraint) {
-        super()
+    public constructor(hinge: Jolt.HingeConstraint, info?: mirabuf.IInfo) {
+        super(info)
 
         this._hinge = hinge
     }

fission/src/systems/simulation/stimulus/SliderStimulus.ts

@@ -1,5 +1,6 @@
 import Jolt from "@barclah/jolt-physics"
 import EncoderStimulus from "./EncoderStimulus"
+import { mirabuf } from "@/proto/mirabuf"
 
 class SliderStimulus extends EncoderStimulus {
     private _slider: Jolt.SliderConstraint
@@ -12,8 +13,8 @@ class SliderStimulus extends EncoderStimulus {
         return this._velocity
     }
 
-    public constructor(slider: Jolt.SliderConstraint) {
-        super()
+    public constructor(slider: Jolt.SliderConstraint, info?: mirabuf.IInfo) {
+        super(info)
 
         this._slider = slider
     }

fission/src/systems/simulation/stimulus/Stimulus.ts

@@ -1,5 +1,17 @@
+import { mirabuf } from "@/proto/mirabuf"
+
 abstract class Stimulus {
+    private _info?: mirabuf.IInfo
+
+    constructor(info?: mirabuf.IInfo) {
+        this._info = info
+    }
+
     public abstract Update(deltaT: number): void
+
+    public get info() {
+        return this._info
+    }
 }
 
 export default Stimulus

fission/src/systems/simulation/stimulus/WheelStimulus.ts

@@ -1,5 +1,6 @@
 import Jolt from "@barclah/jolt-physics"
 import EncoderStimulus from "./EncoderStimulus"
+import { mirabuf } from "@/proto/mirabuf"
 
 /**
  *
@@ -28,8 +29,8 @@ class WheelRotationStimulus extends EncoderStimulus {
         this._accum = shouldAccum
     }
 
-    public constructor(wheel: Jolt.Wheel) {
-        super()
+    public constructor(wheel: Jolt.Wheel, info?: mirabuf.IInfo) {
+        super(info)
 
         this._wheel = wheel
     }

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

@@ -0,0 +1,69 @@
+import World from "@/systems/World"
+import EncoderStimulus from "../stimulus/EncoderStimulus"
+import { SimCANEncoder, SimGyro } from "./WPILibBrain"
+import Mechanism from "@/systems/physics/Mechanism"
+import Jolt from "@barclah/jolt-physics"
+import JOLT from "@/util/loading/JoltSyncLoader"
+
+export interface SimInput {
+    Update: (deltaT: number) => void
+}
+
+export class SimEncoderInput implements SimInput {
+    private _device: string
+    private _stimulus: EncoderStimulus
+
+    constructor(device: string, stimulus: EncoderStimulus) {
+        this._device = device
+        this._stimulus = stimulus
+    }
+
+    public Update(_deltaT: number) {
+        SimCANEncoder.SetPosition(this._device, this._stimulus.positionValue)
+        SimCANEncoder.SetVelocity(this._device, this._stimulus.velocityValue)
+    }
+}
+
+export class SimGyroInput implements SimInput {
+    private _device: string
+    private _robot: Mechanism
+    private _joltID?: Jolt.BodyID
+    private _joltBody?: Jolt.Body
+
+    private static AXIS_X: Jolt.Vec3 = new JOLT.Vec3(1, 0, 0)
+    private static AXIS_Y: Jolt.Vec3 = new JOLT.Vec3(0, 1, 0)
+    private static AXIS_Z: Jolt.Vec3 = new JOLT.Vec3(0, 0, 1)
+
+    constructor(device: string, robot: Mechanism) {
+        this._device = device
+        this._robot = robot
+        this._joltID = this._robot.nodeToBody.get(this._robot.rootBody)
+
+        if (this._joltID) this._joltBody = World.PhysicsSystem.GetBody(this._joltID)
+    }
+
+    private GetAxis(axis: Jolt.Vec3): number {
+        return ((this._joltBody?.GetRotation().GetRotationAngle(axis) ?? 0) * 180) / Math.PI
+    }
+
+    private GetX(): number {
+        return this.GetAxis(SimGyroInput.AXIS_X)
+    }
+
+    private GetY(): number {
+        return this.GetAxis(SimGyroInput.AXIS_Y)
+    }
+
+    private GetZ(): number {
+        return this.GetAxis(SimGyroInput.AXIS_Z)
+    }
+
+    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)
+    }
+}

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

@@ -0,0 +1,74 @@
+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
+    public ports: number[]
+    public drivers: Driver[]
+    public type: SimType
+
+    public constructor(name: string, ports: number[], drivers: Driver[], type: SimType) {
+        this.name = name
+        this.ports = ports
+        this.drivers = drivers
+        this.type = type
+    }
+
+    public abstract Update(deltaT: number): void
+}
+
+export class PWMOutputGroup extends SimOutputGroup {
+    public constructor(name: string, ports: number[], drivers: Driver[]) {
+        super(name, ports, drivers, SimType.PWM)
+    }
+
+    public Update(_deltaT: number) {
+        const average =
+            this.ports.reduce((sum, port) => {
+                const speed = SimPWM.GetSpeed(`${port}`) ?? 0
+                console.debug(port, speed)
+                return sum + speed
+            }, 0) / this.ports.length
+
+        this.drivers.forEach(d => {
+            if (d instanceof WheelDriver) {
+                d.accelerationDirection = average
+            } else if (d instanceof HingeDriver || d instanceof SliderDriver) {
+                d.accelerationDirection = average
+            }
+            d.Update(_deltaT)
+        })
+    }
+}
+
+export class CANOutputGroup extends SimOutputGroup {
+    public constructor(name: string, ports: number[], drivers: Driver[]) {
+        super(name, ports, drivers, SimType.CANMotor)
+    }
+
+    public Update(deltaT: number): void {
+        const average =
+            this.ports.reduce((sum, port) => {
+                const device = SimCAN.GetDeviceWithID(port, SimType.CANMotor)
+                return sum + (device?.get("<percentOutput") ?? 0)
+            }, 0) / this.ports.length
+
+        this.drivers.forEach(d => {
+            if (d instanceof WheelDriver) {
+                d.accelerationDirection = average
+            } else if (d instanceof HingeDriver || d instanceof SliderDriver) {
+                d.accelerationDirection = average
+            }
+            d.Update(deltaT)
+        })
+    }
+}