feat: Auto panel, new sample for testing auto routine

Author: HunterBarclay

fission/src/Synthesis.tsx

@@ -64,6 +64,7 @@ import ContextMenu from "./ui/components/ContextMenu.tsx"
 import GlobalUIComponent from "./ui/components/GlobalUIComponent.tsx"
 import InitialConfigPanel from "./ui/panels/configuring/initial-config/InitialConfigPanel.tsx"
 import WPILibConnectionStatus from "./ui/components/WPILibConnectionStatus.tsx"
+import AutoTestPanel from "./ui/panels/simulation/AutoTestPanel.tsx"
 
 function Synthesis() {
     const { openModal, closeModal, getActiveModalElement } = useModalManager(initialModals)
@@ -167,7 +168,6 @@ function Synthesis() {
                             <SceneOverlay />
                             <ContextMenu />
                             <MainHUD key={"main-hud"} />
-                            <WPILibConnectionStatus />
                             {panelElements.length > 0 && panelElements}
                             {modalElement && (
                                 <div className="absolute w-full h-full left-0 top-0" key={"modal-element"}>
@@ -176,6 +176,7 @@ function Synthesis() {
                             )}
                             <ProgressNotifications key={"progress-notifications"} />
                             <ToastContainer key={"toast-container"} />
+                            <WPILibConnectionStatus />
 
                             {!consentPopupDisable ? (
                                 <AnalyticsConsent onClose={onDisableConsent} onConsent={onConsent} />
@@ -234,6 +235,7 @@ const initialPanels: ReactElement[] = [
     <WiringPanel key="wiring" panelId="wiring" />,
     <CameraSelectionPanel key="camera-select" panelId="camera-select" />,
     <InitialConfigPanel key="initial-config" panelId="initial-config" />,
+    <AutoTestPanel key="auto-test" panelId="auto-test" />,
 ]
 
 export default Synthesis

fission/src/mirabuf/IntakeSensorSceneObject.ts

@@ -11,8 +11,6 @@ import {
     ThreeVector3_JoltVec3,
 } from "@/util/TypeConversions"
 import { OnContactPersistedEvent } from "@/systems/physics/ContactEvents"
-import InputSystem from "@/systems/input/InputSystem"
-import SynthesisBrain from "@/systems/simulation/synthesis_brain/SynthesisBrain"
 
 class IntakeSensorSceneObject extends SceneObject {
     private _parentAssembly: MirabufSceneObject
@@ -44,9 +42,7 @@ class IntakeSensorSceneObject extends SceneObject {
             }
 
             this._collision = (event: OnContactPersistedEvent) => {
-                const brain = this._parentAssembly.brain
-                const brainIndex = brain instanceof SynthesisBrain ? brain.brainIndex ?? -1 : -1
-                if (InputSystem.getInput("intake", brainIndex)) {
+                if (this._parentAssembly.intakeActive) {
                     if (this._joltBodyId && !World.PhysicsSystem.isPaused) {
                         const body1 = event.message.body1
                         const body2 = event.message.body2

fission/src/mirabuf/MirabufSceneObject.ts

@@ -80,6 +80,14 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
 
     private _nameTag: SceneOverlayTag | undefined
 
+    private _intakeActive = false
+    private _ejectorActive = false
+
+    public get intakeActive() { return this._intakeActive }
+    public get ejectorActive() { return this._ejectorActive }
+    public set intakeActive(a: boolean) { this._intakeActive = a }
+    public set ejectorActive(a: boolean) { this._ejectorActive = a }
+
     get mirabufInstance() {
         return this._mirabufInstance
     }
@@ -191,7 +199,7 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
         if (this.miraType == MiraType.ROBOT) {
             World.SimulationSystem.RegisterMechanism(this._mechanism)
             const simLayer = World.SimulationSystem.GetSimulationLayer(this._mechanism)!
-            this._brain = new SynthesisBrain(this._mechanism, this._assemblyName)
+            this._brain = new SynthesisBrain(this, this._assemblyName)
             simLayer.SetBrain(this._brain)
         }
 
@@ -226,13 +234,11 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
     }
 
     public Update(): void {
-        const brainIndex = this._brain instanceof SynthesisBrain ? this._brain.brainIndex ?? -1 : -1
-        if (InputSystem.getInput("eject", brainIndex)) {
+        if (this.ejectorActive) {
             this.Eject()
         }
 
         this.UpdateMeshTransforms()
-
         this.UpdateBatches()
         this.UpdateNameTag()
     }
@@ -528,9 +534,7 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
                 })
                 Global_OpenPanel?.("configure")
             },
-        })
-
-        data.items.push({
+        }, {
             name: "Configure",
             func: () => {
                 setSelectedConfigurationType(
@@ -544,6 +548,15 @@ class MirabufSceneObject extends SceneObject implements ContextSupplier {
             },
         })
 
+        if (this.brain?.brainType == "wpilib") {
+            data.items.push({
+                name: "Auto Testing",
+                func: () => {
+                    Global_OpenPanel?.("auto-test")
+                },
+            })
+        }
+
         // if (this.miraType == MiraType.ROBOT) {
         //     const brainIndex = (this.brain as SynthesisBrain)?.brainIndex
         //     if (brainIndex != undefined) {

fission/src/systems/physics/PhysicsSystem.ts

@@ -1098,6 +1098,29 @@ class PhysicsSystem extends WorldSystem {
         )
     }
 
+    public SetBodyPositionRotationAndVelocity(
+        id: Jolt.BodyID,
+        position: Jolt.RVec3,
+        rotation: Jolt.Quat,
+        linear: Jolt.Vec3,
+        angular: Jolt.Vec3,
+        activate: boolean = true
+    ): void {
+        if (!this.IsBodyAdded(id)) {
+            return
+        }
+
+        this._joltBodyInterface.SetPositionAndRotation(
+            id,
+            position,
+            rotation,
+            activate ? JOLT.EActivation_Activate : JOLT.EActivation_DontActivate
+        )
+
+        this._joltBodyInterface.SetLinearVelocity(id, linear)
+        this._joltBodyInterface.SetAngularVelocity(id, angular)
+    }
+
     /**
      * Exposes SetShape method on the _joltBodyInterface
      * Sets the shape of the body

fission/src/systems/preferences/PreferenceTypes.ts

@@ -15,6 +15,7 @@ export type GlobalPreference =
     | "RenderSceneTags"
     | "RenderScoreboard"
     | "SubsystemGravity"
+    | "SimAutoReconnect"
 
 export const RobotPreferencesKey: string = "Robots"
 export const FieldPreferencesKey: string = "Fields"
@@ -35,6 +36,7 @@ export const DefaultGlobalPreferences: { [key: string]: unknown } = {
     RenderSceneTags: true,
     RenderScoreboard: true,
     SubsystemGravity: false,
+    SimAutoReconnect: false,
 }
 
 export type QualitySetting = "Low" | "Medium" | "High"

fission/src/systems/simulation/SimulationSystem.ts

@@ -2,7 +2,7 @@ import JOLT from "@/util/loading/JoltSyncLoader"
 import Mechanism from "../physics/Mechanism"
 import WorldSystem from "../WorldSystem"
 import Brain from "./Brain"
-import Driver, { makeDriverID } from "./driver/Driver"
+import Driver, { DriverType, makeDriverID } from "./driver/Driver"
 import Stimulus, { makeStimulusID, StimulusType } from "./stimulus/Stimulus"
 import HingeDriver from "./driver/HingeDriver"
 import WheelDriver from "./driver/WheelDriver"
@@ -11,6 +11,10 @@ import HingeStimulus from "./stimulus/HingeStimulus"
 import WheelRotationStimulus from "./stimulus/WheelStimulus"
 import SliderStimulus from "./stimulus/SliderStimulus"
 import ChassisStimulus from "./stimulus/ChassisStimulus"
+import IntakeDriver from "./driver/IntakeDriver"
+import World from "../World"
+import MirabufSceneObject from "@/mirabuf/MirabufSceneObject"
+import EjectorDriver from "./driver/EjectorDriver"
 
 class SimulationSystem extends WorldSystem {
     private _simMechanisms: Map<Mechanism, SimulationLayer>
@@ -79,6 +83,8 @@ class SimulationLayer {
     constructor(mechanism: Mechanism) {
         this._mechanism = mechanism
 
+        const assembly = [...World.SceneRenderer.sceneObjects.values()].find(x => (x as MirabufSceneObject).mechanism == mechanism) as MirabufSceneObject
+
         // Generate standard drivers and stimuli
         this._drivers = new Map()
         this._stimuli = new Map()
@@ -103,8 +109,31 @@ class SimulationLayer {
                 this._stimuli.set(JSON.stringify(stim.id), stim)
             }
         })
-        const chassisStim = new ChassisStimulus({ type: StimulusType.Stim_ChassisAccel, guid: "unknown" }, mechanism.nodeToBody.get(mechanism.rootBody)!)
-        this._stimuli.set(JSON.stringify(chassisStim), chassisStim)
+
+        const chassisStim = new ChassisStimulus(
+            { type: StimulusType.Stim_ChassisAccel, guid: "CHASSIS_GUID" },
+            mechanism.nodeToBody.get(mechanism.rootBody)!,
+            { GUID: "CHASSIS_GUID", name: "Chassis" }
+        )
+        this._stimuli.set(JSON.stringify(chassisStim.id), chassisStim)
+
+        if (assembly) {
+            const intakeDriv = new IntakeDriver(
+                { type: DriverType.Driv_Intake, guid: "INTAKE_GUID" },
+                assembly,
+                { GUID: "INTAKE_GUID", name: "Intake" }
+            )
+            const ejectorDriv = new EjectorDriver(
+                { type: DriverType.Driv_Ejector, guid: "EJECTOR_GUID" },
+                assembly,
+                { GUID: "EJECTOR_GUID", name: "Ejector" }
+            )
+            this._drivers.set(JSON.stringify(ejectorDriv.id), ejectorDriv)
+            this._drivers.set(JSON.stringify(intakeDriv.id), intakeDriv)
+        } else {
+            console.debug("No Assembly found with given mechanism, skipping intake and ejector...")
+        }
+        
     }
 
     public Update(deltaT: number) {

fission/src/systems/simulation/behavior/synthesis/GamepieceManipBehavior.ts

@@ -0,0 +1,30 @@
+import Behavior from "@/systems/simulation/behavior/Behavior"
+import InputSystem from "@/systems/input/InputSystem"
+import EjectorDriver from "../../driver/EjectorDriver"
+import IntakeDriver from "../../driver/IntakeDriver"
+
+class GamepieceManipBehavior extends Behavior {
+    private _brainIndex: number
+
+    private _ejector: EjectorDriver
+    private _intake: IntakeDriver
+
+    constructor(
+        ejector: EjectorDriver,
+        intake: IntakeDriver,
+        brainIndex: number
+    ) {
+        super([ejector, intake], [])
+    
+        this._brainIndex = brainIndex
+        this._ejector = ejector
+        this._intake = intake
+    }
+
+    public Update(_: number): void {
+        this._ejector.value = InputSystem.getInput("eject", this._brainIndex)
+        this._intake.value = InputSystem.getInput("intake", this._brainIndex)
+    }
+}
+
+export default GamepieceManipBehavior

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

@@ -8,6 +8,8 @@ export enum DriverType {
     Driv_Hinge = "Driv_Hinge",
     Driv_Wheel = "Driv_Wheel",
     Driv_Slider = "Driv_Slider",
+    Driv_Intake = "Driv_Intake",
+    Driv_Ejector = "Driv_Ejector",
     Driv_Unknown = "Driv_Unknown",
 }
 

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

@@ -0,0 +1,35 @@
+import { mirabuf } from "@/proto/mirabuf";
+import { NoraNumber, NoraTypes } from "../Nora";
+import Driver, { DriverID } from "./Driver";
+import MirabufSceneObject from "@/mirabuf/MirabufSceneObject";
+
+class EjectorDriver extends Driver {
+
+    public value: number
+
+    private _assembly: MirabufSceneObject
+
+    public constructor(id: DriverID, assembly: MirabufSceneObject, info?: mirabuf.IInfo) {
+        super(id, info)
+
+        this._assembly = assembly
+        this.value = 0.0
+    }
+
+    public Update(_deltaT: number): void {
+        this._assembly.ejectorActive = this.value > 0.5
+    }
+
+    public setReceiverValue(val: NoraNumber): void {
+        this.value = val
+    }
+    public getReceiverType(): NoraTypes {
+        return NoraTypes.Number
+    }
+    public DisplayName(): string {
+        return "Ejector"
+    }
+
+}
+
+export default EjectorDriver

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

@@ -0,0 +1,35 @@
+import { mirabuf } from "@/proto/mirabuf";
+import { NoraNumber, NoraTypes } from "../Nora";
+import Driver, { DriverID } from "./Driver";
+import MirabufSceneObject from "@/mirabuf/MirabufSceneObject";
+
+class IntakeDriver extends Driver {
+
+    public value: number
+
+    private _assembly: MirabufSceneObject
+
+    public constructor(id: DriverID, assembly: MirabufSceneObject, info?: mirabuf.IInfo) {
+        super(id, info)
+
+        this._assembly = assembly
+        this.value = 0.0
+    }
+
+    public Update(_deltaT: number): void {
+        this._assembly.intakeActive = this.value > 0.5
+    }
+
+    public setReceiverValue(val: NoraNumber): void {
+        this.value = val
+    }
+    public getReceiverType(): NoraTypes {
+        return NoraTypes.Number
+    }
+    public DisplayName(): string {
+        return "Intake"
+    }
+
+}
+
+export default IntakeDriver

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

@@ -38,7 +38,7 @@ class ChassisStimulus extends Stimulus {
         throw new Error("Method not implemented.")
     }
     public DisplayName(): string {
-        return "Chassis"
+        return "Chassis [Accel|Gyro]"
     }
 }