Motors in Drivers

fission/src/Synthesis.tsx

@@ -40,6 +40,7 @@ import SpawnLocationsPanel from "@/panels/SpawnLocationPanel"
 import ConfigureGamepiecePickupPanel from "@/panels/configuring/ConfigureGamepiecePickupPanel"
 import ConfigureShotTrajectoryPanel from "@/panels/configuring/ConfigureShotTrajectoryPanel"
 import ScoringZonesPanel from "@/panels/configuring/scoring/ScoringZonesPanel"
+import ConfigureJointsPanel from "@/panels/configuring/ConfigureJointsPanel"
 import ScoreboardPanel from "@/panels/information/ScoreboardPanel"
 import DriverStationPanel from "@/panels/simulation/DriverStationPanel"
 import PokerPanel from "@/panels/PokerPanel.tsx"
@@ -253,6 +254,7 @@ const initialPanels: ReactElement[] = [
     <ChooseInputSchemePanel key="choose-scheme" panelId="choose-scheme" />,
     <WSViewPanel key="ws-view" panelId="ws-view" />,
     <DebugPanel key="debug" panelId="debug" />,
+    <ConfigureJointsPanel key="joint-config" panelId="joint-config" openLocation="right" sidePadding={8} />,
 ]
 
 export default Synthesis

fission/src/mirabuf/MirabufParser.ts

@@ -240,7 +240,7 @@ class MirabufParser {
             console.log("Failed to get part definitions")
             return
         }
-        console.log(partDefinitions)
+        console.debug(partDefinitions)
     }
 
     private NewRigidNode(suffix?: string): RigidNode {

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

@@ -45,8 +45,17 @@ export type EjectorPreferences = {
 
 export type RobotPreferences = {
     inputsSchemes: InputScheme[]
+    motors: MotorPreferences[]
     intake: IntakePreferences
     ejector: EjectorPreferences
+    driveVelocity: number
+    driveAcceleration: number
+}
+
+export type MotorPreferences = {
+    name: string
+    maxVelocity: number
+    maxForce: number
 }
 
 export type Alliance = "red" | "blue"
@@ -70,6 +79,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,
@@ -80,6 +90,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
-
     constructor(
         leftWheels: WheelDriver[],
         rightWheels: WheelDriver[],
@@ -26,19 +23,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

@@ -8,8 +8,6 @@ class GenericArmBehavior extends Behavior {
     private _inputName: string
     private _brainIndex: number
 
-    private _rotationalSpeed = 6
-
     constructor(hingeDriver: HingeDriver, hingeStimulus: HingeStimulus, jointIndex: number, brainIndex: number) {
         super([hingeDriver], [hingeStimulus])
 
@@ -18,13 +16,13 @@ class GenericArmBehavior extends Behavior {
         this._brainIndex = brainIndex
     }
 
-    // Sets the arms target rotational velocity
-    rotateArm(rotationalVelocity: number) {
-        this._hingeDriver.targetVelocity = rotationalVelocity
+    // Sets the arm's acceleration direction
+    rotateArm(input: number) {
+        this._hingeDriver.accelerationDirection = input
     }
 
     public Update(_: number): void {
-        this.rotateArm(InputSystem.getInput(this._inputName, this._brainIndex) * this._rotationalSpeed)
+        this.rotateArm(InputSystem.getInput(this._inputName, this._brainIndex))
     }
 }
 

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

@@ -8,8 +8,6 @@ class GenericElevatorBehavior extends Behavior {
     private _inputName: string
     private _brainIndex: number
 
-    private _linearSpeed = 2.5
-
     constructor(sliderDriver: SliderDriver, sliderStimulus: SliderStimulus, jointIndex: number, brainIndex: number) {
         super([sliderDriver], [sliderStimulus])
 
@@ -18,13 +16,13 @@ class GenericElevatorBehavior extends Behavior {
         this._brainIndex = brainIndex
     }
 
-    // Changes the elevators target position
-    moveElevator(linearVelocity: number) {
-        this._sliderDriver.targetVelocity = linearVelocity
+    // Changes the elevator's acceleration direction
+    moveElevator(input: number) {
+        this._sliderDriver.accelerationDirection = input
     }
 
     public Update(_: number): void {
-        this.moveElevator(InputSystem.getInput(this._inputName, this._brainIndex) * this._linearSpeed)
+        this.moveElevator(InputSystem.getInput(this._inputName, this._brainIndex))
     }
 }