Motors in Drivers (#1078)

fission/package.json

@@ -32,7 +32,7 @@
     "colord": "^2.9.3",
     "framer-motion": "^10.13.1",
     "lygia": "^1.1.3",
-    "playwright": "^1.45.0",
+    "playwright": "^1.46.0",
     "postprocessing": "^6.35.6",
     "react": "^18.2.0",
     "react-colorful": "^5.6.1",

fission/src/Synthesis.tsx

@@ -36,6 +36,7 @@ import ThemeEditorModal from "@/modals/configuring/theme-editor/ThemeEditorModal
 import MatchModeModal from "@/modals/spawning/MatchModeModal"
 import RobotSwitchPanel from "@/panels/RobotSwitchPanel"
 import SpawnLocationsPanel from "@/panels/SpawnLocationPanel"
+import ConfigureSubsystemsPanel from "@/ui/panels/configuring/ConfigureSubsystemsPanel.tsx"
 import ScoreboardPanel from "@/panels/information/ScoreboardPanel"
 import DriverStationPanel from "@/panels/simulation/DriverStationPanel"
 import PokerPanel from "@/panels/PokerPanel.tsx"
@@ -241,6 +242,7 @@ const initialPanels: ReactElement[] = [
     <WSViewPanel key="ws-view" panelId="ws-view" />,
     <DebugPanel key="debug" panelId="debug" />,
     <ConfigurePanel key="configure" panelId="configure" />,
+    <ConfigureSubsystemsPanel key="subsystem-config" panelId="subsystem-config" openLocation="right" sidePadding={8} />,
 ]
 
 export default Synthesis

fission/src/systems/physics/Mechanism.ts

@@ -7,6 +7,7 @@ export interface MechanismConstraint {
     parentBody: Jolt.BodyID
     childBody: Jolt.BodyID
     constraint: Jolt.Constraint
+    maxVelocity: number
     info?: mirabuf.IInfo
 }
 

fission/src/systems/physics/PhysicsSystem.ts

@@ -23,6 +23,7 @@ import {
     OnContactValidateData,
     PhysicsEvent,
 } from "./ContactEvents"
+import PreferencesSystem from "../preferences/PreferencesSystem"
 
 export type JoltBodyIndexAndSequence = number
 
@@ -61,6 +62,9 @@ const FLOOR_FRICTION = 0.7
 const SUSPENSION_MIN_FACTOR = 0.1
 const SUSPENSION_MAX_FACTOR = 0.3
 
+// Motor constant
+const VELOCITY_DEFAULT = 30
+
 /**
  * The PhysicsSystem handles all Jolt Physics interactions within Synthesis.
  * This system can create physical representations of objects such as Robots,
@@ -107,7 +111,7 @@ class PhysicsSystem extends WorldSystem {
         const ground = this.CreateBox(
             new THREE.Vector3(5.0, 0.5, 5.0),
             undefined,
-            new THREE.Vector3(0.0, -2.05, 0.0),
+            new THREE.Vector3(0.0, -2.0, 0.0),
             undefined
         )
         ground.SetFriction(FLOOR_FRICTION)
@@ -353,13 +357,39 @@ class PhysicsSystem extends WorldSystem {
             const constraints: Jolt.Constraint[] = []
             let listener: Jolt.PhysicsStepListener | undefined = undefined
 
+            // Motor velocity and acceleration. Prioritizes preferences then mirabuf.
+            const prefMotors = PreferencesSystem.getRobotPreferences(parser.assembly.info?.name ?? "").motors
+            const prefMotor = prefMotors ? prefMotors.filter(x => x.name == jInst.info?.name) : undefined
+            const miraMotor = jointData.motorDefinitions![jDef.motorReference]
+
+            let maxVel = VELOCITY_DEFAULT
+            let maxForce
+            if (prefMotor && prefMotor[0]) {
+                maxVel = prefMotor[0].maxVelocity
+                maxForce = prefMotor[0].maxForce
+            } else if (miraMotor && miraMotor.simpleMotor) {
+                maxVel = miraMotor.simpleMotor.maxVelocity ?? VELOCITY_DEFAULT
+                maxForce = miraMotor.simpleMotor.stallTorque
+            }
+
             switch (jDef.jointMotionType!) {
                 case mirabuf.joint.JointMotion.REVOLUTE:
                     if (this.IsWheel(jDef)) {
+                        const prefVel = PreferencesSystem.getRobotPreferences(
+                            parser.assembly.info?.name ?? ""
+                        ).driveVelocity
+                        if (prefVel > 0) maxVel = prefVel
+
+                        const prefAcc = PreferencesSystem.getRobotPreferences(
+                            parser.assembly.info?.name ?? ""
+                        ).driveAcceleration
+                        if (prefAcc > 0) maxForce = prefAcc
+
                         if (parser.directedGraph.GetAdjacencyList(rnA.id).length > 0) {
                             const res = this.CreateWheelConstraint(
                                 jInst,
                                 jDef,
+                                maxForce ?? 1.5,
                                 bodyA,
                                 bodyB,
                                 parser.assembly.info!.version!
@@ -371,6 +401,7 @@ class PhysicsSystem extends WorldSystem {
                             const res = this.CreateWheelConstraint(
                                 jInst,
                                 jDef,
+                                maxForce ?? 1.5,
                                 bodyB,
                                 bodyA,
                                 parser.assembly.info!.version!
@@ -381,12 +412,19 @@ class PhysicsSystem extends WorldSystem {
                         }
                     } else {
                         constraints.push(
-                            this.CreateHingeConstraint(jInst, jDef, bodyA, bodyB, parser.assembly.info!.version!)
+                            this.CreateHingeConstraint(
+                                jInst,
+                                jDef,
+                                maxForce ?? 50,
+                                bodyA,
+                                bodyB,
+                                parser.assembly.info!.version!
+                            )
                         )
                     }
                     break
                 case mirabuf.joint.JointMotion.SLIDER:
-                    constraints.push(this.CreateSliderConstraint(jInst, jDef, bodyA, bodyB))
+                    constraints.push(this.CreateSliderConstraint(jInst, jDef, maxForce ?? 200, bodyA, bodyB))
                     break
                 default:
                     console.debug("Unsupported joint detected. Skipping...")
@@ -399,6 +437,7 @@ class PhysicsSystem extends WorldSystem {
                         parentBody: bodyIdA,
                         childBody: bodyIdB,
                         constraint: x,
+                        maxVelocity: maxVel ?? VELOCITY_DEFAULT,
                         info: jInst.info ?? undefined, // remove possibility for null
                     })
                 )
@@ -422,6 +461,7 @@ class PhysicsSystem extends WorldSystem {
     private CreateHingeConstraint(
         jointInstance: mirabuf.joint.JointInstance,
         jointDefinition: mirabuf.joint.Joint,
+        torque: number,
         bodyA: Jolt.Body,
         bodyB: Jolt.Body,
         versionNum: number
@@ -471,7 +511,11 @@ class PhysicsSystem extends WorldSystem {
             hingeConstraintSettings.mLimitsMax = -lower
         }
 
+        hingeConstraintSettings.mMotorSettings.mMaxTorqueLimit = torque
+        hingeConstraintSettings.mMotorSettings.mMinTorqueLimit = -torque
+
         const constraint = hingeConstraintSettings.Create(bodyA, bodyB)
+        this._constraints.push(constraint)
         this._joltPhysSystem.AddConstraint(constraint)
 
         return constraint
@@ -490,6 +534,7 @@ class PhysicsSystem extends WorldSystem {
     private CreateSliderConstraint(
         jointInstance: mirabuf.joint.JointInstance,
         jointDefinition: mirabuf.joint.Joint,
+        maxForce: number,
         bodyA: Jolt.Body,
         bodyB: Jolt.Body
     ): Jolt.Constraint {
@@ -535,6 +580,9 @@ class PhysicsSystem extends WorldSystem {
             sliderConstraintSettings.mLimitsMin = -halfRange
         }
 
+        sliderConstraintSettings.mMotorSettings.mMaxForceLimit = maxForce
+        sliderConstraintSettings.mMotorSettings.mMinForceLimit = -maxForce
+
         const constraint = sliderConstraintSettings.Create(bodyA, bodyB)
 
         this._constraints.push(constraint)
@@ -546,6 +594,7 @@ class PhysicsSystem extends WorldSystem {
     public CreateWheelConstraint(
         jointInstance: mirabuf.joint.JointInstance,
         jointDefinition: mirabuf.joint.Joint,
+        maxAcc: number,
         bodyMain: Jolt.Body,
         bodyWheel: Jolt.Body,
         versionNum: number
@@ -592,8 +641,11 @@ class PhysicsSystem extends WorldSystem {
         vehicleSettings.mWheels.clear()
         vehicleSettings.mWheels.push_back(wheelSettings)
 
+        // Other than maxTorque, these controller settings are not being used as of now
+        // because ArcadeDriveBehavior goes directly to the WheelDrivers.
+        // MaxTorque is only used as communication for WheelDriver to get maxAcceleration
         const controllerSettings = new JOLT.WheeledVehicleControllerSettings()
-        controllerSettings.mEngine.mMaxTorque = 1500.0
+        controllerSettings.mEngine.mMaxTorque = maxAcc
         controllerSettings.mTransmission.mClutchStrength = 10.0
         controllerSettings.mTransmission.mGearRatios.clear()
         controllerSettings.mTransmission.mGearRatios.push_back(2)

fission/src/systems/preferences/PreferenceTypes.ts

@@ -12,6 +12,7 @@ export type GlobalPreference =
     | "InputSchemes"
     | "RenderSceneTags"
     | "RenderScoreboard"
+    | "SubsystemGravity"
 
 export const RobotPreferencesKey: string = "Robots"
 export const FieldPreferencesKey: string = "Fields"
@@ -27,6 +28,7 @@ export const DefaultGlobalPreferences: { [key: string]: unknown } = {
     InputSchemes: [],
     RenderSceneTags: true,
     RenderScoreboard: true,
+    SubsystemGravity: false,
 }
 
 export type QualitySetting = "Low" | "Medium" | "High"
@@ -63,11 +65,20 @@ export function DefaultSequentialConfig(index: number, type: BehaviorType): Sequ
 
 export type RobotPreferences = {
     inputsSchemes: InputScheme[]
+    motors: MotorPreferences[]
     intake: IntakePreferences
     ejector: EjectorPreferences
+    driveVelocity: number
+    driveAcceleration: number
     sequentialConfig?: SequentialBehaviorPreferences[]
 }
 
+export type MotorPreferences = {
+    name: string
+    maxVelocity: number
+    maxForce: number
+}
+
 export type Alliance = "red" | "blue"
 
 export type ScoringZonePreferences = {
@@ -89,6 +100,7 @@ export type FieldPreferences = {
 export function DefaultRobotPreferences(): RobotPreferences {
     return {
         inputsSchemes: [],
+        motors: [],
         intake: {
             deltaTransformation: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1],
             zoneDiameter: 0.5,
@@ -99,6 +111,8 @@ export function DefaultRobotPreferences(): RobotPreferences {
             ejectorVelocity: 1,
             parentNode: undefined,
         },
+        driveVelocity: 0,
+        driveAcceleration: 0,
     }
 }
 

fission/src/systems/preferences/PreferencesSystem.ts

@@ -29,14 +29,6 @@ class PreferencesSystem {
         window.addEventListener("preferenceChanged", callback as EventListener)
     }
 
-    /** Sets a global preference to be a value of a specific type */
-    public static setGlobalPreference<T>(key: GlobalPreference, value: T) {
-        if (this._preferences == undefined) this.loadPreferences()
-
-        window.dispatchEvent(new PreferenceEvent(key, value))
-        this._preferences[key] = value
-    }
-
     /** Gets any preference from the preferences map */
     private static getPreference<T>(key: string): T | undefined {
         if (this._preferences == undefined) this.loadPreferences()
@@ -55,6 +47,14 @@ class PreferencesSystem {
         throw new Error("Preference '" + key + "' is not assigned a default!")
     }
 
+    /** Sets a global preference to be a value of a specific type */
+    public static setGlobalPreference<T>(key: GlobalPreference, value: T) {
+        if (this._preferences == undefined) this.loadPreferences()
+
+        window.dispatchEvent(new PreferenceEvent(key, value))
+        this._preferences[key] = value
+    }
+
     /** Gets a RobotPreferences object for a robot of a specific mira name */
     public static getRobotPreferences(miraName: string): RobotPreferences {
         const allRoboPrefs = this.getAllRobotPreferences()
@@ -68,6 +68,12 @@ class PreferencesSystem {
         return allRoboPrefs[miraName]
     }
 
+    /** Sets the RobotPreferences object for the robot of a specific mira name */
+    public static setRobotPreferences(miraName: string, value: RobotPreferences) {
+        const allRoboPrefs = this.getAllRobotPreferences()
+        allRoboPrefs[miraName] = value
+    }
+
     /** Gets preferences for every robot in local storage */
     public static getAllRobotPreferences(): { [key: string]: RobotPreferences } {
         let allRoboPrefs = this.getPreference<{ [key: string]: RobotPreferences }>(RobotPreferencesKey)

fission/src/systems/simulation/SimulationSystem.ts

@@ -85,19 +85,19 @@ class SimulationLayer {
         this._mechanism.constraints.forEach(x => {
             if (x.constraint.GetSubType() == JOLT.EConstraintSubType_Hinge) {
                 const hinge = JOLT.castObject(x.constraint, JOLT.HingeConstraint)
-                const driver = new HingeDriver(hinge, x.info)
+                const driver = new HingeDriver(hinge, x.maxVelocity, x.info)
                 this._drivers.push(driver)
                 const stim = new HingeStimulus(hinge)
                 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.info)
+                const driver = new WheelDriver(vehicle, x.maxVelocity, x.info)
                 this._drivers.push(driver)
                 const stim = new WheelRotationStimulus(vehicle.GetWheel(0))
                 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.info)
+                const driver = new SliderDriver(slider, x.maxVelocity, x.info)
                 this._drivers.push(driver)
                 const stim = new SliderStimulus(slider)
                 this._stimuli.push(stim)

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

@@ -8,9 +8,6 @@ class ArcadeDriveBehavior extends Behavior {
     private rightWheels: WheelDriver[]
     private _brainIndex: number
 
-    private _driveSpeed = 30
-    private _turnSpeed = 30
-
     public get wheels(): WheelDriver[] {
         return this.leftWheels.concat(this.rightWheels)
     }
@@ -30,19 +27,19 @@ class ArcadeDriveBehavior extends Behavior {
     }
 
     // Sets the drivetrains target linear and rotational velocity
-    private DriveSpeeds(linearVelocity: number, rotationVelocity: number) {
-        const leftSpeed = linearVelocity + rotationVelocity
-        const rightSpeed = linearVelocity - rotationVelocity
+    private DriveSpeeds(driveInput: number, turnInput: number) {
+        const leftDirection = driveInput + turnInput
+        const rightDirection = driveInput - turnInput
 
-        this.leftWheels.forEach(wheel => (wheel.targetWheelSpeed = leftSpeed))
-        this.rightWheels.forEach(wheel => (wheel.targetWheelSpeed = rightSpeed))
+        this.leftWheels.forEach(wheel => (wheel.accelerationDirection = leftDirection))
+        this.rightWheels.forEach(wheel => (wheel.accelerationDirection = rightDirection))
     }
 
     public Update(_: number): void {
-        const driveInput = InputSystem.getInput("arcadeDrive", this._brainIndex)
-        const turnInput = InputSystem.getInput("arcadeTurn", this._brainIndex)
-
-        this.DriveSpeeds(driveInput * this._driveSpeed, turnInput * this._turnSpeed)
+        this.DriveSpeeds(
+            InputSystem.getInput("arcadeDrive", this._brainIndex),
+            InputSystem.getInput("arcadeTurn", this._brainIndex)
+        )
     }
 }
 

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

@@ -10,8 +10,6 @@ class GenericArmBehavior extends SequenceableBehavior {
         return this._hingeDriver
     }
 
-    maxVelocity: number = 6
-
     constructor(
         hingeDriver: HingeDriver,
         hingeStimulus: HingeStimulus,
@@ -24,8 +22,8 @@ class GenericArmBehavior extends SequenceableBehavior {
         this._hingeDriver = hingeDriver
     }
 
-    applyInput = (velocity: number) => {
-        this._hingeDriver.targetVelocity = velocity
+    applyInput = (direction: number) => {
+        this._hingeDriver.accelerationDirection = direction
     }
 }
 

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

@@ -6,8 +6,6 @@ import SequenceableBehavior from "./SequenceableBehavior"
 class GenericElevatorBehavior extends SequenceableBehavior {
     private _sliderDriver: SliderDriver
 
-    maxVelocity = 6
-
     public get sliderDriver(): SliderDriver {
         return this._sliderDriver
     }
@@ -24,8 +22,8 @@ class GenericElevatorBehavior extends SequenceableBehavior {
         this._sliderDriver = sliderDriver
     }
 
-    applyInput = (velocity: number) => {
-        this._sliderDriver.targetVelocity = velocity
+    applyInput = (direction: number) => {
+        this._sliderDriver.accelerationDirection = direction
     }
 }