diff --git a/fission/package.json b/fission/package.json index 6e1ae08b2b..0643c80a52 100644 --- a/fission/package.json +++ b/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", diff --git a/fission/src/Synthesis.tsx b/fission/src/Synthesis.tsx index 0e2fe2a29b..c1d33806ca 100644 --- a/fission/src/Synthesis.tsx +++ b/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[] = [ , , , + , ] export default Synthesis diff --git a/fission/src/systems/physics/Mechanism.ts b/fission/src/systems/physics/Mechanism.ts index bf6502b4f5..ae5e0722d3 100644 --- a/fission/src/systems/physics/Mechanism.ts +++ b/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 } diff --git a/fission/src/systems/physics/PhysicsSystem.ts b/fission/src/systems/physics/PhysicsSystem.ts index 42c43f213a..eaeb5f3dcc 100644 --- a/fission/src/systems/physics/PhysicsSystem.ts +++ b/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) diff --git a/fission/src/systems/preferences/PreferenceTypes.ts b/fission/src/systems/preferences/PreferenceTypes.ts index 9331cc425e..64ee580142 100644 --- a/fission/src/systems/preferences/PreferenceTypes.ts +++ b/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, } } diff --git a/fission/src/systems/preferences/PreferencesSystem.ts b/fission/src/systems/preferences/PreferencesSystem.ts index 8933a44bd6..09b8c3961e 100644 --- a/fission/src/systems/preferences/PreferencesSystem.ts +++ b/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(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(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(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) diff --git a/fission/src/systems/simulation/SimulationSystem.ts b/fission/src/systems/simulation/SimulationSystem.ts index f0feeba8b2..e9bad980f6 100644 --- a/fission/src/systems/simulation/SimulationSystem.ts +++ b/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) diff --git a/fission/src/systems/simulation/behavior/synthesis/ArcadeDriveBehavior.ts b/fission/src/systems/simulation/behavior/synthesis/ArcadeDriveBehavior.ts index a76d2f98d3..ae93c45444 100644 --- a/fission/src/systems/simulation/behavior/synthesis/ArcadeDriveBehavior.ts +++ b/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) + ) } } diff --git a/fission/src/systems/simulation/behavior/synthesis/GenericArmBehavior.ts b/fission/src/systems/simulation/behavior/synthesis/GenericArmBehavior.ts index e177549d54..eb98b7346a 100644 --- a/fission/src/systems/simulation/behavior/synthesis/GenericArmBehavior.ts +++ b/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 } } diff --git a/fission/src/systems/simulation/behavior/synthesis/GenericElevatorBehavior.ts b/fission/src/systems/simulation/behavior/synthesis/GenericElevatorBehavior.ts index ceb93f813f..b325ff54cf 100644 --- a/fission/src/systems/simulation/behavior/synthesis/GenericElevatorBehavior.ts +++ b/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 } } diff --git a/fission/src/systems/simulation/behavior/synthesis/SequenceableBehavior.ts b/fission/src/systems/simulation/behavior/synthesis/SequenceableBehavior.ts index 034765097c..3da51b26b4 100644 --- a/fission/src/systems/simulation/behavior/synthesis/SequenceableBehavior.ts +++ b/fission/src/systems/simulation/behavior/synthesis/SequenceableBehavior.ts @@ -9,8 +9,6 @@ abstract class SequenceableBehavior extends Behavior { private _brainIndex: number private _sequentialConfig: SequentialBehaviorPreferences | undefined - abstract maxVelocity: number - public get jointIndex(): number { return this._jointIndex } @@ -35,7 +33,7 @@ abstract class SequenceableBehavior extends Behavior { const inputName = "joint " + (this._sequentialConfig?.parentJointIndex ?? this._jointIndex) const inverted = this._sequentialConfig?.inverted ?? false - this.applyInput(InputSystem.getInput(inputName, this._brainIndex) * this.maxVelocity * (inverted ? -1 : 1)) + this.applyInput(InputSystem.getInput(inputName, this._brainIndex) * (inverted ? -1 : 1)) } } diff --git a/fission/src/systems/simulation/driver/HingeDriver.ts b/fission/src/systems/simulation/driver/HingeDriver.ts index 1558f0f4c0..755994921d 100644 --- a/fission/src/systems/simulation/driver/HingeDriver.ts +++ b/fission/src/systems/simulation/driver/HingeDriver.ts @@ -3,24 +3,26 @@ import Driver, { DriverControlMode } from "./Driver" import { GetLastDeltaT } from "@/systems/physics/PhysicsSystem" import JOLT from "@/util/loading/JoltSyncLoader" import { mirabuf } from "@/proto/mirabuf" +import PreferencesSystem, { PreferenceEvent } from "@/systems/preferences/PreferencesSystem" + +const MAX_TORQUE_WITHOUT_GRAV = 100 class HingeDriver extends Driver { private _constraint: Jolt.HingeConstraint private _controlMode: DriverControlMode = DriverControlMode.Velocity - private _targetVelocity: number = 0.0 private _targetAngle: number + private _maxTorqueWithGrav: number = 0.0 + public accelerationDirection: number = 0.0 + public maxVelocity: number public get constraint(): Jolt.HingeConstraint { return this._constraint } - public get targetVelocity(): number { - return this._targetVelocity - } - public set targetVelocity(radsPerSec: number) { - this._targetVelocity = radsPerSec - } + private _prevAng: number = 0.0 + + private _gravityChange?: (event: PreferenceEvent) => void public get targetAngle(): number { return this._targetAngle @@ -29,13 +31,14 @@ class HingeDriver extends Driver { this._targetAngle = Math.max(this._constraint.GetLimitsMin(), Math.min(this._constraint.GetLimitsMax(), rads)) } - public set minTorqueLimit(nm: number) { - const motorSettings = this._constraint.GetMotorSettings() - motorSettings.mMinTorqueLimit = nm + public get maxForce() { + return this._constraint.GetMotorSettings().mMaxTorqueLimit } - public set maxTorqueLimit(nm: number) { + + public set maxForce(nm: number) { const motorSettings = this._constraint.GetMotorSettings() - motorSettings.mMaxTorqueLimit = nm + motorSettings.set_mMaxTorqueLimit(nm) + motorSettings.set_mMinTorqueLimit(-nm) } public get controlMode(): DriverControlMode { @@ -57,10 +60,12 @@ class HingeDriver extends Driver { } } - public constructor(constraint: Jolt.HingeConstraint, info?: mirabuf.IInfo) { + public constructor(constraint: Jolt.HingeConstraint, maxVelocity: number, info?: mirabuf.IInfo) { super(info) this._constraint = constraint + this.maxVelocity = maxVelocity + this._targetAngle = this._constraint.GetCurrentAngle() const motorSettings = this._constraint.GetMotorSettings() const springSettings = motorSettings.mSpringSettings @@ -68,21 +73,41 @@ class HingeDriver extends Driver { // These values were selected based on the suggestions of the documentation for stiff control. springSettings.mFrequency = 20 * (1.0 / GetLastDeltaT()) springSettings.mDamping = 0.995 - motorSettings.mSpringSettings = springSettings - motorSettings.mMinTorqueLimit = -200.0 - motorSettings.mMaxTorqueLimit = 200.0 - this._targetAngle = this._constraint.GetCurrentAngle() + this._maxTorqueWithGrav = motorSettings.get_mMaxTorqueLimit() + if (!PreferencesSystem.getGlobalPreference("SubsystemGravity")) { + motorSettings.set_mMaxTorqueLimit(MAX_TORQUE_WITHOUT_GRAV) + motorSettings.set_mMinTorqueLimit(-MAX_TORQUE_WITHOUT_GRAV) + } this.controlMode = DriverControlMode.Velocity + + this._gravityChange = (event: PreferenceEvent) => { + if (event.prefName == "SubsystemGravity") { + const motorSettings = this._constraint.GetMotorSettings() + if (event.prefValue) { + motorSettings.set_mMaxTorqueLimit(this._maxTorqueWithGrav) + motorSettings.set_mMinTorqueLimit(-this._maxTorqueWithGrav) + } else { + motorSettings.set_mMaxTorqueLimit(MAX_TORQUE_WITHOUT_GRAV) + motorSettings.set_mMinTorqueLimit(-MAX_TORQUE_WITHOUT_GRAV) + } + } + } + + PreferencesSystem.addEventListener(this._gravityChange) } public Update(_: number): void { if (this._controlMode == DriverControlMode.Velocity) { - this._constraint.SetTargetAngularVelocity(this._targetVelocity) + this._constraint.SetTargetAngularVelocity(this.accelerationDirection * this.maxVelocity) } else if (this._controlMode == DriverControlMode.Position) { - this._constraint.SetTargetAngle(this._targetAngle) + let ang = this._targetAngle + + if (ang - this._prevAng < -this.maxVelocity) ang = this._prevAng - this.maxVelocity + if (ang - this._prevAng > this.maxVelocity) ang = this._prevAng + this.maxVelocity + this._constraint.SetTargetAngle(ang) } } } diff --git a/fission/src/systems/simulation/driver/SliderDriver.ts b/fission/src/systems/simulation/driver/SliderDriver.ts index 9af74046e1..eb80a871e6 100644 --- a/fission/src/systems/simulation/driver/SliderDriver.ts +++ b/fission/src/systems/simulation/driver/SliderDriver.ts @@ -3,24 +3,26 @@ import Driver, { DriverControlMode } from "./Driver" import { GetLastDeltaT } from "@/systems/physics/PhysicsSystem" import JOLT from "@/util/loading/JoltSyncLoader" import { mirabuf } from "@/proto/mirabuf" +import PreferencesSystem, { PreferenceEvent } from "@/systems/preferences/PreferencesSystem" + +const MAX_FORCE_WITHOUT_GRAV = 500 class SliderDriver extends Driver { private _constraint: Jolt.SliderConstraint private _controlMode: DriverControlMode = DriverControlMode.Velocity - private _targetVelocity: number = 0.0 private _targetPosition: number = 0.0 + private _maxForceWithGrav: number = 0.0 + public accelerationDirection: number = 0.0 + public maxVelocity: number = 1.0 public get constraint(): Jolt.SliderConstraint { return this._constraint } - public get targetVelocity(): number { - return this._targetVelocity - } - public set targetVelocity(radsPerSec: number) { - this._targetVelocity = radsPerSec - } + private _prevPos: number = 0.0 + + private _gravityChange?: (event: PreferenceEvent) => void public get targetPosition(): number { return this._targetPosition @@ -32,13 +34,13 @@ class SliderDriver extends Driver { ) } - public set minForceLimit(newtons: number) { - const motorSettings = this._constraint.GetMotorSettings() - motorSettings.mMinForceLimit = newtons + public get maxForce(): number { + return this._constraint.GetMotorSettings().mMaxForceLimit } - public set maxForceLimit(newtons: number) { + public set maxForce(newtons: number) { const motorSettings = this._constraint.GetMotorSettings() - motorSettings.mMaxForceLimit = newtons + motorSettings.set_mMaxForceLimit(newtons) + motorSettings.set_mMinForceLimit(-newtons) } public get controlMode(): DriverControlMode { @@ -60,29 +62,53 @@ class SliderDriver extends Driver { } } - public constructor(constraint: Jolt.SliderConstraint, info?: mirabuf.IInfo) { + public constructor(constraint: Jolt.SliderConstraint, maxVelocity: number, info?: mirabuf.IInfo) { super(info) this._constraint = constraint + this.maxVelocity = maxVelocity const motorSettings = this._constraint.GetMotorSettings() const springSettings = motorSettings.mSpringSettings springSettings.mFrequency = 20 * (1.0 / GetLastDeltaT()) springSettings.mDamping = 0.999 - motorSettings.mSpringSettings = springSettings - motorSettings.mMinForceLimit = -900.0 - motorSettings.mMaxForceLimit = 900.0 + + this._maxForceWithGrav = motorSettings.get_mMaxForceLimit() + if (!PreferencesSystem.getGlobalPreference("SubsystemGravity")) { + motorSettings.set_mMaxForceLimit(MAX_FORCE_WITHOUT_GRAV) + motorSettings.set_mMinForceLimit(-MAX_FORCE_WITHOUT_GRAV) + } this._constraint.SetMotorState(JOLT.EMotorState_Velocity) this.controlMode = DriverControlMode.Velocity + + this._gravityChange = (event: PreferenceEvent) => { + if (event.prefName == "SubsystemGravity") { + const motorSettings = this._constraint.GetMotorSettings() + if (event.prefValue) { + motorSettings.set_mMaxForceLimit(this._maxForceWithGrav) + motorSettings.set_mMinForceLimit(-this._maxForceWithGrav) + } else { + motorSettings.set_mMaxForceLimit(MAX_FORCE_WITHOUT_GRAV) + motorSettings.set_mMinForceLimit(-MAX_FORCE_WITHOUT_GRAV) + } + } + } + + PreferencesSystem.addEventListener(this._gravityChange) } public Update(_: number): void { if (this._controlMode == DriverControlMode.Velocity) { - this._constraint.SetTargetVelocity(this._targetVelocity) + this._constraint.SetTargetVelocity(this.accelerationDirection * this.maxVelocity) } else if (this._controlMode == DriverControlMode.Position) { - this._constraint.SetTargetPosition(this._targetPosition) + let pos = this._targetPosition + + if (pos - this._prevPos < -this.maxVelocity) pos = this._prevPos - this.maxVelocity + if (pos - this._prevPos > this.maxVelocity) pos = this._prevPos + this.maxVelocity + + this._constraint.SetTargetPosition(pos) } } } diff --git a/fission/src/systems/simulation/driver/WheelDriver.ts b/fission/src/systems/simulation/driver/WheelDriver.ts index 426e893f83..916a6c6a28 100644 --- a/fission/src/systems/simulation/driver/WheelDriver.ts +++ b/fission/src/systems/simulation/driver/WheelDriver.ts @@ -14,13 +14,25 @@ class WheelDriver extends Driver { public device?: string private _reversed: boolean - private _targetWheelSpeed: number = 0.0 + public accelerationDirection: number = 0.0 + private _prevVel: number = 0.0 + public maxVelocity = 30.0 + private _maxAcceleration = 1.5 - public get targetWheelSpeed(): number { - return this._targetWheelSpeed + private _targetVelocity = () => { + let vel = this.accelerationDirection * (this._reversed ? -1 : 1) * this.maxVelocity + + if (vel - this._prevVel < -this._maxAcceleration) vel = this._prevVel - this._maxAcceleration + if (vel - this._prevVel > this._maxAcceleration) vel = this._prevVel + this._maxAcceleration + + return vel } - public set targetWheelSpeed(radsPerSec: number) { - this._targetWheelSpeed = radsPerSec + + public get maxForce(): number { + return this._maxAcceleration + } + public set maxForce(acc: number) { + this._maxAcceleration = acc } public get constraint(): Jolt.VehicleConstraint { @@ -29,6 +41,7 @@ class WheelDriver extends Driver { public constructor( constraint: Jolt.VehicleConstraint, + maxVel: number, info?: mirabuf.IInfo, deviceType?: SimType, device?: string, @@ -37,6 +50,10 @@ class WheelDriver extends Driver { super(info) this._constraint = constraint + this.maxVelocity = maxVel + const controller = JOLT.castObject(this._constraint.GetController(), JOLT.WheeledVehicleController) + this._maxAcceleration = controller.GetEngine().mMaxTorque + this._reversed = reversed this.deviceType = deviceType this.device = device @@ -46,7 +63,9 @@ class WheelDriver extends Driver { } public Update(_: number): void { - this._wheel.SetAngularVelocity(this._targetWheelSpeed * (this._reversed ? -1 : 1)) + const vel = this._targetVelocity() + this._wheel.SetAngularVelocity(vel) + this._prevVel = vel } public set reversed(val: boolean) { diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index 57ca1a40bd..63bbba188d 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -298,9 +298,9 @@ export class PWMGroup extends SimOutputGroup { this.drivers.forEach(d => { if (d instanceof WheelDriver) { - d.targetWheelSpeed = average * 40 + d.accelerationDirection = average * 40 } else if (d instanceof HingeDriver || d instanceof SliderDriver) { - d.targetVelocity = average * 40 + d.accelerationDirection = average * 40 } d.Update(_deltaT) }) diff --git a/fission/src/ui/components/MainHUD.tsx b/fission/src/ui/components/MainHUD.tsx index 5c1e13a907..e42c0f00c9 100644 --- a/fission/src/ui/components/MainHUD.tsx +++ b/fission/src/ui/components/MainHUD.tsx @@ -95,6 +95,11 @@ const MainHUD: React.FC = () => { icon={SynthesisIcons.MagnifyingGlass} onClick={() => openModal("view")} /> */} + openPanel("subsystem-config")} + /> = ({ modalId }) => { const [renderScoreboard, setRenderScoreboard] = useState( PreferencesSystem.getGlobalPreference("RenderScoreboard") ) + const [subsystemGravity, setSubsystemGravity] = useState( + PreferencesSystem.getGlobalPreference("SubsystemGravity") + ) const saveSettings = () => { PreferencesSystem.setGlobalPreference("QualitySettings", qualitySettings) @@ -53,6 +56,7 @@ const SettingsModal: React.FC = ({ modalId }) => { PreferencesSystem.setGlobalPreference("RenderScoringZones", renderScoringZones) PreferencesSystem.setGlobalPreference("RenderSceneTags", renderSceneTags) PreferencesSystem.setGlobalPreference("RenderScoreboard", renderScoreboard) + PreferencesSystem.setGlobalPreference("SubsystemGravity", subsystemGravity) PreferencesSystem.savePreferences() } @@ -124,6 +128,13 @@ const SettingsModal: React.FC = ({ modalId }) => { setUseMetric(checked) }} /> + ("SubsystemGravity")} + onClick={checked => { + setSubsystemGravity(checked) + }} + /> ("RenderScoringZones")} diff --git a/fission/src/ui/panels/configuring/ConfigureSubsystemsPanel.tsx b/fission/src/ui/panels/configuring/ConfigureSubsystemsPanel.tsx new file mode 100644 index 0000000000..cf343f012f --- /dev/null +++ b/fission/src/ui/panels/configuring/ConfigureSubsystemsPanel.tsx @@ -0,0 +1,279 @@ +import { MiraType } from "@/mirabuf/MirabufLoader" +import MirabufSceneObject from "@/mirabuf/MirabufSceneObject" +import PreferencesSystem from "@/systems/preferences/PreferencesSystem" +import { RobotPreferences } from "@/systems/preferences/PreferenceTypes" +import Driver from "@/systems/simulation/driver/Driver" +import HingeDriver from "@/systems/simulation/driver/HingeDriver" +import SliderDriver from "@/systems/simulation/driver/SliderDriver" +import WheelDriver from "@/systems/simulation/driver/WheelDriver" +import World from "@/systems/World" +import Button from "@/ui/components/Button" +import Label, { LabelSize } from "@/ui/components/Label" +import Panel, { PanelPropsImpl } from "@/ui/components/Panel" +import ScrollView from "@/ui/components/ScrollView" +import Slider from "@/ui/components/Slider" +import Stack, { StackDirection } from "@/ui/components/Stack" +import { SectionDivider } from "@/ui/components/StyledComponents" +import { Box } from "@mui/material" +import { useCallback, useMemo, useState } from "react" +import { FaGear } from "react-icons/fa6" + +type SubsystemRowProps = { + robot: MirabufSceneObject + driver: Driver +} + +const SubsystemRow: React.FC = ({ robot, driver }) => { + const driverSwitch = (driver: Driver, slider: unknown, hinge: unknown, drivetrain: unknown) => { + switch (driver.constructor) { + case SliderDriver: + return slider + case HingeDriver: + return hinge + case WheelDriver: + return drivetrain + default: + return drivetrain + } + } + + const [velocity, setVelocity] = useState( + ((driver as SliderDriver) || (driver as HingeDriver) || (driver as WheelDriver)).maxVelocity + ) + const [force, setForce] = useState( + ((driver as SliderDriver) || (driver as HingeDriver) || (driver as WheelDriver)).maxForce + ) + + const onChange = useCallback( + (vel: number, force: number) => { + if (driver instanceof WheelDriver) { + const wheelDrivers = robot?.mechanism + ? World.SimulationSystem.GetSimulationLayer(robot.mechanism)?.drivers.filter( + x => x instanceof WheelDriver + ) + : undefined + wheelDrivers?.forEach(x => { + x.maxVelocity = vel + x.maxForce = force + }) + + // Preferences + PreferencesSystem.getRobotPreferences(robot.assemblyName).driveVelocity = vel + PreferencesSystem.getRobotPreferences(robot.assemblyName).driveAcceleration = force + } else { + // Preferences + if (driver.info && driver.info.name) { + const removedMotor = PreferencesSystem.getRobotPreferences(robot.assemblyName).motors + ? PreferencesSystem.getRobotPreferences(robot.assemblyName).motors.filter(x => { + if (x.name) return x.name != driver.info?.name + return false + }) + : [] + + removedMotor.push({ + name: driver.info?.name ?? "", + maxVelocity: vel, + maxForce: force, + }) + + PreferencesSystem.getRobotPreferences(robot.assemblyName).motors = removedMotor + } + + // eslint-disable-next-line no-extra-semi + ;((driver as SliderDriver) || (driver as HingeDriver)).maxVelocity = vel + ;((driver as SliderDriver) || (driver as HingeDriver)).maxForce = force + } + + PreferencesSystem.savePreferences() + }, + [driver, robot.mechanism, robot.assemblyName] + ) + + return ( + <> + + + + { + setVelocity(_velocity as number) + onChange(_velocity as number, force) + }} + step={0.01} + /> + {PreferencesSystem.getGlobalPreference("SubsystemGravity") || driver instanceof WheelDriver ? ( + { + setForce(_force as number) + onChange(velocity, _force as number) + }} + step={0.01} + /> + ) : driver instanceof HingeDriver ? ( + + ) : ( + + )} + + + + + ) +} + +const ConfigureSubsystemsPanel: React.FC = ({ panelId, openLocation, sidePadding }) => { + const [selectedRobot, setSelectedRobot] = useState(undefined) + const [origPref, setOrigPref] = useState(undefined) + + const robots = useMemo(() => { + const assemblies = [...World.SceneRenderer.sceneObjects.values()].filter(x => { + if (x instanceof MirabufSceneObject) { + return x.miraType === MiraType.ROBOT + } + return false + }) as MirabufSceneObject[] + return assemblies + }, []) + + const drivers = useMemo(() => { + return selectedRobot?.mechanism + ? World.SimulationSystem.GetSimulationLayer(selectedRobot.mechanism)?.drivers + : undefined + }, [selectedRobot]) + + // Gets motors in preferences for ease of saving into origPrefs which can be used to revert on Cancel() + function saveOrigMotors(robot: MirabufSceneObject) { + drivers?.forEach(driver => { + if (driver.info && driver.info.name && !(driver instanceof WheelDriver)) { + const motors = PreferencesSystem.getRobotPreferences(robot.assemblyName).motors + const removedMotor = motors.filter(x => { + if (x.name) return x.name != driver.info?.name + return false + }) + + if (removedMotor.length == drivers.length) { + removedMotor.push({ + name: driver.info?.name ?? "", + maxVelocity: ((driver as SliderDriver) || (driver as HingeDriver)).maxVelocity, + maxForce: ((driver as SliderDriver) || (driver as HingeDriver)).maxForce, + }) + PreferencesSystem.getRobotPreferences(robot.assemblyName).motors = removedMotor + } + } + }) + PreferencesSystem.savePreferences() + setOrigPref({ ...PreferencesSystem.getRobotPreferences(robot.assemblyName) }) // clone + } + + function Cancel() { + if (selectedRobot && origPref) { + drivers?.forEach(driver => { + if (driver instanceof WheelDriver) { + driver.maxVelocity = origPref.driveVelocity + driver.maxForce = origPref.driveAcceleration + } else { + if (driver.info && driver.info.name) { + const motor = origPref.motors.filter(x => { + if (x.name) return x.name == driver.info?.name + return false + })[0] + if (motor) { + // This line is a separate variable to get ES Lint and Prettier to agree on formatting the semicolon below + const forcePref = PreferencesSystem.getGlobalPreference("SubsystemGravity") + ? motor.maxForce + : driver instanceof SliderDriver + ? 500 + : 100 + ;((driver as SliderDriver) || (driver as HingeDriver)).maxVelocity = motor.maxVelocity + ;((driver as SliderDriver) || (driver as HingeDriver)).maxForce = forcePref + } + } + } + }) + PreferencesSystem.setRobotPreferences(selectedRobot.assemblyName, origPref) + } + PreferencesSystem.savePreferences() + } + + return ( + } + panelId={panelId} + openLocation={openLocation} + sidePadding={sidePadding} + onAccept={() => { + PreferencesSystem.savePreferences() + }} + onCancel={Cancel} + acceptEnabled={true} + > + {selectedRobot?.ejectorPreferences == undefined ? ( + <> + + {/** Scroll view for selecting a robot to configure */} +
+ {robots.map(mirabufSceneObject => { + return ( + + ) + })} +
+ + ) : ( + <> + {drivers ? ( + + {/** Drivetrain row. Then other SliderDrivers and HingeDrivers */} + { + return selectedRobot + })()} + driver={(() => { + return drivers.filter(x => x instanceof WheelDriver)[0] + })()} + /> + {drivers + .filter(x => x instanceof SliderDriver || x instanceof HingeDriver) + .map((driver: Driver, i: number) => ( + { + return selectedRobot + })()} + driver={(() => { + return driver + })()} + /> + ))} + + ) : ( + + )} + + )} +
+ ) +} + +export default ConfigureSubsystemsPanel