From 2c557d62e7e50a15cd39717496251e127364f309 Mon Sep 17 00:00:00 2001 From: PepperLola Date: Tue, 20 Aug 2024 17:58:36 -0700 Subject: [PATCH 01/16] gyro angle support, rate and robot-side outputs TODO --- .../simulation/wpilib_brain/SimInput.ts | 11 +- .../simulation/wpilib_brain/WPILibBrain.ts | 13 +- .../java/com/autodesk/synthesis/Gyro.java | 121 ++++++++++++++++++ .../src/main/java/frc/robot/Robot.java | 11 +- 4 files changed, 138 insertions(+), 18 deletions(-) create mode 100644 simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/Gyro.java diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index 6061d9a47e..a65a9e6c7b 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -40,12 +40,11 @@ export class SimGyroInput implements SimInput { this._robot = robot this._joltID = this._robot.nodeToBody.get(this._robot.rootBody) - if (this._joltID) - this._joltBody = World.PhysicsSystem.GetBody(this._joltID) + 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 + return ((this._joltBody?.GetRotation().GetRotationAngle(axis) ?? 0) * 180) / Math.PI } private GetX(): number { @@ -65,8 +64,8 @@ export class SimGyroInput implements SimInput { const y = this.GetY() const z = this.GetZ() // console.log(`${this._device}\n${x}\n${y}\n${z}`) - SimGyro.SetAngleX(this._device, x); - SimGyro.SetAngleY(this._device, y); - SimGyro.SetAngleZ(this._device, z); + SimGyro.SetAngleX(this._device, x) + SimGyro.SetAngleY(this._device, y) + SimGyro.SetAngleZ(this._device, z) } } diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index 1e83bc686e..57b7b4e453 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -161,15 +161,15 @@ export class SimGyro { private constructor() {} public static SetAngleX(device: string, angle: number): boolean { - return SimGeneric.Set("Gyro", device, ">angle_x", angle); + return SimGeneric.Set("Gyro", device, ">angle_x", angle) } public static SetAngleY(device: string, angle: number): boolean { - return SimGeneric.Set("Gyro", device, ">angle_y", angle); + return SimGeneric.Set("Gyro", device, ">angle_y", angle) } public static SetAngleZ(device: string, angle: number): boolean { - return SimGeneric.Set("Gyro", device, ">angle_z", angle); + return SimGeneric.Set("Gyro", device, ">angle_z", angle) } } @@ -209,6 +209,9 @@ worker.addEventListener("message", (eventData: MessageEvent) => { case "CANEncoder": UpdateSimMap("CANEncoder", device, updateData) break + case "Gyro": + UpdateSimMap("Gyro", device, updateData) + break default: break } @@ -247,7 +250,7 @@ class WPILibBrain extends Brain { return } - this.addSimInput(new SimGyroInput("Gyro:ADXRS450[0]", mechanism)); + this.addSimInput(new SimGyroInput("Test Gyro[1]", mechanism)) } public addSimOutputGroup(device: SimOutputGroup) { @@ -261,7 +264,7 @@ class WPILibBrain extends Brain { public Update(deltaT: number): void { this._simOutputs.forEach(d => d.Update(deltaT)) this._simInputs.forEach(i => i.Update(deltaT)) - console.log(simMap) + // console.log(simMap) } public Enable(): void { diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/Gyro.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/Gyro.java new file mode 100644 index 0000000000..c17f1af4ab --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/Gyro.java @@ -0,0 +1,121 @@ +package com.autodesk.synthesis; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDevice.Direction; +import edu.wpi.first.hal.SimDouble; + +/** + * Gyro class for easy implementation of documentation-compliant simulation data. + * + * See https://github.com/wpilibsuite/allwpilib/blob/6478ba6e3fa317ee041b8a41e562d925602b6ea4/simulation/halsim_ws_core/doc/hardware_ws_api.md + * for documentation on the WebSocket API Specification. + */ +public class Gyro { + + private SimDevice m_device; + + private SimDouble m_range; + private SimBoolean m_connected; + private SimDouble m_angleX; + private SimDouble m_angleY; + private SimDouble m_angleZ; + private SimDouble m_rateX; + private SimDouble m_rateY; + private SimDouble m_rateZ; + + /** + * Creates a CANMotor sim device in accordance with the WebSocket API Specification. + * + * @param name Name of the Gyro. This is generally the class name of the originating gyro (i.e. "ADXRS450"). + * @param deviceId ID of the Gyro. + */ + public Gyro(String name, int deviceId) { + m_device = SimDevice.create("Gyro:" + name, deviceId); + + m_range = m_device.createDouble("range", Direction.kOutput, 0.0); + m_connected = m_device.createBoolean("connected", Direction.kOutput, false); + m_angleX = m_device.createDouble("angle_x", Direction.kInput, 0.0); + m_angleY = m_device.createDouble("angle_y", Direction.kInput, 0.0); + m_angleZ = m_device.createDouble("angle_z", Direction.kInput, 0.0); + m_rateX = m_device.createDouble("rate_x", Direction.kInput, 0.0); + m_rateY = m_device.createDouble("rate_y", Direction.kInput, 0.0); + m_rateZ = m_device.createDouble("rate_z", Direction.kInput, 0.0); + } + + /** + * Set the range of the gyro. + * + * @param range Range of the gyro + */ + public void setRange(double range) { + if (Double.isNaN(range) || Double.isInfinite(range)) { + range = 0.0; + } + + m_range.set(range); + } + + /** + * Set whether the gyro is connected. + * + * @param connected Whether the gyro is connected + */ + public void setConnected(boolean connected) { + m_connected.set(connected); + } + + /** + * Get the angleX of the gyro. + * + * @return angleX + */ + public double getAngleX() { + return m_angleX.get(); + } + + /** + * Get the angleY of the gyro. + * + * @return angleY + */ + public double getAngleY() { + return m_angleY.get(); + } + + /** + * Get the angleZ of the gyro. + * + * @return angleZ + */ + public double getAngleZ() { + return m_angleZ.get(); + } + + /** + * Get the rateX of the gyro. + * + * @return rateX + */ + public double getRateX() { + return m_rateX.get(); + } + + /** + * Get the rateY of the gyro. + * + * @return rateY + */ + public double getRateY() { + return m_rateY.get(); + } + + /** + * Get the rateZ of the gyro. + * + * @return rateZ + */ + public double getRateZ() { + return m_rateZ.get(); + } +} diff --git a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java index 3dc569c051..70cfac85d9 100644 --- a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java +++ b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java @@ -19,7 +19,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import com.autodesk.synthesis.revrobotics.CANSparkMax; -import com.autodesk.synthesis.Joystick; +// import com.autodesk.synthesis.Joystick; +import com.autodesk.synthesis.Gyro; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -38,8 +39,7 @@ public class Robot extends TimedRobot { private CANSparkMax m_SparkMax = new CANSparkMax(1, MotorType.kBrushless); private TalonFX m_Talon = new TalonFX(2); private XboxController m_Controller = new XboxController(0); - private ADXRS450_Gyro m_Gyro; - private ADXRS450_GyroSim m_GyroSim; + private Gyro m_Gyro = new Gyro("Test Gyro", 1); /** * This function is run when the robot is first started up and should be used for any @@ -50,9 +50,6 @@ public void robotInit() { m_chooser.setDefaultOption("Default Auto", kDefaultAuto); m_chooser.addOption("My Auto", kCustomAuto); SmartDashboard.putData("Auto choices", m_chooser); - m_Gyro = new ADXRS450_Gyro(); - m_GyroSim = new ADXRS450_GyroSim(m_Gyro); - m_Gyro.calibrate(); } /** @@ -111,7 +108,7 @@ public void teleopInit() {} public void teleopPeriodic() { m_Spark1.set(m_Controller.getLeftY()); m_Spark2.set(-m_Controller.getRightY()); - System.out.println(m_Gyro.getAngle()); + System.out.println(m_Gyro.getAngleY()); // m_Spark1.set(0.25); // m_Spark2.set(0.25); m_SparkMax.set(0.75); From 8ce978d0f19320703299851c9d6befe31067d6f7 Mon Sep 17 00:00:00 2001 From: PepperLola Date: Wed, 21 Aug 2024 14:46:39 -0700 Subject: [PATCH 02/16] gyro rate works --- .../systems/simulation/wpilib_brain/SimInput.ts | 17 +++++++++++++++++ .../simulation/wpilib_brain/WPILibBrain.ts | 12 ++++++++++++ 2 files changed, 29 insertions(+) diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index a65a9e6c7b..97a285cfc0 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -59,6 +59,20 @@ export class SimGyroInput implements SimInput { return this.GetAxis(SimGyroInput.AXIS_Z) } + private GetAxisVelocity(axis: "x" | "y" | "z"): number { + const axes = this._joltBody?.GetAngularVelocity() + if (!axes) return 0 + + switch (axis) { + case "x": + return axes.GetX() + case "y": + return axes.GetY() + case "z": + return axes.GetZ() + } + } + public Update(_deltaT: number) { const x = this.GetX() const y = this.GetY() @@ -67,5 +81,8 @@ export class SimGyroInput implements SimInput { SimGyro.SetAngleX(this._device, x) SimGyro.SetAngleY(this._device, y) SimGyro.SetAngleZ(this._device, z) + SimGyro.SetRateX(this._device, this.GetAxisVelocity("x")) + SimGyro.SetRateY(this._device, this.GetAxisVelocity("y")) + SimGyro.SetRateZ(this._device, this.GetAxisVelocity("z")) } } diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index 57b7b4e453..e5edd9de2f 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -171,6 +171,18 @@ export class SimGyro { public static SetAngleZ(device: string, angle: number): boolean { return SimGeneric.Set("Gyro", device, ">angle_z", angle) } + + public static SetRateX(device: string, rate: number): boolean { + return SimGeneric.Set("Gyro", device, ">rate_x", rate) + } + + public static SetRateY(device: string, rate: number): boolean { + return SimGeneric.Set("Gyro", device, ">rate_y", rate) + } + + public static SetRateZ(device: string, rate: number): boolean { + return SimGeneric.Set("Gyro", device, ">rate_z", rate) + } } worker.addEventListener("message", (eventData: MessageEvent) => { From 72af99237a6d768cf14e9a1808486ede8360f4e0 Mon Sep 17 00:00:00 2001 From: PepperLola Date: Wed, 21 Aug 2024 15:38:30 -0700 Subject: [PATCH 03/16] accelerometer data is sent but don't know how to get acceleration from Jolt --- .../simulation/wpilib_brain/SimInput.ts | 39 +++++++- .../simulation/wpilib_brain/WPILibBrain.ts | 38 +++++-- fission/src/ui/panels/WSViewPanel.tsx | 99 +++---------------- .../src/main/java/frc/robot/Robot.java | 9 ++ 4 files changed, 88 insertions(+), 97 deletions(-) diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index 97a285cfc0..d74940619d 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -1,6 +1,6 @@ import World from "@/systems/World" import EncoderStimulus from "../stimulus/EncoderStimulus" -import { SimCANEncoder, SimGyro } from "./WPILibBrain" +import { SimCANEncoder, SimGyro, SimAccel } from "./WPILibBrain" import Mechanism from "@/systems/physics/Mechanism" import Jolt from "@barclah/jolt-physics" import JOLT from "@/util/loading/JoltSyncLoader" @@ -77,7 +77,7 @@ export class SimGyroInput implements SimInput { const x = this.GetX() const y = this.GetY() const z = this.GetZ() - // console.log(`${this._device}\n${x}\n${y}\n${z}`) + SimGyro.SetAngleX(this._device, x) SimGyro.SetAngleY(this._device, y) SimGyro.SetAngleZ(this._device, z) @@ -86,3 +86,38 @@ export class SimGyroInput implements SimInput { SimGyro.SetRateZ(this._device, this.GetAxisVelocity("z")) } } + +export class SimAccelInput implements SimInput { + private _device: string + private _robot: Mechanism + private _joltID?: Jolt.BodyID + private _joltBody?: Jolt.Body + + 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: "x" | "y" | "z"): number { + const forces = this._joltBody?.GetAccumulatedForce() + if (!forces) return 0 + + switch (axis) { + case "x": + return forces.GetX() + case "y": + return forces.GetY() + case "z": + return forces.GetZ() + } + } + + public Update(_deltaT: number) { + SimAccel.SetX(this._device, this.GetAxis("x")) + SimAccel.SetY(this._device, this.GetAxis("y")) + SimAccel.SetZ(this._device, this.GetAxis("z")) + } +} diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index e5edd9de2f..30282dc2f5 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -7,7 +7,7 @@ import { SimulationLayer } from "../SimulationSystem" import World from "@/systems/World" import { SimOutputGroup } from "./SimOutput" -import { SimGyroInput, SimInput } from "./SimInput" +import { SimAccelInput, SimGyroInput, SimInput } from "./SimInput" const worker = new WPILibWSWorker() @@ -17,7 +17,7 @@ const CANMOTOR_DUTY_CYCLE = ">() export class SimGeneric { - private constructor() {} + private constructor() { } public static Get(simType: SimType, device: string, field: string, defaultValue?: T): T | undefined { const fieldType = GetFieldType(field) @@ -106,7 +106,7 @@ export class SimGeneric { } export class SimPWM { - private constructor() {} + private constructor() { } public static GetSpeed(device: string): number | undefined { return SimGeneric.Get("PWM", device, PWM_SPEED, 0.0) @@ -118,7 +118,7 @@ export class SimPWM { } export class SimCAN { - private constructor() {} + private constructor() { } public static GetDeviceWithID(id: number, type: SimType): any { const id_exp = /.*\[(\d+)\]/g @@ -138,7 +138,7 @@ export class SimCAN { } export class SimCANMotor { - private constructor() {} + private constructor() { } public static GetDutyCycle(device: string): number | undefined { return SimGeneric.Get("CANMotor", device, CANMOTOR_DUTY_CYCLE, 0.0) @@ -150,7 +150,7 @@ export class SimCANMotor { } export class SimCANEncoder { - private constructor() {} + private constructor() { } public static SetRawInputPosition(device: string, rawInputPosition: number): boolean { return SimGeneric.Set("CANEncoder", device, CANENCODER_RAW_INPUT_POSITION, rawInputPosition) @@ -158,7 +158,7 @@ export class SimCANEncoder { } export class SimGyro { - private constructor() {} + private constructor() { } public static SetAngleX(device: string, angle: number): boolean { return SimGeneric.Set("Gyro", device, ">angle_x", angle) @@ -185,6 +185,22 @@ export class SimGyro { } } +export class SimAccel { + private constructor() { } + + public static SetX(device: string, accel: number): boolean { + return SimGeneric.Set("Accel", device, ">x", accel) + } + + public static SetY(device: string, accel: number): boolean { + return SimGeneric.Set("Accel", device, ">y", accel) + } + + public static SetZ(device: string, accel: number): boolean { + return SimGeneric.Set("Accel", device, ">z", accel) + } +} + worker.addEventListener("message", (eventData: MessageEvent) => { let data: any | undefined try { @@ -224,6 +240,9 @@ worker.addEventListener("message", (eventData: MessageEvent) => { case "Gyro": UpdateSimMap("Gyro", device, updateData) break + case "Accel": + UpdateSimMap("Accel", device, updateData) + break default: break } @@ -262,7 +281,8 @@ class WPILibBrain extends Brain { return } - this.addSimInput(new SimGyroInput("Test Gyro[1]", mechanism)) + // this.addSimInput(new SimGyroInput("Test Gyro[1]", mechanism)) + this.addSimInput(new SimAccelInput("ADXL362[4]", mechanism)) } public addSimOutputGroup(device: SimOutputGroup) { diff --git a/fission/src/ui/panels/WSViewPanel.tsx b/fission/src/ui/panels/WSViewPanel.tsx index f7b47eff28..31699e2164 100644 --- a/fission/src/ui/panels/WSViewPanel.tsx +++ b/fission/src/ui/panels/WSViewPanel.tsx @@ -27,16 +27,19 @@ const TypoStyled = styled(Typography)({ }) function generateTableBody() { + const names: SimType[] = ["PWM", "SimDevice", "CANMotor", "CANEncoder", "Gyro", "Accel"] + return ( - {simMap.has("PWM") ? ( - [...simMap.get("PWM")!.entries()] - .filter(x => x[1][" { - return ( + {names.map(name => + simMap.has(name) ? ( + [...simMap.get(name)!.entries()] + // most devices don't have !Object.keys(x[1]).includes(" ( - PWM + {name} {x[0]} @@ -45,86 +48,10 @@ function generateTableBody() { {JSON.stringify(x[1])} - ) - }) - ) : ( - <> - )} - {simMap.has("SimDevice") ? ( - [...simMap.get("SimDevice")!.entries()].map(x => { - return ( - - - SimDevice - - - {x[0]} - - - {JSON.stringify(x[1])} - - - ) - }) - ) : ( - <> - )} - {simMap.has("CANMotor") ? ( - [...simMap.get("CANMotor")!.entries()].map(x => { - return ( - - - CAN Motor - - - {x[0]} - - - {JSON.stringify(x[1])} - - - ) - }) - ) : ( - <> - )} - {simMap.has("CANEncoder") ? ( - [...simMap.get("CANEncoder")!.entries()].map(x => { - return ( - - - CAN Encoder - - - {x[0]} - - - {JSON.stringify(x[1])} - - - ) - }) - ) : ( - <> - )} - {simMap.has("Gyro") ? ( - [...simMap.get("Gyro")!.entries()].map(x => { - return ( - - - Gyro - - - {x[0]} - - - {JSON.stringify(x[1])} - - - ) - }) - ) : ( - <> + )) + ) : ( + <> + ) )} ) diff --git a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java index 70cfac85d9..3a8a2b4901 100644 --- a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java +++ b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java @@ -4,13 +4,17 @@ package frc.robot; +import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; +import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANSparkBase.IdleMode; // import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.wpilibj.ADXL362; import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.motorcontrol.Spark; @@ -40,6 +44,11 @@ public class Robot extends TimedRobot { private TalonFX m_Talon = new TalonFX(2); private XboxController m_Controller = new XboxController(0); private Gyro m_Gyro = new Gyro("Test Gyro", 1); + private AHRS m_NavX = new AHRS(); + // Creates an ADXL362 accelerometer object on the MXP SPI port + // with a measurement range from -8 to 8 G's + private ADXL362 m_Accelerometer = new ADXL362(SPI.Port.kMXP, ADXL362.Range.k8G); + private Pigeon2 m_Pigeon2 = new Pigeon2(0); /** * This function is run when the robot is first started up and should be used for any From 9f9948d275d78ebc08ccdf76f996b5e89e42133e Mon Sep 17 00:00:00 2001 From: PepperLola Date: Thu, 22 Aug 2024 17:53:53 -0700 Subject: [PATCH 04/16] hopefully fission-side DIO/AIO is done --- .../simulation/wpilib_brain/SimInput.ts | 44 ++++++- .../simulation/wpilib_brain/SimOutput.ts | 28 ++++- .../simulation/wpilib_brain/WPILibBrain.ts | 114 ++++++++++++++++-- .../rio-config/RCConfigCANGroupModal.tsx | 2 +- .../rio-config/RCConfigPWMGroupModal.tsx | 2 +- fission/src/ui/panels/WSViewPanel.tsx | 6 +- .../src/main/java/frc/robot/Robot.java | 18 ++- 7 files changed, 191 insertions(+), 23 deletions(-) diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index 87b0367969..974503b5b0 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -1,6 +1,6 @@ import World from "@/systems/World" import EncoderStimulus from "../stimulus/EncoderStimulus" -import { SimCANEncoder, SimGyro, SimAccel } from "./WPILibBrain" +import { SimCANEncoder, SimGyro, SimAccel, SimDIO as WSSimDIO, SimAI } from "./WPILibBrain" import Mechanism from "@/systems/physics/Mechanism" import Jolt from "@barclah/jolt-physics" import JOLT from "@/util/loading/JoltSyncLoader" @@ -120,3 +120,45 @@ export class SimAccelInput implements SimInput { SimAccel.SetZ(this._device, this.GetAxis("z")) } } + +export class SimDIO implements SimInput { + private _device: string + private _valueSupplier?: () => boolean + + /** + * Creates a Simulation Digital Input/Output object. + * + * @param device Device ID + * @param valueSupplier Called each frame and returns what the value should be set to. Don't specify if DIO should be treated as an output. + */ + constructor(device: string, valueSupplier?: () => boolean) { + this._device = device + this._valueSupplier = valueSupplier + } + + public SetValue(value: boolean) { + WSSimDIO.SetValue(this._device, value); + } + + public GetValue(): boolean { + return WSSimDIO.GetValue(this._device) + } + + public Update(_deltaT: number) { + if (this._valueSupplier) this.SetValue(this._valueSupplier()) + } +} + +export class SimAnalogInput implements SimInput { + private _device: string + private _valueSupplier: () => number + + constructor(device: string, valueSupplier: () => number) { + this._device = device + this._valueSupplier = valueSupplier + } + + public Update(_deltaT: number) { + SimAI.SetValue(this._device, this._valueSupplier()) + } +} diff --git a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts index 40fe5494e9..a4ce35d7ff 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts @@ -2,7 +2,7 @@ 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" +import { SimAO, 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 @@ -10,14 +10,24 @@ import { SimCAN, SimPWM, SimType } from "./WPILibBrain" // 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 { + +export abstract class SimOutput { public name: string + + constructor(name: string) { + this.name = name + } + + public abstract Update(deltaT: number): void +} + +export abstract class SimOutputGroup extends SimOutput { public ports: number[] public drivers: Driver[] public type: SimType public constructor(name: string, ports: number[], drivers: Driver[], type: SimType) { - this.name = name + super(name) this.ports = ports this.drivers = drivers this.type = type @@ -72,3 +82,15 @@ export class CANOutputGroup extends SimOutputGroup { }) } } + +export class SimAnalogOutput extends SimOutput { + public constructor(name: string) { + super(name) + } + + public GetVoltage(): number { + return SimAO.GetVoltage(this.name) + } + + public Update(_deltaT: number) { } +} diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index 1e046a68a7..a6495ac20a 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -5,8 +5,8 @@ import WPILibWSWorker from "./WPILibWSWorker?worker" import { SimulationLayer } from "../SimulationSystem" import World from "@/systems/World" -import { SimOutputGroup } from "./SimOutput" -import { SimAccelInput, SimGyroInput, SimInput } from "./SimInput" +import { SimAnalogOutput, SimOutput, SimOutputGroup } from "./SimOutput" +import { SimAccelInput, SimInput, SimDIO as SimDIOIn, SimAnalogInput } from "./SimInput" const worker = new WPILibWSWorker() const PWM_SPEED = ">() export class SimGeneric { private constructor() { } + public static Get(simType: SimType, device: string, field: string): T | undefined; + public static Get(simType: SimType, device: string, field: string, defaultValue: T): T; public static Get(simType: SimType, device: string, field: string, defaultValue?: T): T | undefined { const fieldType = GetFieldType(field) if (fieldType != FieldType.Read && fieldType != FieldType.Both) { @@ -195,27 +201,27 @@ export class SimGyro { private constructor() { } public static SetAngleX(device: string, angle: number): boolean { - return SimGeneric.Set("Gyro", device, ">angle_x", angle) + return SimGeneric.Set(SimType.Gyro, device, ">angle_x", angle) } public static SetAngleY(device: string, angle: number): boolean { - return SimGeneric.Set("Gyro", device, ">angle_y", angle) + return SimGeneric.Set(SimType.Gyro, device, ">angle_y", angle) } public static SetAngleZ(device: string, angle: number): boolean { - return SimGeneric.Set("Gyro", device, ">angle_z", angle) + return SimGeneric.Set(SimType.Gyro, device, ">angle_z", angle) } public static SetRateX(device: string, rate: number): boolean { - return SimGeneric.Set("Gyro", device, ">rate_x", rate) + return SimGeneric.Set(SimType.Gyro, device, ">rate_x", rate) } public static SetRateY(device: string, rate: number): boolean { - return SimGeneric.Set("Gyro", device, ">rate_y", rate) + return SimGeneric.Set(SimType.Gyro, device, ">rate_y", rate) } public static SetRateZ(device: string, rate: number): boolean { - return SimGeneric.Set("Gyro", device, ">rate_z", rate) + return SimGeneric.Set(SimType.Gyro, device, ">rate_z", rate) } } @@ -223,15 +229,92 @@ export class SimAccel { private constructor() { } public static SetX(device: string, accel: number): boolean { - return SimGeneric.Set("Accel", device, ">x", accel) + return SimGeneric.Set(SimType.Accel, device, ">x", accel) } public static SetY(device: string, accel: number): boolean { - return SimGeneric.Set("Accel", device, ">y", accel) + return SimGeneric.Set(SimType.Accel, device, ">y", accel) } public static SetZ(device: string, accel: number): boolean { - return SimGeneric.Set("Accel", device, ">z", accel) + return SimGeneric.Set(SimType.Accel, device, ">z", accel) + } +} + +export class SimDIO { + private constructor() { } + + public static SetValue(device: string, value: boolean): boolean { + return SimGeneric.Set(SimType.DIO, device, "<>value", +value); + } + + public static GetValue(device: string): boolean { + return SimGeneric.Get(SimType.DIO, device, "<>value", false) + } +} + +export class SimAI { + constructor() { } + + public static SetValue(device: string, value: number): boolean { + return SimGeneric.Set(SimType.AI, device, ">voltage", value) + } + + /** + * The number of averaging bits + */ + public static GetAvgBits(device: string) { + return SimGeneric.Get(SimType.AI, device, "voltage", voltage) + } + /** + * If the accumulator is initialized in the robot program + */ + public static GetAccumInit(device: string) { + return SimGeneric.Get(SimType.AI, device, "accum_value", accum_value) + } + /** + * The number of accumulated values + */ + public static SetAccumCount(device: string, accum_count: number) { + return SimGeneric.Set(SimType.AI, device, ">accum_count", accum_count) + } + /** + * The center value of the accumulator + */ + public static GetAccumCenter(device: string) { + return SimGeneric.Get(SimType.AI, device, "voltage", 0.0) } } @@ -282,7 +365,7 @@ function UpdateSimMap(type: SimType, device: string, updateData: DeviceData) { class WPILibBrain extends Brain { private _simLayer: SimulationLayer - private _simOutputs: SimOutputGroup[] = [] + private _simOutputs: SimOutput[] = [] private _simInputs: SimInput[] = [] constructor(mechanism: Mechanism) { @@ -297,9 +380,13 @@ class WPILibBrain extends Brain { // this.addSimInput(new SimGyroInput("Test Gyro[1]", mechanism)) // this.addSimInput(new SimAccelInput("ADXL362[4]", mechanism)) + // this.addSimInput(new SimDIOIn("In[0]", () => Math.random() > 0.5)) + // this.addSimInput(new SimDIOIn("Out[1]")) + // this.addSimInput(new SimAnalogInput("In[0]", () => Math.random() * 12)) + // this.addSimOutput(new SimAnalogOutput("Out[1]")) } - public addSimOutputGroup(device: SimOutputGroup) { + public addSimOutput(device: SimOutput) { this._simOutputs.push(device) } @@ -310,6 +397,7 @@ class WPILibBrain extends Brain { public Update(deltaT: number): void { this._simOutputs.forEach(d => d.Update(deltaT)) this._simInputs.forEach(i => i.Update(deltaT)) + console.log(simMap) } public Enable(): void { diff --git a/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx b/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx index 624e133708..1584ba3d50 100644 --- a/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx +++ b/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx @@ -47,7 +47,7 @@ const RCConfigCANGroupModal: React.FC = ({ modalId }) => { acceptName="Done" onAccept={() => { // no eslint complain - brain.addSimOutputGroup(new CANOutputGroup(name, checkedPorts, checkedDrivers)) + brain.addSimOutput(new CANOutputGroup(name, checkedPorts, checkedDrivers)) console.log(name, checkedPorts, checkedDrivers) const replacer = (_: unknown, value: unknown) => { if (value instanceof Map) { diff --git a/fission/src/ui/modals/configuring/rio-config/RCConfigPWMGroupModal.tsx b/fission/src/ui/modals/configuring/rio-config/RCConfigPWMGroupModal.tsx index 700094449c..3a227dc4a2 100644 --- a/fission/src/ui/modals/configuring/rio-config/RCConfigPWMGroupModal.tsx +++ b/fission/src/ui/modals/configuring/rio-config/RCConfigPWMGroupModal.tsx @@ -47,7 +47,7 @@ const RCConfigPWMGroupModal: React.FC = ({ modalId }) => { acceptName="Done" onAccept={() => { // no eslint complain - brain.addSimOutputGroup(new PWMOutputGroup(name, checkedPorts, checkedDrivers)) + brain.addSimOutput(new PWMOutputGroup(name, checkedPorts, checkedDrivers)) console.log(name, checkedPorts, checkedDrivers) }} onCancel={() => { diff --git a/fission/src/ui/panels/WSViewPanel.tsx b/fission/src/ui/panels/WSViewPanel.tsx index 6e90cfbaff..9dd2b6fbdd 100644 --- a/fission/src/ui/panels/WSViewPanel.tsx +++ b/fission/src/ui/panels/WSViewPanel.tsx @@ -35,7 +35,9 @@ function formatMap(map: Map): string { } function generateTableBody() { - const names: SimType[] = ["PWM", "SimDevice", "CANMotor", "CANEncoder", "Gyro", "Accel"] + const names: SimType[] = [SimType.PWM, SimType.SimDevice, SimType.CANMotor, SimType.CANEncoder, SimType.Gyro, SimType.Accel, SimType.DIO, SimType.AI, SimType.AO] + + console.log(simMap) return ( @@ -43,7 +45,7 @@ function generateTableBody() { simMap.has(name) ? ( [...simMap.get(name)!.entries()] // most devices don't have !Object.keys(x[1]).includes(" !Object.keys(x[1]).includes(" ( diff --git a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java index e3febe19b3..891e77651f 100644 --- a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java +++ b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java @@ -13,6 +13,10 @@ import edu.wpi.first.wpilibj.ADXL362; import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.AnalogOutput; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.TimedRobot; @@ -50,6 +54,10 @@ public class Robot extends TimedRobot { // with a measurement range from -8 to 8 G's private ADXL362 m_Accelerometer = new ADXL362(SPI.Port.kMXP, ADXL362.Range.k8G); private Pigeon2 m_Pigeon2 = new Pigeon2(0); + private DigitalInput m_DI = new DigitalInput(0); + private DigitalOutput m_DO = new DigitalOutput(1); + private AnalogInput m_AI = new AnalogInput(0); + private AnalogOutput m_AO = new AnalogOutput(1); private CANSparkMax m_SparkMax1 = new CANSparkMax(1, MotorType.kBrushless); private CANSparkMax m_SparkMax2 = new CANSparkMax(2, MotorType.kBrushless); @@ -94,6 +102,8 @@ public void autonomousInit() { m_autoSelected = m_chooser.getSelected(); // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto); System.out.println("Auto selected: " + m_autoSelected); + m_DO.set(true); + m_AO.setVoltage(0.0); } /** This function is called periodically during autonomous. */ @@ -135,14 +145,17 @@ public void autonomousPeriodic() { /** This function is called once when teleop is enabled. */ @Override - public void teleopInit() { } + public void teleopInit() { + m_DO.set(false); + m_AO.setVoltage(6.0); + } /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() { m_Spark1.set(m_Controller.getLeftY()); m_Spark2.set(-m_Controller.getRightY()); - System.out.println(m_Gyro.getAngleY()); + System.out.println(m_DI.get()); // m_Spark1.set(0.25); // m_Spark2.set(0.25); m_Talon.set(-0.5); @@ -164,6 +177,7 @@ public void disabledInit() { m_SparkMax4.set(0.0); m_SparkMax5.set(0.0); m_SparkMax6.set(0.0); + m_AO.setVoltage(12.0); } /** This function is called periodically when disabled. */ From 88c551b7b3bbcd0f95fe9632d52954799577ec42 Mon Sep 17 00:00:00 2001 From: PepperLola Date: Mon, 26 Aug 2024 16:36:37 -0700 Subject: [PATCH 05/16] digital and analog inputs work but can't be configured show joint name in CAN config panel update robot code to use all the sim things --- fission/src/Synthesis.tsx | 6 -- .../simulation/wpilib_brain/SimOutput.ts | 1 - .../simulation/wpilib_brain/WPILibBrain.ts | 23 ++--- .../rio-config/RCConfigCANGroupModal.tsx | 14 +-- .../autodesk/synthesis/io/AnalogInput.java | 90 +++++++++++++++++++ .../autodesk/synthesis/io/AnalogOutput.java | 32 +++++++ .../autodesk/synthesis/io/DigitalInput.java | 33 +++++++ .../autodesk/synthesis/io/DigitalOutput.java | 38 ++++++++ .../src/main/java/frc/robot/Robot.java | 27 ++---- 9 files changed, 212 insertions(+), 52 deletions(-) create mode 100644 simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/AnalogInput.java create mode 100644 simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/AnalogOutput.java create mode 100644 simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/DigitalInput.java create mode 100644 simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/DigitalOutput.java diff --git a/fission/src/Synthesis.tsx b/fission/src/Synthesis.tsx index badb33a281..7052b887d5 100644 --- a/fission/src/Synthesis.tsx +++ b/fission/src/Synthesis.tsx @@ -47,9 +47,7 @@ import ChooseInputSchemePanel from "./ui/panels/configuring/ChooseInputSchemePan import ProgressNotifications from "./ui/components/ProgressNotification.tsx" import SceneOverlay from "./ui/components/SceneOverlay.tsx" -import WPILibWSWorker from "@/systems/simulation/wpilib_brain/WPILibWSWorker.ts?worker" import WSViewPanel from "./ui/panels/WSViewPanel.tsx" -import Lazy from "./util/Lazy.ts" import RCConfigPWMGroupModal from "@/modals/configuring/rio-config/RCConfigPWMGroupModal.tsx" import RCConfigCANGroupModal from "@/modals/configuring/rio-config/RCConfigCANGroupModal.tsx" @@ -61,8 +59,6 @@ import PreferencesSystem from "./systems/preferences/PreferencesSystem.ts" import APSManagementModal from "./ui/modals/APSManagementModal.tsx" import ConfigurePanel from "./ui/panels/configuring/assembly-config/ConfigurePanel.tsx" -const worker = new Lazy(() => new WPILibWSWorker()) - function Synthesis() { const { openModal, closeModal, getActiveModalElement } = useModalManager(initialModals) const { openPanel, closePanel, closeAllPanels, getActivePanelElements } = usePanelManager(initialPanels) @@ -93,8 +89,6 @@ function Synthesis() { setConsentPopupDisable(false) } - worker.getValue() - let mainLoopHandle = 0 const mainLoop = () => { mainLoopHandle = requestAnimationFrame(mainLoop) diff --git a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts index a4ce35d7ff..4a64f221f2 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts @@ -45,7 +45,6 @@ export class PWMOutputGroup extends SimOutputGroup { const average = this.ports.reduce((sum, port) => { const speed = SimPWM.GetSpeed(`${port}`) ?? 0 - console.debug(port, speed) return sum + speed }, 0) / this.ports.length diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index a6495ac20a..7eaad915e8 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -1,6 +1,7 @@ import Mechanism from "@/systems/physics/Mechanism" import Brain from "../Brain" +import Lazy from "@/util/Lazy.ts" import WPILibWSWorker from "./WPILibWSWorker?worker" import { SimulationLayer } from "../SimulationSystem" import World from "@/systems/World" @@ -8,7 +9,8 @@ import World from "@/systems/World" import { SimAnalogOutput, SimOutput, SimOutputGroup } from "./SimOutput" import { SimAccelInput, SimInput, SimDIO as SimDIOIn, SimAnalogInput } from "./SimInput" -const worker = new WPILibWSWorker() +const worker: Lazy = new Lazy(() => new WPILibWSWorker()) + const PWM_SPEED = " } -worker.addEventListener("message", (eventData: MessageEvent) => { +worker.getValue().addEventListener("message", (eventData: MessageEvent) => { let data: WSMessage | undefined if (typeof eventData.data == "object") { @@ -338,7 +340,7 @@ worker.addEventListener("message", (eventData: MessageEvent) => { } } - if (!data?.type || !(Object.values(SimType) as string[]).includes(data.type) || data.device.split(" ")[0] != "SYN") + if (!data?.type || !(Object.values(SimType) as string[]).includes(data.type))// || data.device.split(" ")[0] != "SYN") return UpdateSimMap(data.type as SimType, data.device, data.data) @@ -380,10 +382,10 @@ class WPILibBrain extends Brain { // this.addSimInput(new SimGyroInput("Test Gyro[1]", mechanism)) // this.addSimInput(new SimAccelInput("ADXL362[4]", mechanism)) - // this.addSimInput(new SimDIOIn("In[0]", () => Math.random() > 0.5)) - // this.addSimInput(new SimDIOIn("Out[1]")) - // this.addSimInput(new SimAnalogInput("In[0]", () => Math.random() * 12)) - // this.addSimOutput(new SimAnalogOutput("Out[1]")) + // this.addSimInput(new SimDIOIn("SYN DI[0]", () => Math.random() > 0.5)) + // this.addSimInput(new SimDIOIn("SYN DO[1]")) + // this.addSimInput(new SimAnalogInput("SYN AI[0]", () => Math.random() * 12)) + // this.addSimOutput(new SimAnalogOutput("SYN AO[1]")) } public addSimOutput(device: SimOutput) { @@ -397,15 +399,14 @@ class WPILibBrain extends Brain { public Update(deltaT: number): void { this._simOutputs.forEach(d => d.Update(deltaT)) this._simInputs.forEach(i => i.Update(deltaT)) - console.log(simMap) } public Enable(): void { - worker.postMessage({ command: "connect" }) + worker.getValue().postMessage({ command: "connect" }) } public Disable(): void { - worker.postMessage({ command: "disconnect" }) + worker.getValue().postMessage({ command: "disconnect" }) } } diff --git a/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx b/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx index 1584ba3d50..075eda2d2f 100644 --- a/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx +++ b/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx @@ -25,13 +25,11 @@ const RCConfigCANGroupModal: React.FC = ({ modalId }) => { let brain: WPILibBrain const miraObjs = [...World.SceneRenderer.sceneObjects.entries()].filter(x => x[1] instanceof MirabufSceneObject) - console.log(`Number of mirabuf scene objects: ${miraObjs.length}`) if (miraObjs.length > 0) { const mechanism = (miraObjs[0][1] as MirabufSceneObject).mechanism simLayer = World.SimulationSystem.GetSimulationLayer(mechanism) drivers = simLayer?.drivers ?? [] - brain = new WPILibBrain(mechanism) - simLayer?.SetBrain(brain) + brain = simLayer?.brain as WPILibBrain } const cans = simMap.get(SimType.CANMotor) ?? new Map>() @@ -49,14 +47,6 @@ const RCConfigCANGroupModal: React.FC = ({ modalId }) => { // no eslint complain brain.addSimOutput(new CANOutputGroup(name, checkedPorts, checkedDrivers)) console.log(name, checkedPorts, checkedDrivers) - const replacer = (_: unknown, value: unknown) => { - if (value instanceof Map) { - return Object.fromEntries(value) - } else { - return value - } - } - console.log(JSON.stringify(simMap, replacer)) }} onCancel={() => { openModal("roborio") @@ -92,7 +82,7 @@ const RCConfigCANGroupModal: React.FC = ({ modalId }) => { {drivers.map((driver, idx) => ( { if (checked && !checkedDrivers.includes(driver)) { diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/AnalogInput.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/AnalogInput.java new file mode 100644 index 0000000000..d093a510c4 --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/AnalogInput.java @@ -0,0 +1,90 @@ +package com.autodesk.synthesis.io; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimInt; +import edu.wpi.first.hal.SimDevice.Direction; + +public class AnalogInput extends edu.wpi.first.wpilibj.AnalogInput { + private SimDevice m_device; + + private SimBoolean m_init; + private SimInt m_avgBits; + private SimInt m_oversampleBits; + private SimDouble m_voltage; + private SimBoolean m_accumInit; + private SimInt m_accumValue; + private SimInt m_accumCount; + private SimInt m_accumCenter; + private SimInt m_accumDeadband; + + public AnalogInput(int channel) { + super(channel); + + m_device = SimDevice.create("AI:SYN AI", channel); + + m_init = m_device.createBoolean("init", Direction.kOutput, true); + m_avgBits = m_device.createInt("avg_bits", Direction.kOutput, 0); + m_oversampleBits = m_device.createInt("oversample_bits", Direction.kOutput, 0); + m_voltage = m_device.createDouble("voltage", Direction.kInput, 0.0); + m_accumInit = m_device.createBoolean("accum_init", Direction.kOutput, false); + m_accumValue = m_device.createInt("accum_value", Direction.kInput, 0); + m_accumCount = m_device.createInt("accum_count", Direction.kInput, 0); + m_accumCenter = m_device.createInt("accum_center", Direction.kOutput, 0); + m_accumDeadband = m_device.createInt("accum_deadband", Direction.kOutput, 0); + + this.setSimDevice(m_device); + } + + @Override + public double getVoltage() { + return m_voltage.get(); + } + + @Override + public int getAverageBits() { + return m_avgBits.get(); + } + + @Override + public void setAverageBits(int bits) { + m_avgBits.set(bits); + } + + @Override + public int getOversampleBits() { + return m_oversampleBits.get(); + } + + @Override + public void setOversampleBits(int bits) { + m_oversampleBits.set(bits); + } + + @Override + public void initAccumulator() { + super.initAccumulator(); + m_accumInit.set(true); + } + + @Override + public long getAccumulatorValue() { + return m_accumValue.get(); + } + + @Override + public long getAccumulatorCount() { + return m_accumCount.get(); + } + + @Override + public void setAccumulatorCenter(int center) { + m_accumCenter.set(center); + } + + @Override + public void setAccumulatorDeadband(int deadband) { + m_accumDeadband.set(deadband); + } +} diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/AnalogOutput.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/AnalogOutput.java new file mode 100644 index 0000000000..46b66b05b9 --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/AnalogOutput.java @@ -0,0 +1,32 @@ +package com.autodesk.synthesis.io; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimDevice.Direction; + +public class AnalogOutput extends edu.wpi.first.wpilibj.AnalogOutput { + private SimDevice m_device; + + private SimBoolean m_init; + private SimDouble m_voltage; + + public AnalogOutput(int channel) { + super(channel); + + m_device = SimDevice.create("AI:SYN AO", channel); + + m_init = m_device.createBoolean("init", Direction.kOutput, true); + m_voltage = m_device.createDouble("voltage", Direction.kOutput, 0.0); + } + + @Override + public void setVoltage(double voltage) { + m_voltage.set(voltage); + } + + @Override + public double getVoltage() { + return m_voltage.get(); + } +} diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/DigitalInput.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/DigitalInput.java new file mode 100644 index 0000000000..e4df20931f --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/DigitalInput.java @@ -0,0 +1,33 @@ +package com.autodesk.synthesis.io; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimDevice.Direction; + +public class DigitalInput extends edu.wpi.first.wpilibj.DigitalInput { + private SimDevice m_device; + + private SimBoolean m_init; + private SimBoolean m_input; + private SimBoolean m_value; + private SimDouble m_pulseLength; // unused but in HALSim spec + + public DigitalInput(int channel) { + super(channel); + + m_device = SimDevice.create("DIO:SYN DI", channel); + + m_init = m_device.createBoolean("init", Direction.kOutput, true); + m_input = m_device.createBoolean("input", Direction.kOutput, true); + m_value = m_device.createBoolean("value", Direction.kBidir, false); + m_pulseLength = m_device.createDouble("pulse_length", Direction.kOutput, 0.0); + + this.setSimDevice(m_device); + } + + @Override + public boolean get() { + return m_value.get(); + } +} diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/DigitalOutput.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/DigitalOutput.java new file mode 100644 index 0000000000..ea76679485 --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/io/DigitalOutput.java @@ -0,0 +1,38 @@ +package com.autodesk.synthesis.io; + +import edu.wpi.first.hal.SimBoolean; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.hal.SimDevice.Direction; + +public class DigitalOutput extends edu.wpi.first.wpilibj.DigitalOutput { + private SimDevice m_device; + + private SimBoolean m_init; + private SimBoolean m_input; + private SimBoolean m_value; + private SimDouble m_pulseLength; // unused but in HALSim spec + + public DigitalOutput(int channel) { + super(channel); + + m_device = SimDevice.create("DIO:SYN DO", channel); + + m_init = m_device.createBoolean("init", Direction.kOutput, true); + m_input = m_device.createBoolean("input", Direction.kOutput, true); + m_value = m_device.createBoolean("value", Direction.kBidir, false); + m_pulseLength = m_device.createDouble("pulse_length", Direction.kOutput, 0.0); + + this.setSimDevice(m_device); + } + + @Override + public boolean get() { + return m_value.get(); + } + + @Override + public void set(boolean value) { + m_value.set(value); + } +} diff --git a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java index 891e77651f..3f85f59bc4 100644 --- a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java +++ b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java @@ -4,20 +4,9 @@ package frc.robot; -import com.ctre.phoenix6.hardware.Pigeon2; -import com.kauailabs.navx.frc.AHRS; -import com.revrobotics.CANSparkBase.IdleMode; -// import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; -import edu.wpi.first.wpilibj.ADXL362; -import edu.wpi.first.wpilibj.ADXRS450_Gyro; -import edu.wpi.first.wpilibj.AnalogGyro; -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.AnalogOutput; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DigitalOutput; -import edu.wpi.first.wpilibj.SPI; +import com.autodesk.synthesis.io.*; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.motorcontrol.Spark; @@ -26,7 +15,6 @@ import edu.wpi.first.wpilibj.XboxController; import com.autodesk.synthesis.revrobotics.CANSparkMax; -import com.autodesk.synthesis.Gyro; import com.autodesk.synthesis.ctre.TalonFX; /** @@ -48,12 +36,7 @@ public class Robot extends TimedRobot { private Spark m_Spark2 = new Spark(1); private TalonFX m_Talon = new TalonFX(2); private XboxController m_Controller = new XboxController(0); - private Gyro m_Gyro = new Gyro("Test Gyro", 1); - private AHRS m_NavX = new AHRS(); - // Creates an ADXL362 accelerometer object on the MXP SPI port - // with a measurement range from -8 to 8 G's - private ADXL362 m_Accelerometer = new ADXL362(SPI.Port.kMXP, ADXL362.Range.k8G); - private Pigeon2 m_Pigeon2 = new Pigeon2(0); + private DigitalInput m_DI = new DigitalInput(0); private DigitalOutput m_DO = new DigitalOutput(1); private AnalogInput m_AI = new AnalogInput(0); @@ -155,9 +138,9 @@ public void teleopInit() { public void teleopPeriodic() { m_Spark1.set(m_Controller.getLeftY()); m_Spark2.set(-m_Controller.getRightY()); - System.out.println(m_DI.get()); - // m_Spark1.set(0.25); - // m_Spark2.set(0.25); + System.out.println("IN: " + m_DI.get()); + System.out.println("OUT: " + m_DO.get()); + System.out.println("AI: " + m_AI.getVoltage()); m_Talon.set(-0.5); m_SparkMax1.set(-0.75); m_SparkMax2.set(-0.75); From e6d3522349e6f6343fdd75078fdfdcfdb518168f Mon Sep 17 00:00:00 2001 From: PepperLola Date: Mon, 26 Aug 2024 16:40:38 -0700 Subject: [PATCH 06/16] ran formatter and fixed lint errors --- .../simulation/wpilib_brain/SimInput.ts | 2 +- .../simulation/wpilib_brain/SimOutput.ts | 2 +- .../simulation/wpilib_brain/WPILibBrain.ts | 35 +++++++++---------- .../rio-config/RCConfigCANGroupModal.tsx | 2 +- fission/src/ui/panels/WSViewPanel.tsx | 14 ++++++-- .../ConfigureSubsystemsInterface.tsx | 2 +- 6 files changed, 33 insertions(+), 24 deletions(-) diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index 974503b5b0..88e930e04d 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -137,7 +137,7 @@ export class SimDIO implements SimInput { } public SetValue(value: boolean) { - WSSimDIO.SetValue(this._device, value); + WSSimDIO.SetValue(this._device, value) } public GetValue(): boolean { diff --git a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts index 4a64f221f2..85521c7bed 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts @@ -91,5 +91,5 @@ export class SimAnalogOutput extends SimOutput { return SimAO.GetVoltage(this.name) } - public Update(_deltaT: number) { } + public Update(_deltaT: number) {} } diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index 7eaad915e8..ccd672e54a 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -6,8 +6,8 @@ import WPILibWSWorker from "./WPILibWSWorker?worker" import { SimulationLayer } from "../SimulationSystem" import World from "@/systems/World" -import { SimAnalogOutput, SimOutput, SimOutputGroup } from "./SimOutput" -import { SimAccelInput, SimInput, SimDIO as SimDIOIn, SimAnalogInput } from "./SimInput" +import { SimOutput } from "./SimOutput" +import { SimInput } from "./SimInput" const worker: Lazy = new Lazy(() => new WPILibWSWorker()) @@ -35,7 +35,7 @@ export enum SimType { Accel = "Accel", DIO = "DIO", AI = "AI", - AO = "AO" + AO = "AO", } enum FieldType { @@ -66,10 +66,10 @@ type DeviceData = Map export const simMap = new Map>() export class SimGeneric { - private constructor() { } + private constructor() {} - public static Get(simType: SimType, device: string, field: string): T | undefined; - public static Get(simType: SimType, device: string, field: string, defaultValue: T): T; + public static Get(simType: SimType, device: string, field: string): T | undefined + public static Get(simType: SimType, device: string, field: string, defaultValue: T): T public static Get(simType: SimType, device: string, field: string, defaultValue?: T): T | undefined { const fieldType = GetFieldType(field) if (fieldType != FieldType.Read && fieldType != FieldType.Both) { @@ -130,7 +130,7 @@ export class SimGeneric { } export class SimPWM { - private constructor() { } + private constructor() {} public static GetSpeed(device: string): number | undefined { return SimGeneric.Get(SimType.PWM, device, PWM_SPEED, 0.0) @@ -142,7 +142,7 @@ export class SimPWM { } export class SimCAN { - private constructor() { } + private constructor() {} public static GetDeviceWithID(id: number, type: SimType): DeviceData | undefined { const id_exp = /.*\[(\d+)\]/g @@ -161,7 +161,7 @@ export class SimCAN { } export class SimCANMotor { - private constructor() { } + private constructor() {} public static GetPercentOutput(device: string): number | undefined { return SimGeneric.Get(SimType.CANMotor, device, CANMOTOR_PERCENT_OUTPUT, 0.0) @@ -188,7 +188,7 @@ export class SimCANMotor { } } export class SimCANEncoder { - private constructor() { } + private constructor() {} public static SetVelocity(device: string, velocity: number): boolean { return SimGeneric.Set(SimType.CANEncoder, device, CANENCODER_VELOCITY, velocity) @@ -200,7 +200,7 @@ export class SimCANEncoder { } export class SimGyro { - private constructor() { } + private constructor() {} public static SetAngleX(device: string, angle: number): boolean { return SimGeneric.Set(SimType.Gyro, device, ">angle_x", angle) @@ -228,7 +228,7 @@ export class SimGyro { } export class SimAccel { - private constructor() { } + private constructor() {} public static SetX(device: string, accel: number): boolean { return SimGeneric.Set(SimType.Accel, device, ">x", accel) @@ -244,10 +244,10 @@ export class SimAccel { } export class SimDIO { - private constructor() { } + private constructor() {} public static SetValue(device: string, value: boolean): boolean { - return SimGeneric.Set(SimType.DIO, device, "<>value", +value); + return SimGeneric.Set(SimType.DIO, device, "<>value", +value) } public static GetValue(device: string): boolean { @@ -256,7 +256,7 @@ export class SimDIO { } export class SimAI { - constructor() { } + constructor() {} public static SetValue(device: string, value: number): boolean { return SimGeneric.Set(SimType.AI, device, ">voltage", value) @@ -313,7 +313,7 @@ export class SimAI { } export class SimAO { - constructor() { } + constructor() {} public static GetVoltage(device: string): number { return SimGeneric.Get(SimType.AI, device, ">voltage", 0.0) @@ -340,8 +340,7 @@ worker.getValue().addEventListener("message", (eventData: MessageEvent) => { } } - if (!data?.type || !(Object.values(SimType) as string[]).includes(data.type))// || data.device.split(" ")[0] != "SYN") - return + if (!data?.type || !(Object.values(SimType) as string[]).includes(data.type)) return UpdateSimMap(data.type as SimType, data.device, data.data) }) diff --git a/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx b/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx index 075eda2d2f..d3afc4e799 100644 --- a/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx +++ b/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx @@ -82,7 +82,7 @@ const RCConfigCANGroupModal: React.FC = ({ modalId }) => { {drivers.map((driver, idx) => ( { if (checked && !checkedDrivers.includes(driver)) { diff --git a/fission/src/ui/panels/WSViewPanel.tsx b/fission/src/ui/panels/WSViewPanel.tsx index 9dd2b6fbdd..91726b42a0 100644 --- a/fission/src/ui/panels/WSViewPanel.tsx +++ b/fission/src/ui/panels/WSViewPanel.tsx @@ -35,13 +35,23 @@ function formatMap(map: Map): string { } function generateTableBody() { - const names: SimType[] = [SimType.PWM, SimType.SimDevice, SimType.CANMotor, SimType.CANEncoder, SimType.Gyro, SimType.Accel, SimType.DIO, SimType.AI, SimType.AO] + const names: SimType[] = [ + SimType.PWM, + SimType.SimDevice, + SimType.CANMotor, + SimType.CANEncoder, + SimType.Gyro, + SimType.Accel, + SimType.DIO, + SimType.AI, + SimType.AO, + ] console.log(simMap) return ( - {names.map(name => + {names.map(name => simMap.has(name) ? ( [...simMap.get(name)!.entries()] // most devices don't have = ({ selected (selectedRobot.brain as SynthesisBrain).behaviors .filter(b => b instanceof SequenceableBehavior) .map(b => DefaultSequentialConfig(b.jointIndex, b instanceof GenericArmBehavior ? "Arm" : "Elevator")), - [] + [selectedRobot.assemblyName, selectedRobot.brain] ) const drivers = useMemo(() => { From 18cd843fd0a9cdcd5f2aeda74cca8e5b0d87f9ba Mon Sep 17 00:00:00 2001 From: PepperLola Date: Mon, 26 Aug 2024 17:51:51 -0700 Subject: [PATCH 07/16] ran formatter, added string as possible ws type --- .../simulation/wpilib_brain/SimInput.ts | 51 ++++++++++--------- .../simulation/wpilib_brain/SimOutput.ts | 44 ++++++++++------ .../simulation/wpilib_brain/WPILibBrain.ts | 12 ++--- .../rio-config/RCConfigCANGroupModal.tsx | 2 +- fission/src/ui/panels/WSViewPanel.tsx | 2 +- 5 files changed, 63 insertions(+), 48 deletions(-) diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index 88e930e04d..245bff176a 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -1,20 +1,25 @@ import World from "@/systems/World" import EncoderStimulus from "../stimulus/EncoderStimulus" -import { SimCANEncoder, SimGyro, SimAccel, SimDIO as WSSimDIO, SimAI } from "./WPILibBrain" +import { SimCANEncoder, SimGyro, SimAccel, SimDIO, SimAI } 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 abstract class SimInput { + constructor(protected _device: string) {} + + public abstract Update(deltaT: number): void + + public get device(): string { + return this._device + } } -export class SimEncoderInput implements SimInput { - private _device: string +export class SimEncoderInput extends SimInput { private _stimulus: EncoderStimulus constructor(device: string, stimulus: EncoderStimulus) { - this._device = device + super(device) this._stimulus = stimulus } @@ -24,8 +29,7 @@ export class SimEncoderInput implements SimInput { } } -export class SimGyroInput implements SimInput { - private _device: string +export class SimGyroInput extends SimInput { private _robot: Mechanism private _joltID?: Jolt.BodyID private _joltBody?: Jolt.Body @@ -35,7 +39,7 @@ export class SimGyroInput implements SimInput { private static AXIS_Z: Jolt.Vec3 = new JOLT.Vec3(0, 0, 1) constructor(device: string, robot: Mechanism) { - this._device = device + super(device) this._robot = robot this._joltID = this._robot.nodeToBody.get(this._robot.rootBody) @@ -86,14 +90,13 @@ export class SimGyroInput implements SimInput { } } -export class SimAccelInput implements SimInput { - private _device: string +export class SimAccelInput extends SimInput { private _robot: Mechanism private _joltID?: Jolt.BodyID private _joltBody?: Jolt.Body constructor(device: string, robot: Mechanism) { - this._device = device + super(device) this._robot = robot this._joltID = this._robot.nodeToBody.get(this._robot.rootBody) @@ -121,27 +124,26 @@ export class SimAccelInput implements SimInput { } } -export class SimDIO implements SimInput { - private _device: string - private _valueSupplier?: () => boolean +export class SimDigitalInput extends SimInput { + private _valueSupplier: () => boolean /** - * Creates a Simulation Digital Input/Output object. + * Creates a Simulation Digital Input object. * * @param device Device ID - * @param valueSupplier Called each frame and returns what the value should be set to. Don't specify if DIO should be treated as an output. + * @param valueSupplier Called each frame and returns what the value should be set to */ - constructor(device: string, valueSupplier?: () => boolean) { - this._device = device + constructor(device: string, valueSupplier: () => boolean) { + super(device) this._valueSupplier = valueSupplier } - public SetValue(value: boolean) { - WSSimDIO.SetValue(this._device, value) + private SetValue(value: boolean) { + SimDIO.SetValue(this._device, value) } public GetValue(): boolean { - return WSSimDIO.GetValue(this._device) + return SimDIO.GetValue(this._device) } public Update(_deltaT: number) { @@ -149,12 +151,11 @@ export class SimDIO implements SimInput { } } -export class SimAnalogInput implements SimInput { - private _device: string +export class SimAnalogInput extends SimInput { private _valueSupplier: () => number constructor(device: string, valueSupplier: () => number) { - this._device = device + super(device) this._valueSupplier = valueSupplier } diff --git a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts index 85521c7bed..c9c5c0f091 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts @@ -2,23 +2,16 @@ import Driver from "../driver/Driver" import HingeDriver from "../driver/HingeDriver" import SliderDriver from "../driver/SliderDriver" import WheelDriver from "../driver/WheelDriver" -import { SimAO, 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 +import { SimAO, SimCAN, SimDIO, SimPWM, SimType } from "./WPILibBrain" export abstract class SimOutput { - public name: string - - constructor(name: string) { - this.name = name - } + constructor(protected _name: string) {} public abstract Update(deltaT: number): void + + public get name(): string { + return this._name + } } export abstract class SimOutputGroup extends SimOutput { @@ -68,7 +61,7 @@ export class CANOutputGroup extends SimOutputGroup { const average = this.ports.reduce((sum, port) => { const device = SimCAN.GetDeviceWithID(port, SimType.CANMotor) - return sum + (device?.get(" { @@ -82,13 +75,34 @@ export class CANOutputGroup extends SimOutputGroup { } } +export class SimDigitalOutput extends SimOutput { + /** + * Creates a Simulation Digital Input/Output object. + * + * @param device Device ID + */ + constructor(name: string) { + super(name) + } + + public SetValue(value: boolean) { + SimDIO.SetValue(this._name, value) + } + + public GetValue(): boolean { + return SimDIO.GetValue(this._name) + } + + public Update(_deltaT: number) {} +} + export class SimAnalogOutput extends SimOutput { public constructor(name: string) { super(name) } public GetVoltage(): number { - return SimAO.GetVoltage(this.name) + return SimAO.GetVoltage(this._name) } public Update(_deltaT: number) {} diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index ccd672e54a..2f51979084 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -61,7 +61,7 @@ function GetFieldType(field: string): FieldType { } type DeviceName = string -type DeviceData = Map +type DeviceData = Map export const simMap = new Map>() @@ -92,7 +92,7 @@ export class SimGeneric { return (data.get(field) as T | undefined) ?? defaultValue } - public static Set(simType: SimType, device: string, field: string, value: T): boolean { + public static Set(simType: SimType, device: string, field: string, value: T): boolean { const fieldType = GetFieldType(field) if (fieldType != FieldType.Write && fieldType != FieldType.Both) { console.warn(`Field '${field}' is not a write or both field type`) @@ -111,7 +111,7 @@ export class SimGeneric { return false } - const selectedData: { [key: string]: number } = {} + const selectedData: { [key: string]: number | boolean } = {} selectedData[field] = value data.set(field, value) @@ -247,7 +247,7 @@ export class SimDIO { private constructor() {} public static SetValue(device: string, value: boolean): boolean { - return SimGeneric.Set(SimType.DIO, device, "<>value", +value) + return SimGeneric.Set(SimType.DIO, device, "<>value", value) } public static GetValue(device: string): boolean { @@ -381,8 +381,8 @@ class WPILibBrain extends Brain { // this.addSimInput(new SimGyroInput("Test Gyro[1]", mechanism)) // this.addSimInput(new SimAccelInput("ADXL362[4]", mechanism)) - // this.addSimInput(new SimDIOIn("SYN DI[0]", () => Math.random() > 0.5)) - // this.addSimInput(new SimDIOIn("SYN DO[1]")) + // this.addSimInput(new SimDigitalInput("SYN DI[0]", () => Math.random() > 0.5)) + // this.addSimOutput(new SimDigitalOutput("SYN DO[1]")) // this.addSimInput(new SimAnalogInput("SYN AI[0]", () => Math.random() * 12)) // this.addSimOutput(new SimAnalogOutput("SYN AO[1]")) } diff --git a/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx b/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx index d3afc4e799..b2a1641d08 100644 --- a/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx +++ b/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx @@ -33,7 +33,7 @@ const RCConfigCANGroupModal: React.FC = ({ modalId }) => { } const cans = simMap.get(SimType.CANMotor) ?? new Map>() - const devices: [string, Map][] = [...cans.entries()] + const devices: [string, Map][] = [...cans.entries()] .filter(([_, data]) => data.get("): string { +function formatMap(map: Map): string { let entries: string = "" map.forEach((value, key) => { entries += `${key} : ${value}` From 4b0519e9c5b49d4fd529398dabdda4c55891be88 Mon Sep 17 00:00:00 2001 From: PepperLola Date: Wed, 28 Aug 2024 18:25:50 -0700 Subject: [PATCH 08/16] manually calculate acceleration --- .../simulation/wpilib_brain/SimInput.ts | 29 +++++++++---------- .../src/main/java/frc/robot/Robot.java | 5 ++++ 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index 245bff176a..7013c10516 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -94,33 +94,30 @@ export class SimAccelInput extends SimInput { private _robot: Mechanism private _joltID?: Jolt.BodyID private _joltBody?: Jolt.Body + private _prevVel: Jolt.Vec3 constructor(device: string, robot: Mechanism) { super(device) this._robot = robot this._joltID = this._robot.nodeToBody.get(this._robot.rootBody) + this._prevVel = new JOLT.Vec3(0, 0, 0) if (this._joltID) this._joltBody = World.PhysicsSystem.GetBody(this._joltID) } - private GetAxis(axis: "x" | "y" | "z"): number { - const forces = this._joltBody?.GetAccumulatedForce() - if (!forces) return 0 + public Update(deltaT: number) { + const newVel = this._joltBody?.GetLinearVelocity() + if (!newVel) return; - switch (axis) { - case "x": - return forces.GetX() - case "y": - return forces.GetY() - case "z": - return forces.GetZ() - } - } + const x = (newVel.GetX() - this._prevVel.GetX()) / deltaT + const y = (newVel.GetY() - this._prevVel.GetY()) / deltaT + const z = (newVel.GetZ() - this._prevVel.GetZ()) / deltaT - public Update(_deltaT: number) { - SimAccel.SetX(this._device, this.GetAxis("x")) - SimAccel.SetY(this._device, this.GetAxis("y")) - SimAccel.SetZ(this._device, this.GetAxis("z")) + SimAccel.SetX(this._device, x) + SimAccel.SetY(this._device, y) + SimAccel.SetZ(this._device, z) + + this._prevVel = new JOLT.Vec3(newVel.GetX(), newVel.GetY(), newVel.GetZ()) } } diff --git a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java index 3f85f59bc4..28e4035ea9 100644 --- a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java +++ b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java @@ -8,6 +8,9 @@ import com.autodesk.synthesis.io.*; +import edu.wpi.first.wpilibj.SPI; + +import edu.wpi.first.wpilibj.ADXL362; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -37,6 +40,8 @@ public class Robot extends TimedRobot { private TalonFX m_Talon = new TalonFX(2); private XboxController m_Controller = new XboxController(0); + private ADXL362 m_Accelerometer = new ADXL362(SPI.Port.kMXP, ADXL362.Range.k8G); + private DigitalInput m_DI = new DigitalInput(0); private DigitalOutput m_DO = new DigitalOutput(1); private AnalogInput m_AI = new AnalogInput(0); From 7e8b646f4c739dd46d249d4676e00b997f68b4b5 Mon Sep 17 00:00:00 2001 From: PepperLola Date: Wed, 28 Aug 2024 18:26:40 -0700 Subject: [PATCH 09/16] ran formatter --- fission/src/systems/simulation/wpilib_brain/SimInput.ts | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index 7013c10516..ae4b427efd 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -107,7 +107,7 @@ export class SimAccelInput extends SimInput { public Update(deltaT: number) { const newVel = this._joltBody?.GetLinearVelocity() - if (!newVel) return; + if (!newVel) return const x = (newVel.GetX() - this._prevVel.GetX()) / deltaT const y = (newVel.GetY() - this._prevVel.GetY()) / deltaT From e00848148feece8f1bd1f8e6c257b8754307b686 Mon Sep 17 00:00:00 2001 From: Hunter Barclay Date: Wed, 4 Sep 2024 21:27:53 -0600 Subject: [PATCH 10/16] [BUG] Vite Server Crashing Chrome (#1108) * Isolated at least one issue to pickup interface * fix(vite-server)!: Vite server working with Chrome. Removed circular dependencies and isolated key structures into separate files. Cleanup still needed. * fix(vite-server)!: Readd panels and modals, fix imports, cleanup. * Formatted * Formatted --- .../theme-editor/AssignNewSchemeModal.tsx | 5 ++- .../theme-editor/NewInputSchemeModal.tsx | 5 ++- .../configuring/ChooseInputSchemePanel.tsx | 2 +- .../ConfigurationSavedEvent.ts | 16 +++++++ .../assembly-config/ConfigurationType.ts | 15 +++++++ .../assembly-config/ConfigurePanel.tsx | 42 +++---------------- .../ConfigureGamepiecePickupInterface.tsx | 2 +- .../ConfigureShotTrajectoryInterface.tsx | 2 +- .../ConfigureSubsystemsInterface.tsx | 2 +- .../SequentialBehaviorsInterface.tsx | 2 +- .../inputs/ConfigureInputsInterface.tsx | 2 +- .../inputs/ConfigureSchemeInterface.tsx | 2 +- .../ConfigureScoringZonesInterface.tsx | 2 +- .../scoring/ManageZonesInterface.tsx | 2 +- .../scoring/ZoneConfigInterface.tsx | 2 +- 15 files changed, 55 insertions(+), 48 deletions(-) create mode 100644 fission/src/ui/panels/configuring/assembly-config/ConfigurationSavedEvent.ts create mode 100644 fission/src/ui/panels/configuring/assembly-config/ConfigurationType.ts diff --git a/fission/src/ui/modals/configuring/theme-editor/AssignNewSchemeModal.tsx b/fission/src/ui/modals/configuring/theme-editor/AssignNewSchemeModal.tsx index 1f5c98bd2f..668590791b 100644 --- a/fission/src/ui/modals/configuring/theme-editor/AssignNewSchemeModal.tsx +++ b/fission/src/ui/modals/configuring/theme-editor/AssignNewSchemeModal.tsx @@ -6,8 +6,11 @@ import InputSystem from "@/systems/input/InputSystem" import SynthesisBrain from "@/systems/simulation/synthesis_brain/SynthesisBrain" import { SynthesisIcons } from "@/ui/components/StyledComponents" import { usePanelControlContext } from "@/ui/PanelContext" -import { ConfigurationType, setSelectedConfigurationType } from "@/ui/panels/configuring/assembly-config/ConfigurePanel" import { setSelectedScheme } from "@/ui/panels/configuring/assembly-config/interfaces/inputs/ConfigureInputsInterface" +import { + setSelectedConfigurationType, + ConfigurationType, +} from "@/ui/panels/configuring/assembly-config/ConfigurationType" const AssignNewSchemeModal: React.FC = ({ modalId }) => { const { openPanel } = usePanelControlContext() diff --git a/fission/src/ui/modals/configuring/theme-editor/NewInputSchemeModal.tsx b/fission/src/ui/modals/configuring/theme-editor/NewInputSchemeModal.tsx index 06b95d7a1f..acbce6b5e2 100644 --- a/fission/src/ui/modals/configuring/theme-editor/NewInputSchemeModal.tsx +++ b/fission/src/ui/modals/configuring/theme-editor/NewInputSchemeModal.tsx @@ -5,8 +5,11 @@ import InputSchemeManager from "@/systems/input/InputSchemeManager" import DefaultInputs from "@/systems/input/DefaultInputs" import { SynthesisIcons } from "@/ui/components/StyledComponents" import { usePanelControlContext } from "@/ui/PanelContext" -import { ConfigurationType, setSelectedConfigurationType } from "@/ui/panels/configuring/assembly-config/ConfigurePanel" import { setSelectedScheme } from "@/ui/panels/configuring/assembly-config/interfaces/inputs/ConfigureInputsInterface" +import { + ConfigurationType, + setSelectedConfigurationType, +} from "@/ui/panels/configuring/assembly-config/ConfigurationType" const NewInputSchemeModal: React.FC = ({ modalId }) => { const { openPanel } = usePanelControlContext() diff --git a/fission/src/ui/panels/configuring/ChooseInputSchemePanel.tsx b/fission/src/ui/panels/configuring/ChooseInputSchemePanel.tsx index 0d5b549f36..315be12ebb 100644 --- a/fission/src/ui/panels/configuring/ChooseInputSchemePanel.tsx +++ b/fission/src/ui/panels/configuring/ChooseInputSchemePanel.tsx @@ -18,8 +18,8 @@ import { useModalControlContext } from "@/ui/ModalContext" import { usePanelControlContext } from "@/ui/PanelContext" import { Box } from "@mui/material" import { useEffect, useReducer } from "react" -import { ConfigurationType, setSelectedConfigurationType } from "./assembly-config/ConfigurePanel" import { setSelectedScheme } from "./assembly-config/interfaces/inputs/ConfigureInputsInterface" +import { ConfigurationType, setSelectedConfigurationType } from "./assembly-config/ConfigurationType" /** We store the selected brain index globally to specify which robot the input scheme should be bound to. */ let selectedBrainIndexGlobal: number | undefined = undefined diff --git a/fission/src/ui/panels/configuring/assembly-config/ConfigurationSavedEvent.ts b/fission/src/ui/panels/configuring/assembly-config/ConfigurationSavedEvent.ts new file mode 100644 index 0000000000..9f240286f3 --- /dev/null +++ b/fission/src/ui/panels/configuring/assembly-config/ConfigurationSavedEvent.ts @@ -0,0 +1,16 @@ +/** An event to save whatever configuration interface is open when it is closed */ +export class ConfigurationSavedEvent extends Event { + public constructor() { + super("ConfigurationSaved") + + window.dispatchEvent(this) + } + + public static Listen(func: (e: Event) => void) { + window.addEventListener("ConfigurationSaved", func) + } + + public static RemoveListener(func: (e: Event) => void) { + window.removeEventListener("ConfigurationSaved", func) + } +} diff --git a/fission/src/ui/panels/configuring/assembly-config/ConfigurationType.ts b/fission/src/ui/panels/configuring/assembly-config/ConfigurationType.ts new file mode 100644 index 0000000000..927beac567 --- /dev/null +++ b/fission/src/ui/panels/configuring/assembly-config/ConfigurationType.ts @@ -0,0 +1,15 @@ +export enum ConfigurationType { + ROBOT, + FIELD, + INPUTS, +} + +let selectedConfigurationType: ConfigurationType = ConfigurationType.ROBOT + +export function setSelectedConfigurationType(type: ConfigurationType) { + selectedConfigurationType = type +} + +export function getConfigurationType() { + return selectedConfigurationType +} diff --git a/fission/src/ui/panels/configuring/assembly-config/ConfigurePanel.tsx b/fission/src/ui/panels/configuring/assembly-config/ConfigurePanel.tsx index 9dbdb7e076..0c261a96fa 100644 --- a/fission/src/ui/panels/configuring/assembly-config/ConfigurePanel.tsx +++ b/fission/src/ui/panels/configuring/assembly-config/ConfigurePanel.tsx @@ -19,6 +19,8 @@ import ConfigureSubsystemsInterface from "./interfaces/ConfigureSubsystemsInterf import SequentialBehaviorsInterface from "./interfaces/SequentialBehaviorsInterface" import ConfigureShotTrajectoryInterface from "./interfaces/ConfigureShotTrajectoryInterface" import ConfigureGamepiecePickupInterface from "./interfaces/ConfigureGamepiecePickupInterface" +import { ConfigurationSavedEvent } from "./ConfigurationSavedEvent" +import { ConfigurationType, getConfigurationType, setSelectedConfigurationType } from "./ConfigurationType" enum ConfigMode { SUBSYSTEMS, @@ -29,23 +31,6 @@ enum ConfigMode { SCORING_ZONES, } -// eslint-disable-next-line react-refresh/only-export-components -export enum ConfigurationType { - ROBOT, - FIELD, - INPUTS, -} - -let selectedConfigurationType: ConfigurationType = ConfigurationType.ROBOT -// eslint-disable-next-line react-refresh/only-export-components -export function setSelectedConfigurationType(type: ConfigurationType) { - selectedConfigurationType = type -} - -function getConfigurationType() { - return selectedConfigurationType -} - /** Option for selecting a robot of field */ class AssemblySelectionOption extends SelectMenuOption { assemblyObject: MirabufSceneObject @@ -125,7 +110,7 @@ class ConfigModeSelectionOption extends SelectMenuOption { } } -const robotModes = [ +const robotModes: ConfigModeSelectionOption[] = [ new ConfigModeSelectionOption("Intake", ConfigMode.INTAKE), new ConfigModeSelectionOption("Ejector", ConfigMode.EJECTOR), new ConfigModeSelectionOption( @@ -140,7 +125,9 @@ const robotModes = [ ), new ConfigModeSelectionOption("Controls", ConfigMode.CONTROLS), ] -const fieldModes = [new ConfigModeSelectionOption("Scoring Zones", ConfigMode.SCORING_ZONES)] +const fieldModes: ConfigModeSelectionOption[] = [ + new ConfigModeSelectionOption("Scoring Zones", ConfigMode.SCORING_ZONES), +] interface ConfigModeSelectionProps { configurationType: ConfigurationType @@ -207,23 +194,6 @@ const ConfigInterface: React.FC = ({ configMode, assembly, } } -/** An event to save whatever configuration interface is open when it is closed */ -export class ConfigurationSavedEvent extends Event { - public constructor() { - super("ConfigurationSaved") - - window.dispatchEvent(this) - } - - public static Listen(func: (e: Event) => void) { - window.addEventListener("ConfigurationSaved", func) - } - - public static RemoveListener(func: (e: Event) => void) { - window.removeEventListener("ConfigurationSaved", func) - } -} - const ConfigurePanel: React.FC = ({ panelId }) => { const { openPanel, closePanel } = usePanelControlContext() const [configurationType, setConfigurationType] = useState(getConfigurationType()) diff --git a/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureGamepiecePickupInterface.tsx b/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureGamepiecePickupInterface.tsx index 0e7dea833e..f15c2361a6 100644 --- a/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureGamepiecePickupInterface.tsx +++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureGamepiecePickupInterface.tsx @@ -15,9 +15,9 @@ import { ThreeMatrix4_Array, } from "@/util/TypeConversions" import { useTheme } from "@/ui/ThemeContext" -import { ConfigurationSavedEvent } from "../ConfigurePanel" import Button from "@/ui/components/Button" import { Spacer } from "@/ui/components/StyledComponents" +import { ConfigurationSavedEvent } from "../ConfigurationSavedEvent" // slider constants const MIN_ZONE_SIZE = 0.1 diff --git a/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureShotTrajectoryInterface.tsx b/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureShotTrajectoryInterface.tsx index f5293e93cf..74bd39db7f 100644 --- a/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureShotTrajectoryInterface.tsx +++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureShotTrajectoryInterface.tsx @@ -15,7 +15,7 @@ import { } from "@/util/TypeConversions" import { useTheme } from "@/ui/ThemeContext" import { RigidNodeId } from "@/mirabuf/MirabufParser" -import { ConfigurationSavedEvent } from "../ConfigurePanel" +import { ConfigurationSavedEvent } from "../ConfigurationSavedEvent" import Button from "@/ui/components/Button" import { Spacer } from "@/ui/components/StyledComponents" diff --git a/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureSubsystemsInterface.tsx b/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureSubsystemsInterface.tsx index d2e98e94d2..c034a446c4 100644 --- a/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureSubsystemsInterface.tsx +++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureSubsystemsInterface.tsx @@ -1,7 +1,7 @@ import MirabufSceneObject from "@/mirabuf/MirabufSceneObject" import SelectMenu, { SelectMenuOption } from "@/ui/components/SelectMenu" import React, { useMemo, useState } from "react" -import { ConfigurationSavedEvent } from "../ConfigurePanel" +import { ConfigurationSavedEvent } from "../ConfigurationSavedEvent" import World from "@/systems/World" import SliderDriver from "@/systems/simulation/driver/SliderDriver" import HingeDriver from "@/systems/simulation/driver/HingeDriver" diff --git a/fission/src/ui/panels/configuring/assembly-config/interfaces/SequentialBehaviorsInterface.tsx b/fission/src/ui/panels/configuring/assembly-config/interfaces/SequentialBehaviorsInterface.tsx index bec98015dd..575dc60db3 100644 --- a/fission/src/ui/panels/configuring/assembly-config/interfaces/SequentialBehaviorsInterface.tsx +++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/SequentialBehaviorsInterface.tsx @@ -8,7 +8,7 @@ import PreferencesSystem from "@/systems/preferences/PreferencesSystem" import SequenceableBehavior from "@/systems/simulation/behavior/synthesis/SequenceableBehavior" import GenericArmBehavior from "@/systems/simulation/behavior/synthesis/GenericArmBehavior" import SynthesisBrain from "@/systems/simulation/synthesis_brain/SynthesisBrain" -import { ConfigurationSavedEvent } from "../ConfigurePanel" +import { ConfigurationSavedEvent } from "../ConfigurationSavedEvent" import { SectionLabel, Spacer, SynthesisIcons } from "@/ui/components/StyledComponents" /** Grey label for a child behavior name */ diff --git a/fission/src/ui/panels/configuring/assembly-config/interfaces/inputs/ConfigureInputsInterface.tsx b/fission/src/ui/panels/configuring/assembly-config/interfaces/inputs/ConfigureInputsInterface.tsx index 175bca83aa..defb467223 100644 --- a/fission/src/ui/panels/configuring/assembly-config/interfaces/inputs/ConfigureInputsInterface.tsx +++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/inputs/ConfigureInputsInterface.tsx @@ -1,5 +1,5 @@ import { useCallback, useEffect, useState } from "react" -import { ConfigurationSavedEvent } from "../../ConfigurePanel" +import { ConfigurationSavedEvent } from "../../ConfigurationSavedEvent" import SelectMenu, { SelectMenuOption } from "@/ui/components/SelectMenu" import InputSystem from "@/systems/input/InputSystem" import InputSchemeManager, { InputScheme } from "@/systems/input/InputSchemeManager" diff --git a/fission/src/ui/panels/configuring/assembly-config/interfaces/inputs/ConfigureSchemeInterface.tsx b/fission/src/ui/panels/configuring/assembly-config/interfaces/inputs/ConfigureSchemeInterface.tsx index 220191cab3..608aa1571e 100644 --- a/fission/src/ui/panels/configuring/assembly-config/interfaces/inputs/ConfigureSchemeInterface.tsx +++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/inputs/ConfigureSchemeInterface.tsx @@ -2,7 +2,7 @@ import InputSchemeManager, { InputScheme } from "@/systems/input/InputSchemeMana import Checkbox from "@/ui/components/Checkbox" import EditInputInterface from "./EditInputInterface" import { useCallback, useEffect, useRef, useState } from "react" -import { ConfigurationSavedEvent } from "../../ConfigurePanel" +import { ConfigurationSavedEvent } from "../../ConfigurationSavedEvent" import { SectionDivider } from "@/ui/components/StyledComponents" interface ConfigSchemeProps { diff --git a/fission/src/ui/panels/configuring/assembly-config/interfaces/scoring/ConfigureScoringZonesInterface.tsx b/fission/src/ui/panels/configuring/assembly-config/interfaces/scoring/ConfigureScoringZonesInterface.tsx index 069c287f7d..12801b04ec 100644 --- a/fission/src/ui/panels/configuring/assembly-config/interfaces/scoring/ConfigureScoringZonesInterface.tsx +++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/scoring/ConfigureScoringZonesInterface.tsx @@ -7,7 +7,7 @@ import PreferencesSystem from "@/systems/preferences/PreferencesSystem" import { Box } from "@mui/material" import { ButtonIcon, SectionDivider, SectionLabel, SynthesisIcons } from "@/ui/components/StyledComponents" import { LabelSize } from "@/ui/components/Label" -import { ConfigurationSavedEvent } from "../../ConfigurePanel" +import { ConfigurationSavedEvent } from "../../ConfigurationSavedEvent" const saveZones = (zones: ScoringZonePreferences[] | undefined, field: MirabufSceneObject | undefined) => { if (!zones || !field) return diff --git a/fission/src/ui/panels/configuring/assembly-config/interfaces/scoring/ManageZonesInterface.tsx b/fission/src/ui/panels/configuring/assembly-config/interfaces/scoring/ManageZonesInterface.tsx index a051cc1120..ab9f47d6b1 100644 --- a/fission/src/ui/panels/configuring/assembly-config/interfaces/scoring/ManageZonesInterface.tsx +++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/scoring/ManageZonesInterface.tsx @@ -7,7 +7,7 @@ import PreferencesSystem from "@/systems/preferences/PreferencesSystem" import World from "@/systems/World" import MirabufSceneObject from "@/mirabuf/MirabufSceneObject" import { Box } from "@mui/material" -import { ConfigurationSavedEvent } from "../../ConfigurePanel" +import { ConfigurationSavedEvent } from "../../ConfigurationSavedEvent" import { AddButtonInteractiveColor, DeleteButton, EditButton } from "@/ui/components/StyledComponents" const saveZones = (zones: ScoringZonePreferences[] | undefined, field: MirabufSceneObject | undefined) => { diff --git a/fission/src/ui/panels/configuring/assembly-config/interfaces/scoring/ZoneConfigInterface.tsx b/fission/src/ui/panels/configuring/assembly-config/interfaces/scoring/ZoneConfigInterface.tsx index 409daeece8..b6a375caa4 100644 --- a/fission/src/ui/panels/configuring/assembly-config/interfaces/scoring/ZoneConfigInterface.tsx +++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/scoring/ZoneConfigInterface.tsx @@ -16,7 +16,7 @@ import { ToggleButton, ToggleButtonGroup } from "@/ui/components/ToggleButtonGro import { Alliance, ScoringZonePreferences } from "@/systems/preferences/PreferenceTypes" import { RigidNodeId } from "@/mirabuf/MirabufParser" import { DeltaFieldTransforms_PhysicalProp as DeltaFieldTransforms_VisualProperties } from "@/util/threejs/MeshCreation" -import { ConfigurationSavedEvent } from "../../ConfigurePanel" +import { ConfigurationSavedEvent } from "../../ConfigurationSavedEvent" /** * Saves ejector configuration to selected field. From 160df44d9880636e4ea93043020d9d04912b4f41 Mon Sep 17 00:00:00 2001 From: Hunter Barclay Date: Tue, 24 Sep 2024 23:09:23 -0600 Subject: [PATCH 11/16] [AARD-1863] Upgrade Vite to >=5.2.14, add missing dependencies. (#1109) --- fission/package.json | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/fission/package.json b/fission/package.json index 3b5df05cdc..fc86170db2 100644 --- a/fission/package.json +++ b/fission/package.json @@ -63,6 +63,7 @@ "eslint": "^8.56.0", "eslint-config-prettier": "^8.8.0", "eslint-import-resolver-alias": "^1.1.2", + "eslint-plugin-import": "^2.30.0", "eslint-plugin-react-hooks": "^4.6.0", "eslint-plugin-react-refresh": "^0.4.5", "jsdom": "^24.1.0", @@ -71,10 +72,11 @@ "prettier": "3.3.2", "protobufjs": "^7.2.6", "protobufjs-cli": "^1.1.2", + "rollup": "^4.22.4", "tailwindcss": "^3.3.3", "tsconfig-paths": "^4.2.0", "typescript": "^5.4.5", - "vite": "^5.1.4", + "vite": "5.2.14", "vite-plugin-glsl": "^1.3.0", "vite-plugin-singlefile": "^0.13.5", "vitest": "^1.5.3" From 394cdea743ca11d01c68c8877396ce026ef9dd31 Mon Sep 17 00:00:00 2001 From: HunterBarclay Date: Mon, 30 Sep 2024 13:57:02 -0600 Subject: [PATCH 12/16] fix: Change ports for TalonFX in Java Sample. Verified that everything seems to be working. However, haven't been able to test Gyros yet. --- .../org.eclipse.buildship.core.prefs | 13 ------ .../.settings/org.eclipse.jdt.core.prefs | 4 -- simulation/SyntheSimJava/build.gradle | 9 +++++ .../com/autodesk/synthesis/CANEncoder.java | 2 + .../java/com/autodesk/synthesis/CANMotor.java | 1 + .../com/autodesk/synthesis/ctre/TalonFX.java | 1 + .../autodesk/synthesis/kauailabs/AHRS.java | 38 ++++++++++++++++++ .../synthesis/revrobotics/CANSparkMax.java | 2 +- simulation/SyntheSimJava/vendordeps/NavX.json | 40 +++++++++++++++++++ .../org.eclipse.buildship.core.prefs | 13 ------ .../.settings/org.eclipse.jdt.core.prefs | 4 -- .../src/main/java/frc/robot/Robot.java | 23 ++++++----- 12 files changed, 106 insertions(+), 44 deletions(-) delete mode 100644 simulation/SyntheSimJava/.settings/org.eclipse.buildship.core.prefs delete mode 100644 simulation/SyntheSimJava/.settings/org.eclipse.jdt.core.prefs create mode 100644 simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/kauailabs/AHRS.java create mode 100644 simulation/SyntheSimJava/vendordeps/NavX.json delete mode 100644 simulation/samples/JavaSample/.settings/org.eclipse.buildship.core.prefs delete mode 100644 simulation/samples/JavaSample/.settings/org.eclipse.jdt.core.prefs diff --git a/simulation/SyntheSimJava/.settings/org.eclipse.buildship.core.prefs b/simulation/SyntheSimJava/.settings/org.eclipse.buildship.core.prefs deleted file mode 100644 index 424e44f630..0000000000 --- a/simulation/SyntheSimJava/.settings/org.eclipse.buildship.core.prefs +++ /dev/null @@ -1,13 +0,0 @@ -arguments=--init-script /Users/seth/.cache/jdtls/config/org.eclipse.osgi/331/0/.cp/gradle/init/init.gradle -auto.sync=false -build.scans.enabled=false -connection.gradle.distribution=GRADLE_DISTRIBUTION(WRAPPER) -connection.project.dir= -eclipse.preferences.version=1 -gradle.user.home= -java.home=/Library/Java/JavaVirtualMachines/zulu-17.jdk/Contents/Home -jvm.arguments= -offline.mode=false -override.workspace.settings=true -show.console.view=true -show.executions.view=true diff --git a/simulation/SyntheSimJava/.settings/org.eclipse.jdt.core.prefs b/simulation/SyntheSimJava/.settings/org.eclipse.jdt.core.prefs deleted file mode 100644 index 626e0e1d5c..0000000000 --- a/simulation/SyntheSimJava/.settings/org.eclipse.jdt.core.prefs +++ /dev/null @@ -1,4 +0,0 @@ -eclipse.preferences.version=1 -org.eclipse.jdt.core.compiler.codegen.targetPlatform=17 -org.eclipse.jdt.core.compiler.compliance=17 -org.eclipse.jdt.core.compiler.source=17 diff --git a/simulation/SyntheSimJava/build.gradle b/simulation/SyntheSimJava/build.gradle index 421980fb3d..e8631a287e 100644 --- a/simulation/SyntheSimJava/build.gradle +++ b/simulation/SyntheSimJava/build.gradle @@ -30,11 +30,17 @@ repositories { maven { url "https://maven.revrobotics.com/" } + + // KAUAI + maven { + url "https://dev.studica.com/maven/release/2024/" + } } def WPI_Version = '2024.3.2' def REV_Version = '2024.2.4' def CTRE_Version = '24.3.0' +def KAUAI_Version = '2024.1.0' dependencies { // This dependency is exported to consumers, that is to say found on their compile classpath. @@ -53,6 +59,9 @@ dependencies { // CTRE implementation "com.ctre.phoenix6:wpiapi-java:$CTRE_Version" + + // KAUAI + implementation "com.kauailabs.navx.frc:navx-frc-java:$KAUAI_Version" } java { diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/CANEncoder.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/CANEncoder.java index d00e5439b8..c15d448ef4 100644 --- a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/CANEncoder.java +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/CANEncoder.java @@ -31,6 +31,8 @@ public CANEncoder(String name, int deviceId) { m_init = m_device.createBoolean("init", Direction.kOutput, true); m_position = m_device.createDouble("position", Direction.kInput, 0.0); m_velocity = m_device.createDouble("velocity", Direction.kInput, 0.0); + + m_init.set(true); } /** diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/CANMotor.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/CANMotor.java index e4df23ba66..5116cd9fdb 100644 --- a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/CANMotor.java +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/CANMotor.java @@ -55,6 +55,7 @@ public CANMotor(String name, int deviceId, double defaultPercentOutput, boolean m_motorCurrent = m_device.createDouble("motorCurrent", Direction.kInput, 120.0); m_busVoltage = m_device.createDouble("busVoltage", Direction.kInput, 12.0); m_busVoltage.set(0.0); // disable CANMotor inputs + m_init.set(true); } /** diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/ctre/TalonFX.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/ctre/TalonFX.java index 5e671b86cd..3c67bbd1a5 100644 --- a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/ctre/TalonFX.java +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/ctre/TalonFX.java @@ -20,6 +20,7 @@ public TalonFX(int deviceNumber) { super(deviceNumber); this.m_motor = new CANMotor("SYN TalonFX", deviceNumber, 0.0, false, 0.3); + this.m_encoder = new CANEncoder("SYN TalonFX", deviceNumber); } /// I think we're getting percentOutput and speed mixed up diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/kauailabs/AHRS.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/kauailabs/AHRS.java new file mode 100644 index 0000000000..a69daec358 --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/kauailabs/AHRS.java @@ -0,0 +1,38 @@ +package com.autodesk.synthesis.kauailabs; + +import com.autodesk.synthesis.Gyro; + +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.SerialPort; + +/** + * Outline for a NavX AHRS class. + * TODO + */ +public class AHRS extends com.kauailabs.navx.frc.AHRS { + private Gyro m_gyro; + + public AHRS() { + this(SPI.Port.kMXP); + } + + public AHRS(I2C.Port port) { + super(port); + init("I2C", port.value); + } + + public AHRS(SPI.Port port) { + super(port); + init("SPI", port.value); + } + + public AHRS(SerialPort.Port port) { + super(port); + init("SERIAL", port.value); + } + + private void init(String commType, int port) { + this.m_gyro = new Gyro("SYN AHRS " + commType, port); + } +} diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/revrobotics/CANSparkMax.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/revrobotics/CANSparkMax.java index 560cc5bc79..06ec14707b 100644 --- a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/revrobotics/CANSparkMax.java +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/revrobotics/CANSparkMax.java @@ -28,7 +28,7 @@ public CANSparkMax(int deviceId, MotorType motorType) { this.m_motor = new CANMotor("SYN CANSparkMax", deviceId, 0.0, false, 0.3); this.m_encoder = new CANEncoder("SYN CANSparkMax", deviceId); - this.followers = new ArrayList(); + this.followers = new ArrayList(); } // setting a follower doesn't break the simulated follower - leader relationship diff --git a/simulation/SyntheSimJava/vendordeps/NavX.json b/simulation/SyntheSimJava/vendordeps/NavX.json new file mode 100644 index 0000000000..e978a5f745 --- /dev/null +++ b/simulation/SyntheSimJava/vendordeps/NavX.json @@ -0,0 +1,40 @@ +{ + "fileName": "NavX.json", + "name": "NavX", + "version": "2024.1.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2024/" + ], + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-cpp", + "version": "2024.1.0", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/simulation/samples/JavaSample/.settings/org.eclipse.buildship.core.prefs b/simulation/samples/JavaSample/.settings/org.eclipse.buildship.core.prefs deleted file mode 100644 index 424e44f630..0000000000 --- a/simulation/samples/JavaSample/.settings/org.eclipse.buildship.core.prefs +++ /dev/null @@ -1,13 +0,0 @@ -arguments=--init-script /Users/seth/.cache/jdtls/config/org.eclipse.osgi/331/0/.cp/gradle/init/init.gradle -auto.sync=false -build.scans.enabled=false -connection.gradle.distribution=GRADLE_DISTRIBUTION(WRAPPER) -connection.project.dir= -eclipse.preferences.version=1 -gradle.user.home= -java.home=/Library/Java/JavaVirtualMachines/zulu-17.jdk/Contents/Home -jvm.arguments= -offline.mode=false -override.workspace.settings=true -show.console.view=true -show.executions.view=true diff --git a/simulation/samples/JavaSample/.settings/org.eclipse.jdt.core.prefs b/simulation/samples/JavaSample/.settings/org.eclipse.jdt.core.prefs deleted file mode 100644 index 626e0e1d5c..0000000000 --- a/simulation/samples/JavaSample/.settings/org.eclipse.jdt.core.prefs +++ /dev/null @@ -1,4 +0,0 @@ -eclipse.preferences.version=1 -org.eclipse.jdt.core.compiler.codegen.targetPlatform=17 -org.eclipse.jdt.core.compiler.compliance=17 -org.eclipse.jdt.core.compiler.source=17 diff --git a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java index 28e4035ea9..302f80f47c 100644 --- a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java +++ b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.XboxController; import com.autodesk.synthesis.revrobotics.CANSparkMax; +import com.kauailabs.navx.frc.AHRS; import com.autodesk.synthesis.ctre.TalonFX; /** @@ -37,10 +38,11 @@ public class Robot extends TimedRobot { private Spark m_Spark1 = new Spark(0); private Spark m_Spark2 = new Spark(1); - private TalonFX m_Talon = new TalonFX(2); + private TalonFX m_Talon = new TalonFX(7); private XboxController m_Controller = new XboxController(0); private ADXL362 m_Accelerometer = new ADXL362(SPI.Port.kMXP, ADXL362.Range.k8G); + private AHRS m_Gyro = new AHRS(); private DigitalInput m_DI = new DigitalInput(0); private DigitalOutput m_DO = new DigitalOutput(1); @@ -141,18 +143,21 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() { - m_Spark1.set(m_Controller.getLeftY()); - m_Spark2.set(-m_Controller.getRightY()); - System.out.println("IN: " + m_DI.get()); - System.out.println("OUT: " + m_DO.get()); - System.out.println("AI: " + m_AI.getVoltage()); - m_Talon.set(-0.5); - m_SparkMax1.set(-0.75); - m_SparkMax2.set(-0.75); + m_SparkMax1.set(m_Controller.getLeftY()); + m_SparkMax2.set(-m_Controller.getRightY()); + m_Talon.set(m_Controller.getLeftX()); + System.out.println("LeftX: " + m_Controller.getLeftX()); + // System.out.println("OUT: " + m_DO.get()); + // System.out.println("AI: " + m_AI.getVoltage()); + // m_Talon.set(-0.5); + // m_SparkMax1.set(-0.75); + // m_SparkMax2.set(-0.75); m_SparkMax3.set(-0.75); m_SparkMax4.set(-0.75); m_SparkMax5.set(-0.75); m_SparkMax6.set(-0.75); + + m_SparkMax1.getEncoder().setPosition(0.0); } From 5347554bcd323787e6eeb4de58f590aed1bfb217 Mon Sep 17 00:00:00 2001 From: HunterBarclay Date: Mon, 30 Sep 2024 13:59:37 -0600 Subject: [PATCH 13/16] fix: Accelerometers metrics are relative, matching real devices. Also included some minor tweaks. --- .../simulation/wpilib_brain/SimInput.ts | 25 +++++++++++-------- .../simulation/wpilib_brain/SimOutput.ts | 2 +- .../simulation/wpilib_brain/WPILibBrain.ts | 16 ++++++------ .../rio-config/RCConfigEncoderModal.tsx | 2 +- fission/src/ui/panels/WSViewPanel.tsx | 25 +++++++++---------- 5 files changed, 36 insertions(+), 34 deletions(-) diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index ae4b427efd..637520a316 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -4,6 +4,8 @@ import { SimCANEncoder, SimGyro, SimAccel, SimDIO, SimAI } from "./WPILibBrain" import Mechanism from "@/systems/physics/Mechanism" import Jolt from "@barclah/jolt-physics" import JOLT from "@/util/loading/JoltSyncLoader" +import { JoltQuat_ThreeQuaternion, JoltVec3_ThreeVector3 } from "@/util/TypeConversions" +import * as THREE from 'three' export abstract class SimInput { constructor(protected _device: string) {} @@ -93,31 +95,32 @@ export class SimGyroInput extends SimInput { export class SimAccelInput extends SimInput { private _robot: Mechanism private _joltID?: Jolt.BodyID - private _joltBody?: Jolt.Body - private _prevVel: Jolt.Vec3 + private _prevVel: THREE.Vector3 constructor(device: string, robot: Mechanism) { super(device) this._robot = robot this._joltID = this._robot.nodeToBody.get(this._robot.rootBody) - this._prevVel = new JOLT.Vec3(0, 0, 0) - - if (this._joltID) this._joltBody = World.PhysicsSystem.GetBody(this._joltID) + this._prevVel = new THREE.Vector3(0,0,0) } public Update(deltaT: number) { - const newVel = this._joltBody?.GetLinearVelocity() - if (!newVel) return + if (!this._joltID) return + const body = World.PhysicsSystem.GetBody(this._joltID) + + const rot = JoltQuat_ThreeQuaternion(body.GetRotation()) + const mat = new THREE.Matrix4().makeRotationFromQuaternion(rot).transpose() + const newVel = JoltVec3_ThreeVector3(body.GetLinearVelocity()).applyMatrix4(mat) - const x = (newVel.GetX() - this._prevVel.GetX()) / deltaT - const y = (newVel.GetY() - this._prevVel.GetY()) / deltaT - const z = (newVel.GetZ() - this._prevVel.GetZ()) / deltaT + const x = (newVel.x - this._prevVel.x) / deltaT + const y = (newVel.y - this._prevVel.y) / deltaT + const z = (newVel.y - this._prevVel.y) / deltaT SimAccel.SetX(this._device, x) SimAccel.SetY(this._device, y) SimAccel.SetZ(this._device, z) - this._prevVel = new JOLT.Vec3(newVel.GetX(), newVel.GetY(), newVel.GetZ()) + this._prevVel = newVel } } diff --git a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts index c9c5c0f091..cefef5bf5d 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts @@ -61,7 +61,7 @@ export class CANOutputGroup extends SimOutputGroup { const average = this.ports.reduce((sum, port) => { const device = SimCAN.GetDeviceWithID(port, SimType.CANMotor) - return sum + +(device?.get(" { diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index 2f51979084..bce82be75d 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -6,8 +6,8 @@ import WPILibWSWorker from "./WPILibWSWorker?worker" import { SimulationLayer } from "../SimulationSystem" import World from "@/systems/World" -import { SimOutput } from "./SimOutput" -import { SimInput } from "./SimInput" +import { SimAnalogOutput, SimDigitalOutput, SimOutput } from "./SimOutput" +import { SimAccelInput, SimAnalogInput, SimDigitalInput, SimGyroInput, SimInput } from "./SimInput" const worker: Lazy = new Lazy(() => new WPILibWSWorker()) @@ -145,7 +145,7 @@ export class SimCAN { private constructor() {} public static GetDeviceWithID(id: number, type: SimType): DeviceData | undefined { - const id_exp = /.*\[(\d+)\]/g + const id_exp = /SYN.*\[(\d+)\]/g const entries = [...simMap.entries()].filter(([simType, _data]) => simType == type) for (const [_simType, data] of entries) { for (const key of data.keys()) { @@ -380,11 +380,11 @@ class WPILibBrain extends Brain { } // this.addSimInput(new SimGyroInput("Test Gyro[1]", mechanism)) - // this.addSimInput(new SimAccelInput("ADXL362[4]", mechanism)) - // this.addSimInput(new SimDigitalInput("SYN DI[0]", () => Math.random() > 0.5)) - // this.addSimOutput(new SimDigitalOutput("SYN DO[1]")) - // this.addSimInput(new SimAnalogInput("SYN AI[0]", () => Math.random() * 12)) - // this.addSimOutput(new SimAnalogOutput("SYN AO[1]")) + this.addSimInput(new SimAccelInput("ADXL362[4]", mechanism)) + this.addSimInput(new SimDigitalInput("SYN DI[0]", () => Math.random() > 0.5)) + this.addSimOutput(new SimDigitalOutput("SYN DO[1]")) + this.addSimInput(new SimAnalogInput("SYN AI[0]", () => Math.random() * 12)) + this.addSimOutput(new SimAnalogOutput("SYN AO[1]")) } public addSimOutput(device: SimOutput) { diff --git a/fission/src/ui/modals/configuring/rio-config/RCConfigEncoderModal.tsx b/fission/src/ui/modals/configuring/rio-config/RCConfigEncoderModal.tsx index c60af762f7..00e203565b 100644 --- a/fission/src/ui/modals/configuring/rio-config/RCConfigEncoderModal.tsx +++ b/fission/src/ui/modals/configuring/rio-config/RCConfigEncoderModal.tsx @@ -56,7 +56,7 @@ const RCConfigEncoderModal: React.FC = ({ modalId }) => { > - n[0])} onSelect={s => setSelectedDevice(s)} /> + n[0])} onSelect={s => setSelectedDevice(s)} /> {names.map(name => @@ -95,7 +95,9 @@ function setGeneric(simType: SimType, device: string, field: string, value: stri } const WSViewPanel: React.FC = ({ panelId }) => { - const [tb, setTb] = useState(generateTableBody()) + // const [tb, setTb] = useState(generateTableBody()) + + const [table, updateTable] = useReducer((_) => generateTableBody(), generateTableBody()) const [selectedType, setSelectedType] = useState() const [selectedDevice, setSelectedDevice] = useState() @@ -115,17 +117,14 @@ const WSViewPanel: React.FC = ({ panelId }) => { setSelectedDevice(undefined) }, [selectedType]) - const onSimMapUpdate = useCallback((_: Event) => { - setTb(generateTableBody()) - }, []) - useEffect(() => { - window.addEventListener(SimMapUpdateEvent.TYPE, onSimMapUpdate) + const func = () => { updateTable() } + const id: NodeJS.Timeout = setInterval(func, TABLE_UPDATE_INTERVAL) return () => { - window.removeEventListener(SimMapUpdateEvent.TYPE, onSimMapUpdate) + clearTimeout(id) } - }, [onSimMapUpdate]) + }, [updateTable]) return ( = ({ panelId }) => { - {tb} + {table} From cbc9033050b4a2043c02ae836b08649b2bc04cf9 Mon Sep 17 00:00:00 2001 From: HunterBarclay Date: Fri, 11 Oct 2024 10:58:04 -0600 Subject: [PATCH 14/16] chore: Format and cleanup --- .../src/systems/simulation/wpilib_brain/SimInput.ts | 4 ++-- .../systems/simulation/wpilib_brain/SimOutput.ts | 2 +- .../systems/simulation/wpilib_brain/WPILibBrain.ts | 2 +- fission/src/ui/panels/WSViewPanel.tsx | 6 ++++-- simulation/samples/CppSample/.gitignore | 2 ++ .../.settings/org.eclipse.buildship.core.prefs | 13 ------------- 6 files changed, 10 insertions(+), 19 deletions(-) delete mode 100644 simulation/samples/CppSample/.settings/org.eclipse.buildship.core.prefs diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index 637520a316..b4776f03a3 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -5,7 +5,7 @@ import Mechanism from "@/systems/physics/Mechanism" import Jolt from "@barclah/jolt-physics" import JOLT from "@/util/loading/JoltSyncLoader" import { JoltQuat_ThreeQuaternion, JoltVec3_ThreeVector3 } from "@/util/TypeConversions" -import * as THREE from 'three' +import * as THREE from "three" export abstract class SimInput { constructor(protected _device: string) {} @@ -101,7 +101,7 @@ export class SimAccelInput extends SimInput { super(device) this._robot = robot this._joltID = this._robot.nodeToBody.get(this._robot.rootBody) - this._prevVel = new THREE.Vector3(0,0,0) + this._prevVel = new THREE.Vector3(0, 0, 0) } public Update(deltaT: number) { diff --git a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts index cefef5bf5d..04e9205c7c 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts @@ -61,7 +61,7 @@ export class CANOutputGroup extends SimOutputGroup { const average = this.ports.reduce((sum, port) => { const device = SimCAN.GetDeviceWithID(port, SimType.CANMotor) - return sum + (device?.get(" { diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index bce82be75d..e0bd2779d9 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -379,7 +379,7 @@ class WPILibBrain extends Brain { return } - // this.addSimInput(new SimGyroInput("Test Gyro[1]", mechanism)) + this.addSimInput(new SimGyroInput("Test Gyro[1]", mechanism)) this.addSimInput(new SimAccelInput("ADXL362[4]", mechanism)) this.addSimInput(new SimDigitalInput("SYN DI[0]", () => Math.random() > 0.5)) this.addSimOutput(new SimDigitalOutput("SYN DO[1]")) diff --git a/fission/src/ui/panels/WSViewPanel.tsx b/fission/src/ui/panels/WSViewPanel.tsx index e109c48382..693db7c25c 100644 --- a/fission/src/ui/panels/WSViewPanel.tsx +++ b/fission/src/ui/panels/WSViewPanel.tsx @@ -97,7 +97,7 @@ function setGeneric(simType: SimType, device: string, field: string, value: stri const WSViewPanel: React.FC = ({ panelId }) => { // const [tb, setTb] = useState(generateTableBody()) - const [table, updateTable] = useReducer((_) => generateTableBody(), generateTableBody()) + const [table, updateTable] = useReducer(_ => generateTableBody(), generateTableBody()) const [selectedType, setSelectedType] = useState() const [selectedDevice, setSelectedDevice] = useState() @@ -118,7 +118,9 @@ const WSViewPanel: React.FC = ({ panelId }) => { }, [selectedType]) useEffect(() => { - const func = () => { updateTable() } + const func = () => { + updateTable() + } const id: NodeJS.Timeout = setInterval(func, TABLE_UPDATE_INTERVAL) return () => { diff --git a/simulation/samples/CppSample/.gitignore b/simulation/samples/CppSample/.gitignore index 11c9fdd738..bae79ccf97 100644 --- a/simulation/samples/CppSample/.gitignore +++ b/simulation/samples/CppSample/.gitignore @@ -2,3 +2,5 @@ .vscode/ .wpilib/ build/ +.settings/ + diff --git a/simulation/samples/CppSample/.settings/org.eclipse.buildship.core.prefs b/simulation/samples/CppSample/.settings/org.eclipse.buildship.core.prefs deleted file mode 100644 index bc1ff1181e..0000000000 --- a/simulation/samples/CppSample/.settings/org.eclipse.buildship.core.prefs +++ /dev/null @@ -1,13 +0,0 @@ -arguments=--init-script /Users/colbura/cache/jdtls/config/org.eclipse.osgi/55/0/.cp/gradle/init/init.gradle -auto.sync=false -build.scans.enabled=false -connection.gradle.distribution=GRADLE_DISTRIBUTION(WRAPPER) -connection.project.dir= -eclipse.preferences.version=1 -gradle.user.home= -java.home=/opt/homebrew/Cellar/openjdk/22.0.2/libexec/openjdk.jdk/Contents/Home -jvm.arguments= -offline.mode=false -override.workspace.settings=true -show.console.view=true -show.executions.view=true From 267ee5926a664fddf2588564715db87c687af512 Mon Sep 17 00:00:00 2001 From: HunterBarclay Date: Fri, 11 Oct 2024 11:05:45 -0600 Subject: [PATCH 15/16] fix: Use custom random generation --- fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index e0bd2779d9..7fa35eddaf 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -8,6 +8,7 @@ import World from "@/systems/World" import { SimAnalogOutput, SimDigitalOutput, SimOutput } from "./SimOutput" import { SimAccelInput, SimAnalogInput, SimDigitalInput, SimGyroInput, SimInput } from "./SimInput" +import { Random } from "@/util/Random" const worker: Lazy = new Lazy(() => new WPILibWSWorker()) @@ -381,9 +382,9 @@ class WPILibBrain extends Brain { this.addSimInput(new SimGyroInput("Test Gyro[1]", mechanism)) this.addSimInput(new SimAccelInput("ADXL362[4]", mechanism)) - this.addSimInput(new SimDigitalInput("SYN DI[0]", () => Math.random() > 0.5)) + this.addSimInput(new SimDigitalInput("SYN DI[0]", () => Random() > 0.5)) this.addSimOutput(new SimDigitalOutput("SYN DO[1]")) - this.addSimInput(new SimAnalogInput("SYN AI[0]", () => Math.random() * 12)) + this.addSimInput(new SimAnalogInput("SYN AI[0]", () => Random() * 12)) this.addSimOutput(new SimAnalogOutput("SYN AO[1]")) } From 9a27ecfa1a4a2d32866bbfb7ceb59ee87a2b5bec Mon Sep 17 00:00:00 2001 From: Dhruv Arora Date: Sat, 9 Nov 2024 12:48:57 -0800 Subject: [PATCH 16/16] [AARD-1798] Transform Gizmo Refactor (#1085) Co-authored-by: HunterBarclay Co-authored-by: PepperLola --- .../src/Parser/ExporterOptions.py | 2 +- exporter/SynthesisFusionAddin/src/Types.py | 2 +- fission/src/Synthesis.tsx | 4 + fission/src/aps/APS.ts | 26 +- fission/src/aps/APSDataManagement.ts | 12 +- fission/src/mirabuf/MirabufInstance.ts | 2 - fission/src/mirabuf/MirabufSceneObject.ts | 258 ++++++++++-------- fission/src/mirabuf/ScoringZoneSceneObject.ts | 6 - .../src/systems/analytics/AnalyticsSystem.ts | 2 - fission/src/systems/physics/PhysicsSystem.ts | 50 +++- fission/src/systems/scene/GizmoSceneObject.ts | 254 +++++++++++++++++ fission/src/systems/scene/SceneRenderer.ts | 132 +++------ .../synthesis_brain/SynthesisBrain.ts | 6 + .../src/ui/components/GlobalUIComponent.tsx | 48 ++++ fission/src/ui/components/GlobalUIControls.ts | 20 ++ fission/src/ui/components/MainHUD.tsx | 7 +- fission/src/ui/components/Scene.tsx | 3 - .../ui/components/TransformGizmoControl.tsx | 138 ++++++++++ .../components/TransformGizmoControlProps.ts | 22 ++ fission/src/ui/components/TransformGizmos.tsx | 94 ------- .../mirabuf/ImportLocalMirabufModal.tsx | 10 +- fission/src/ui/panels/DebugPanel.tsx | 4 +- .../configuring/ChooseInputSchemePanel.tsx | 140 +++------- .../configuring/TransformAssemblyPanel.tsx | 51 ++++ .../assembly-config/ConfigurePanel.tsx | 20 +- .../ConfigureGamepiecePickupInterface.tsx | 126 +++++---- .../ConfigureShotTrajectoryInterface.tsx | 115 ++++---- .../scoring/ManageZonesInterface.tsx | 5 +- .../scoring/ZoneConfigInterface.tsx | 193 +++++++------ .../initial-config/InitialConfigPanel.tsx | 114 ++++++++ .../initial-config/InputSchemeSelection.tsx | 106 +++++++ .../InputSchemeSelectionProps.ts | 8 + .../ui/panels/mirabuf/ImportMirabufPanel.tsx | 33 +-- 33 files changed, 1317 insertions(+), 696 deletions(-) create mode 100644 fission/src/systems/scene/GizmoSceneObject.ts create mode 100644 fission/src/ui/components/GlobalUIComponent.tsx create mode 100644 fission/src/ui/components/GlobalUIControls.ts create mode 100644 fission/src/ui/components/TransformGizmoControl.tsx create mode 100644 fission/src/ui/components/TransformGizmoControlProps.ts delete mode 100644 fission/src/ui/components/TransformGizmos.tsx create mode 100644 fission/src/ui/panels/configuring/TransformAssemblyPanel.tsx create mode 100644 fission/src/ui/panels/configuring/initial-config/InitialConfigPanel.tsx create mode 100644 fission/src/ui/panels/configuring/initial-config/InputSchemeSelection.tsx create mode 100644 fission/src/ui/panels/configuring/initial-config/InputSchemeSelectionProps.ts diff --git a/exporter/SynthesisFusionAddin/src/Parser/ExporterOptions.py b/exporter/SynthesisFusionAddin/src/Parser/ExporterOptions.py index 634c005e9d..1650e75380 100644 --- a/exporter/SynthesisFusionAddin/src/Parser/ExporterOptions.py +++ b/exporter/SynthesisFusionAddin/src/Parser/ExporterOptions.py @@ -67,7 +67,7 @@ def readFromDesign(self) -> "ExporterOptions": for field in fields(self): attribute = designAttributes.itemByName(INTERNAL_ID, field.name) if attribute: - attrJsonData = makeObjectFromJson(field.type, json.loads(attribute.value)) + attrJsonData = makeObjectFromJson(type(field.type), json.loads(attribute.value)) setattr(self, field.name, attrJsonData) return self diff --git a/exporter/SynthesisFusionAddin/src/Types.py b/exporter/SynthesisFusionAddin/src/Types.py index 166bc43976..4219bf1814 100644 --- a/exporter/SynthesisFusionAddin/src/Types.py +++ b/exporter/SynthesisFusionAddin/src/Types.py @@ -109,7 +109,7 @@ def makeObjectFromJson(objType: type, data: Any) -> Any: assert is_dataclass(obj) and isinstance(data, dict), "Found unsupported type to decode." for field in fields(obj): if field.name in data: - setattr(obj, field.name, makeObjectFromJson(field.type, data[field.name])) + setattr(obj, field.name, makeObjectFromJson(type(field.type), data[field.name])) else: setattr(obj, field.name, field.default_factory if field.default_factory is not MISSING else field.default) diff --git a/fission/src/Synthesis.tsx b/fission/src/Synthesis.tsx index 7052b887d5..e800323dbd 100644 --- a/fission/src/Synthesis.tsx +++ b/fission/src/Synthesis.tsx @@ -58,6 +58,8 @@ import AnalyticsConsent from "./ui/components/AnalyticsConsent.tsx" import PreferencesSystem from "./systems/preferences/PreferencesSystem.ts" import APSManagementModal from "./ui/modals/APSManagementModal.tsx" import ConfigurePanel from "./ui/panels/configuring/assembly-config/ConfigurePanel.tsx" +import GlobalUIComponent from "./ui/components/GlobalUIComponent.tsx" +import InitialConfigPanel from "./ui/panels/configuring/initial-config/InitialConfigPanel.tsx" function Synthesis() { const { openModal, closeModal, getActiveModalElement } = useModalManager(initialModals) @@ -156,6 +158,7 @@ function Synthesis() { closeAllPanels={closeAllPanels} > + @@ -222,6 +225,7 @@ const initialPanels: ReactElement[] = [ , , , + , ] export default Synthesis diff --git a/fission/src/aps/APS.ts b/fission/src/aps/APS.ts index 4873d92136..d9ae0500e3 100644 --- a/fission/src/aps/APS.ts +++ b/fission/src/aps/APS.ts @@ -1,5 +1,5 @@ import World from "@/systems/World" -import { MainHUD_AddToast } from "@/ui/components/MainHUD" +import { Global_AddToast } from "@/ui/components/GlobalUIControls" import { Mutex } from "async-mutex" const APS_AUTH_KEY = "aps_auth" @@ -14,7 +14,7 @@ export const ENDPOINT_SYNTHESIS_CHALLENGE = `/api/aps/challenge` const ENDPOINT_AUTODESK_AUTHENTICATION_AUTHORIZE = "https://developer.api.autodesk.com/authentication/v2/authorize" const ENDPOINT_AUTODESK_AUTHENTICATION_TOKEN = "https://developer.api.autodesk.com/authentication/v2/token" -const ENDPOINT_AUTODESK_REVOKE_TOKEN = "https://developer.api.autodesk.com/authentication/v2/revoke" +const ENDPOINT_AUTODESK_AUTHENTICATION_REVOKE = "https://developer.api.autodesk.com/authentication/v2/revoke" const ENDPOINT_AUTODESK_USERINFO = "https://api.userprofile.autodesk.com/userinfo" export interface APSAuth { @@ -163,7 +163,7 @@ class APS { ["client_id", CLIENT_ID], ] as string[][]), } - const res = await fetch(ENDPOINT_AUTODESK_REVOKE_TOKEN, opts) + const res = await fetch(ENDPOINT_AUTODESK_AUTHENTICATION_REVOKE, opts) if (!res.ok) { console.log("Failed to revoke auth token:\n") return false @@ -205,7 +205,7 @@ class APS { } catch (e) { console.error(e) World.AnalyticsSystem?.Exception("APS Login Failure") - MainHUD_AddToast("error", "Error signing in.", "Please try again.") + Global_AddToast?.("error", "Error signing in.", "Please try again.") } }) } @@ -236,7 +236,7 @@ class APS { const json = await res.json() if (!res.ok) { if (shouldRelog) { - MainHUD_AddToast("warning", "Must Re-signin.", json.userMessage) + Global_AddToast?.("warning", "Must Re-signin.", json.userMessage) this.auth = undefined await this.requestAuthCode() return false @@ -249,13 +249,13 @@ class APS { if (this.auth) { await this.loadUserInfo(this.auth) if (APS.userInfo) { - MainHUD_AddToast("info", "ADSK Login", `Hello, ${APS.userInfo.givenName}`) + Global_AddToast?.("info", "ADSK Login", `Hello, ${APS.userInfo.givenName}`) } } return true } catch (e) { World.AnalyticsSystem?.Exception("APS Login Failure") - MainHUD_AddToast("error", "Error signing in.", "Please try again.") + Global_AddToast?.("error", "Error signing in.", "Please try again.") this.auth = undefined await this.requestAuthCode() return false @@ -281,7 +281,7 @@ class APS { const json = await res.json() if (!res.ok) { World.AnalyticsSystem?.Exception("APS Login Failure") - MainHUD_AddToast("error", "Error signing in.", json.userMessage) + Global_AddToast?.("error", "Error signing in.", json.userMessage) this.auth = undefined return } @@ -293,7 +293,7 @@ class APS { if (auth) { await this.loadUserInfo(auth) if (APS.userInfo) { - MainHUD_AddToast("info", "ADSK Login", `Hello, ${APS.userInfo.givenName}`) + Global_AddToast?.("info", "ADSK Login", `Hello, ${APS.userInfo.givenName}`) } } else { console.error("Couldn't get auth data.") @@ -306,7 +306,7 @@ class APS { if (retry_login) { this.auth = undefined World.AnalyticsSystem?.Exception("APS Login Failure") - MainHUD_AddToast("error", "Error signing in.", "Please try again.") + Global_AddToast?.("error", "Error signing in.", "Please try again.") } } @@ -327,7 +327,7 @@ class APS { const json = await res.json() if (!res.ok) { World.AnalyticsSystem?.Exception("APS Failure: User Info") - MainHUD_AddToast("error", "Error fetching user data.", json.userMessage) + Global_AddToast?.("error", "Error fetching user data.", json.userMessage) this.auth = undefined await this.requestAuthCode() return @@ -347,7 +347,7 @@ class APS { } catch (e) { console.error(e) World.AnalyticsSystem?.Exception("APS Login Failure: User Info") - MainHUD_AddToast("error", "Error signing in.", "Please try again.") + Global_AddToast?.("error", "Error signing in.", "Please try again.") this.auth = undefined } } @@ -363,7 +363,7 @@ class APS { } catch (e) { console.error(e) World.AnalyticsSystem?.Exception("APS Login Failure: Code Challenge") - MainHUD_AddToast("error", "Error signing in.", "Please try again.") + Global_AddToast?.("error", "Error signing in.", "Please try again.") } } } diff --git a/fission/src/aps/APSDataManagement.ts b/fission/src/aps/APSDataManagement.ts index 6deac410ea..b19e583eda 100644 --- a/fission/src/aps/APSDataManagement.ts +++ b/fission/src/aps/APSDataManagement.ts @@ -1,5 +1,5 @@ /* eslint-disable @typescript-eslint/no-explicit-any */ -import { MainHUD_AddToast } from "@/ui/components/MainHUD" +import { Global_AddToast } from "@/ui/components/GlobalUIControls" import APS from "./APS" import TaskStatus from "@/util/TaskStatus" import { Mutex } from "async-mutex" @@ -140,9 +140,9 @@ export async function getHubs(): Promise { console.log(auth) console.log(APS.userInfo) if (e instanceof APSDataError) { - MainHUD_AddToast("error", e.title, e.detail) + Global_AddToast?.("error", e.title, e.detail) } else if (e instanceof Error) { - MainHUD_AddToast("error", "Failed to get hubs.", e.message) + Global_AddToast?.("error", "Failed to get hubs.", e.message) } return undefined } @@ -179,7 +179,7 @@ export async function getProjects(hub: Hub): Promise { } catch (e) { console.error("Failed to get hubs") if (e instanceof Error) { - MainHUD_AddToast("error", "Failed to get hubs.", e.message) + Global_AddToast?.("error", "Failed to get hubs.", e.message) } return undefined } @@ -224,7 +224,7 @@ export async function getFolderData(project: Project, folder: Folder): Promise { const count = countMap.get(material)! diff --git a/fission/src/mirabuf/MirabufSceneObject.ts b/fission/src/mirabuf/MirabufSceneObject.ts index 2804f3ae1b..13725e9130 100644 --- a/fission/src/mirabuf/MirabufSceneObject.ts +++ b/fission/src/mirabuf/MirabufSceneObject.ts @@ -4,13 +4,12 @@ import MirabufInstance from "./MirabufInstance" import MirabufParser, { ParseErrorSeverity, RigidNodeId, RigidNodeReadOnly } from "./MirabufParser" import World from "@/systems/World" import Jolt from "@barclah/jolt-physics" -import { JoltMat44_ThreeMatrix4 } from "@/util/TypeConversions" +import { JoltMat44_ThreeMatrix4, JoltVec3_ThreeVector3 } from "@/util/TypeConversions" import * as THREE from "three" import JOLT from "@/util/loading/JoltSyncLoader" import { BodyAssociate, LayerReserve } from "@/systems/physics/PhysicsSystem" import Mechanism from "@/systems/physics/Mechanism" import InputSystem from "@/systems/input/InputSystem" -import TransformGizmos from "@/ui/components/TransformGizmos" import { EjectorPreferences, FieldPreferences, IntakePreferences } from "@/systems/preferences/PreferenceTypes" import PreferencesSystem from "@/systems/preferences/PreferencesSystem" import { MiraType } from "./MirabufLoader" @@ -21,6 +20,7 @@ import ScoringZoneSceneObject from "./ScoringZoneSceneObject" import { SceneOverlayTag } from "@/ui/components/SceneOverlayEvents" import { ProgressHandle } from "@/ui/components/ProgressNotificationData" import SynthesisBrain from "@/systems/simulation/synthesis_brain/SynthesisBrain" +import GizmoSceneObject from "@/systems/scene/GizmoSceneObject" const DEBUG_BODIES = false @@ -29,6 +29,23 @@ interface RnDebugMeshes { comMesh: THREE.Mesh } +/** + * The goal with the spotlight assembly is to provide a contextual target assembly + * the user would like to modifiy. Generally this will be which even assembly was + * last spawned in, however, systems (such as the configuration UI) can elect + * assemblies to be in the spotlight when moving from interface to interface. + */ +let spotlightAssembly: number | undefined + +export function setSpotlightAssembly(assembly: MirabufSceneObject) { + spotlightAssembly = assembly.id +} + +// TODO: If nothing is in the spotlight, select last entry before defaulting to undefined +export function getSpotlightAssembly(): MirabufSceneObject | undefined { + return World.SceneRenderer.sceneObjects.get(spotlightAssembly ?? 0) as MirabufSceneObject +} + class MirabufSceneObject extends SceneObject { private _assemblyName: string private _mirabufInstance: MirabufInstance @@ -38,8 +55,6 @@ class MirabufSceneObject extends SceneObject { private _debugBodies: Map | null private _physicsLayerReserve: LayerReserve | undefined - private _transformGizmos: TransformGizmos | undefined - private _intakePreferences: IntakePreferences | undefined private _ejectorPreferences: EjectorPreferences | undefined @@ -106,8 +121,6 @@ class MirabufSceneObject extends SceneObject { this._debugBodies = null - this.EnableTransformControls() // adding transform gizmo to mirabuf object on its creation - this.getPreferences() // creating nametag for robots @@ -150,96 +163,53 @@ class MirabufSceneObject extends SceneObject { }) // Simulation - World.SimulationSystem.RegisterMechanism(this._mechanism) - const simLayer = World.SimulationSystem.GetSimulationLayer(this._mechanism)! - this._brain = new SynthesisBrain(this._mechanism, this._assemblyName) - simLayer.SetBrain(this._brain) + 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) + simLayer.SetBrain(this._brain) + } // Intake this.UpdateIntakeSensor() - this.UpdateScoringZones() - } - public Update(): void { - const brainIndex = this._brain instanceof SynthesisBrain ? this._brain.brainIndex ?? -1 : -1 - if (InputSystem.getInput("eject", brainIndex)) { - this.Eject() - } + setSpotlightAssembly(this) - this._mirabufInstance.parser.rigidNodes.forEach(rn => { - if (!this._mirabufInstance.meshes.size) return // if this.dispose() has been ran then return - const body = World.PhysicsSystem.GetBody(this._mechanism.GetBodyByNodeId(rn.id)!) - const transform = JoltMat44_ThreeMatrix4(body.GetWorldTransform()) - rn.parts.forEach(part => { - const partTransform = this._mirabufInstance.parser.globalTransforms - .get(part)! - .clone() - .premultiply(transform) - const meshes = this._mirabufInstance.meshes.get(part) ?? [] - meshes.forEach(([batch, id]) => batch.setMatrixAt(id, partTransform)) - }) + this.UpdateBatches() - /** - * Update the position and rotation of the body to match the position of the transform gizmo. - * - * This block of code should only be executed if the transform gizmo exists. - */ - if (this._transformGizmos) { - if (InputSystem.isKeyPressed("Enter")) { - // confirming placement of the mirabuf object - this.DisableTransformControls() - return - } else if (InputSystem.isKeyPressed("Escape")) { - // cancelling the creation of the mirabuf scene object - World.SceneRenderer.RemoveSceneObject(this.id) - return - } - - // if the gizmo is being dragged, copy the mesh position and rotation to the Mirabuf body - if (this._transformGizmos.isBeingDragged()) { - this._transformGizmos.UpdateMirabufPositioning(this, rn) - World.PhysicsSystem.DisablePhysicsForBody(this._mechanism.GetBodyByNodeId(rn.id)!) - } - } + const bounds = this.ComputeBoundingBox() + if (!Number.isFinite(bounds.min.y)) return - if (isNaN(body.GetPosition().GetX())) { - const vel = body.GetLinearVelocity() - const pos = body.GetPosition() - console.warn( - `Invalid Position.\nPosition => ${pos.GetX()}, ${pos.GetY()}, ${pos.GetZ()}\nVelocity => ${vel.GetX()}, ${vel.GetY()}, ${vel.GetZ()}` - ) - } - // console.debug(`POSITION: ${body.GetPosition().GetX()}, ${body.GetPosition().GetY()}, ${body.GetPosition().GetZ()}`) + const offset = new JOLT.Vec3( + -(bounds.min.x + bounds.max.x) / 2.0, + 0.1 + (bounds.max.y - bounds.min.y) / 2.0 - (bounds.min.y + bounds.max.y) / 2.0, + -(bounds.min.z + bounds.max.z) / 2.0 + ) - if (this._debugBodies) { - const { colliderMesh, comMesh } = this._debugBodies.get(rn.id)! - colliderMesh.position.setFromMatrixPosition(transform) - colliderMesh.rotation.setFromRotationMatrix(transform) + this._mirabufInstance.parser.rigidNodes.forEach(rn => { + const jBodyId = this._mechanism.GetBodyByNodeId(rn.id) + if (!jBodyId) return - const comTransform = JoltMat44_ThreeMatrix4(body.GetCenterOfMassTransform()) + const newPos = World.PhysicsSystem.GetBody(jBodyId).GetPosition().Add(offset) + World.PhysicsSystem.SetBodyPosition(jBodyId, newPos) - comMesh.position.setFromMatrixPosition(comTransform) - comMesh.rotation.setFromRotationMatrix(comTransform) - } + JOLT.destroy(newPos) }) - this._mirabufInstance.batches.forEach(x => { - x.computeBoundingBox() - x.computeBoundingSphere() - }) + this.UpdateMeshTransforms() + } - /* Updating the position of the name tag according to the robots position on screen */ - if (this._nameTag && PreferencesSystem.getGlobalPreference("RenderSceneTags")) { - const boundingBox = this.ComputeBoundingBox() - this._nameTag.position = World.SceneRenderer.WorldToPixelSpace( - new THREE.Vector3( - (boundingBox.max.x + boundingBox.min.x) / 2, - boundingBox.max.y + 0.1, - (boundingBox.max.z + boundingBox.min.z) / 2 - ) - ) + public Update(): void { + const brainIndex = this._brain instanceof SynthesisBrain ? this._brain.brainIndex ?? -1 : -1 + if (InputSystem.getInput("eject", brainIndex)) { + this.Eject() } + + this.UpdateMeshTransforms() + + this.UpdateBatches() + this.UpdateNameTag() } public Dispose(): void { @@ -261,7 +231,6 @@ class MirabufSceneObject extends SceneObject { }) this._nameTag?.Dispose() - this.DisableTransformControls() World.SimulationSystem.UnregisterMechanism(this._mechanism) World.PhysicsSystem.DestroyMechanism(this._mechanism) this._mirabufInstance.Dispose(World.SceneRenderer.scene) @@ -275,7 +244,9 @@ class MirabufSceneObject extends SceneObject { this._debugBodies?.clear() this._physicsLayerReserve?.Release() - if (this._brain && this._brain instanceof SynthesisBrain) this._brain?.clearControls() + if (this._brain && this._brain instanceof SynthesisBrain) { + this._brain.clearControls() + } } public Eject() { @@ -321,6 +292,70 @@ class MirabufSceneObject extends SceneObject { return mesh } + /** + * Matches mesh transforms to their Jolt counterparts. + */ + public UpdateMeshTransforms() { + this._mirabufInstance.parser.rigidNodes.forEach(rn => { + if (!this._mirabufInstance.meshes.size) return // if this.dispose() has been ran then return + const body = World.PhysicsSystem.GetBody(this._mechanism.GetBodyByNodeId(rn.id)!) + const transform = JoltMat44_ThreeMatrix4(body.GetWorldTransform()) + this.UpdateNodeParts(rn, transform) + + if (isNaN(body.GetPosition().GetX())) { + const vel = body.GetLinearVelocity() + const pos = body.GetPosition() + console.warn( + `Invalid Position.\nPosition => ${pos.GetX()}, ${pos.GetY()}, ${pos.GetZ()}\nVelocity => ${vel.GetX()}, ${vel.GetY()}, ${vel.GetZ()}` + ) + } + + if (this._debugBodies) { + const { colliderMesh, comMesh } = this._debugBodies.get(rn.id)! + colliderMesh.position.setFromMatrixPosition(transform) + colliderMesh.rotation.setFromRotationMatrix(transform) + + const comTransform = JoltMat44_ThreeMatrix4(body.GetCenterOfMassTransform()) + + comMesh.position.setFromMatrixPosition(comTransform) + comMesh.rotation.setFromRotationMatrix(comTransform) + } + }) + } + + public UpdateNodeParts(rn: RigidNodeReadOnly, transform: THREE.Matrix4) { + rn.parts.forEach(part => { + const partTransform = this._mirabufInstance.parser.globalTransforms + .get(part)! + .clone() + .premultiply(transform) + const meshes = this._mirabufInstance.meshes.get(part) ?? [] + meshes.forEach(([batch, id]) => batch.setMatrixAt(id, partTransform)) + }) + } + + /** Updates the batch computations */ + private UpdateBatches() { + this._mirabufInstance.batches.forEach(x => { + x.computeBoundingBox() + x.computeBoundingSphere() + }) + } + + /** Updates the position of the nametag relative to the robots position */ + private UpdateNameTag() { + if (this._nameTag && PreferencesSystem.getGlobalPreference("RenderSceneTags")) { + const boundingBox = this.ComputeBoundingBox() + this._nameTag.position = World.SceneRenderer.WorldToPixelSpace( + new THREE.Vector3( + (boundingBox.max.x + boundingBox.min.x) / 2, + boundingBox.max.y + 0.1, + (boundingBox.max.z + boundingBox.min.z) / 2 + ) + ) + } + } + public UpdateIntakeSensor() { if (this._intakeSensor) { World.SceneRenderer.RemoveSceneObject(this._intakeSensor.id) @@ -372,34 +407,7 @@ class MirabufSceneObject extends SceneObject { } /** - * Changes the mode of the mirabuf object from being interacted with to being placed. - */ - public EnableTransformControls(): void { - if (this._transformGizmos) return - - this._transformGizmos = new TransformGizmos( - new THREE.Mesh( - new THREE.SphereGeometry(3.0), - new THREE.MeshBasicMaterial({ color: 0x000000, transparent: true, opacity: 0 }) - ) - ) - this._transformGizmos.AddMeshToScene() - this._transformGizmos.CreateGizmo("translate", 5.0) - - this.DisablePhysics() - } - - /** - * Changes the mode of the mirabuf object from being placed to being interacted with. - */ - public DisableTransformControls(): void { - if (!this._transformGizmos) return - this._transformGizmos?.RemoveGizmos() - this._transformGizmos = undefined - this.EnablePhysics() - } - - /** + * Calculates the bounding box of the mirabuf object. * * @returns The bounding box of the mirabuf object. */ @@ -412,6 +420,30 @@ class MirabufSceneObject extends SceneObject { return box } + /** + * Once a gizmo is created and attached to this mirabuf object, this will be executed to align the gizmo correctly. + * + * @param gizmo Gizmo attached to the mirabuf object + */ + public PostGizmoCreation(gizmo: GizmoSceneObject) { + const jRootId = this.GetRootNodeId() + if (!jRootId) { + console.error("No root node found.") + return + } + + const jBody = World.PhysicsSystem.GetBody(jRootId) + if (jBody.IsStatic()) { + const aaBox = jBody.GetWorldSpaceBounds() + const mat = new THREE.Matrix4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1) + const center = aaBox.mMin.Add(aaBox.mMax).Div(2.0) + mat.compose(JoltVec3_ThreeVector3(center), new THREE.Quaternion(0, 0, 0, 1), new THREE.Vector3(1, 1, 1)) + gizmo.SetTransform(mat) + } else { + gizmo.SetTransform(JoltMat44_ThreeMatrix4(jBody.GetCenterOfMassTransform())) + } + } + private getPreferences(): void { this._intakePreferences = PreferencesSystem.getRobotPreferences(this.assemblyName)?.intake this._ejectorPreferences = PreferencesSystem.getRobotPreferences(this.assemblyName)?.ejector @@ -419,13 +451,13 @@ class MirabufSceneObject extends SceneObject { this._fieldPreferences = PreferencesSystem.getFieldPreferences(this.assemblyName) } - private EnablePhysics() { + public EnablePhysics() { this._mirabufInstance.parser.rigidNodes.forEach(rn => { World.PhysicsSystem.EnablePhysicsForBody(this._mechanism.GetBodyByNodeId(rn.id)!) }) } - private DisablePhysics() { + public DisablePhysics() { this._mirabufInstance.parser.rigidNodes.forEach(rn => { World.PhysicsSystem.DisablePhysicsForBody(this._mechanism.GetBodyByNodeId(rn.id)!) }) diff --git a/fission/src/mirabuf/ScoringZoneSceneObject.ts b/fission/src/mirabuf/ScoringZoneSceneObject.ts index 469655aa12..d6984bac73 100644 --- a/fission/src/mirabuf/ScoringZoneSceneObject.ts +++ b/fission/src/mirabuf/ScoringZoneSceneObject.ts @@ -58,8 +58,6 @@ class ScoringZoneSceneObject extends SceneObject { public constructor(parentAssembly: MirabufSceneObject, index: number, render?: boolean) { super() - console.debug("Trying to create scoring zone...") - this._parentAssembly = parentAssembly this._prefs = this._parentAssembly.fieldPreferences?.scoringZones[index] this._toRender = render ?? PreferencesSystem.getGlobalPreference("RenderScoringZones") @@ -138,8 +136,6 @@ class ScoringZoneSceneObject extends SceneObject { } OnContactRemovedEvent.AddListener(this._collisionRemoved) } - - console.debug("Scoring zone created successfully") } } } @@ -195,8 +191,6 @@ class ScoringZoneSceneObject extends SceneObject { } public Dispose(): void { - console.debug("Destroying scoring zone") - if (this._joltBodyId) { World.PhysicsSystem.DestroyBodyIds(this._joltBodyId) if (this._mesh) { diff --git a/fission/src/systems/analytics/AnalyticsSystem.ts b/fission/src/systems/analytics/AnalyticsSystem.ts index 4eb0b9fa27..a7dc4da255 100644 --- a/fission/src/systems/analytics/AnalyticsSystem.ts +++ b/fission/src/systems/analytics/AnalyticsSystem.ts @@ -78,8 +78,6 @@ class AnalyticsSystem extends WorldSystem { betaCode = betaCode.substring(betaCode.indexOf("=") + 1, betaCode.indexOf(";")) this.SetUserProperty("Beta Code", betaCode) - } else { - console.debug("No code match") } if (MOBILE_USER_AGENT_REGEX.test(navigator.userAgent)) { diff --git a/fission/src/systems/physics/PhysicsSystem.ts b/fission/src/systems/physics/PhysicsSystem.ts index eaeb5f3dcc..5635ac6721 100644 --- a/fission/src/systems/physics/PhysicsSystem.ts +++ b/fission/src/systems/physics/PhysicsSystem.ts @@ -27,6 +27,10 @@ import PreferencesSystem from "../preferences/PreferencesSystem" export type JoltBodyIndexAndSequence = number +export const PAUSE_REF_ASSEMBLY_SPAWNING = "assembly-spawning" +export const PAUSE_REF_ASSEMBLY_CONFIG = "assembly-config" +export const PAUSE_REF_ASSEMBLY_MOVE = "assembly-move" + /** * Layers used for determining enabled/disabled collisions. */ @@ -79,12 +83,12 @@ class PhysicsSystem extends WorldSystem { private _physicsEventQueue: PhysicsEvent[] = [] - private _pauseCounter = 0 + private _pauseSet = new Set() private _bodyAssociations: Map public get isPaused(): boolean { - return this._pauseCounter > 0 + return this._pauseSet.size > 0 } /** @@ -111,7 +115,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.0, 0.0), + new THREE.Vector3(0.0, -0.5, 0.0), undefined ) ground.SetFriction(FLOOR_FRICTION) @@ -146,28 +150,28 @@ class PhysicsSystem extends WorldSystem { /** * Holds a pause. * - * The pause works off of a request counter. + * @param ref String to reference your hold. */ - public HoldPause() { - this._pauseCounter++ + public HoldPause(ref: string) { + this._pauseSet.add(ref) } /** * Forces all holds on the pause to be released. */ public ForceUnpause() { - this._pauseCounter = 0 + this._pauseSet.clear() } /** * Releases a pause. * - * The pause works off of a request counter. + * @param ref String to reference your hold. + * + * @returns Whether or not your hold was successfully removed. */ - public ReleasePause() { - if (this._pauseCounter > 0) { - this._pauseCounter-- - } + public ReleasePause(ref: string): boolean { + return this._pauseSet.delete(ref) } /** @@ -965,7 +969,7 @@ class PhysicsSystem extends WorldSystem { } public Update(deltaT: number): void { - if (this._pauseCounter > 0) { + if (this._pauseSet.size > 0) { return } @@ -1052,7 +1056,7 @@ class PhysicsSystem extends WorldSystem { * @param id The id of the body * @param position The new position of the body */ - public SetBodyPosition(id: Jolt.BodyID, position: Jolt.Vec3, activate: boolean = true): void { + public SetBodyPosition(id: Jolt.BodyID, position: Jolt.RVec3, activate: boolean = true): void { if (!this.IsBodyAdded(id)) { return } @@ -1076,6 +1080,24 @@ class PhysicsSystem extends WorldSystem { ) } + public SetBodyPositionAndRotation( + id: Jolt.BodyID, + position: Jolt.RVec3, + rotation: Jolt.Quat, + activate: boolean = true + ): void { + if (!this.IsBodyAdded(id)) { + return + } + + this._joltBodyInterface.SetPositionAndRotation( + id, + position, + rotation, + activate ? JOLT.EActivation_Activate : JOLT.EActivation_DontActivate + ) + } + /** * Exposes SetShape method on the _joltBodyInterface * Sets the shape of the body diff --git a/fission/src/systems/scene/GizmoSceneObject.ts b/fission/src/systems/scene/GizmoSceneObject.ts new file mode 100644 index 0000000000..81137b2452 --- /dev/null +++ b/fission/src/systems/scene/GizmoSceneObject.ts @@ -0,0 +1,254 @@ +import * as THREE from "three" +import SceneObject from "./SceneObject" +import { TransformControls } from "three/examples/jsm/controls/TransformControls.js" +import InputSystem from "../input/InputSystem" +import World from "../World" +import MirabufSceneObject from "@/mirabuf/MirabufSceneObject" +import { Object3D, PerspectiveCamera } from "three" +import { ThreeQuaternion_JoltQuat, JoltMat44_ThreeMatrix4, ThreeVector3_JoltVec3 } from "@/util/TypeConversions" +import { RigidNodeId } from "@/mirabuf/MirabufParser" + +export type GizmoMode = "translate" | "rotate" | "scale" + +class GizmoSceneObject extends SceneObject { + private _gizmo: TransformControls + private _obj: Object3D + private _forceUpdate: boolean = false + + private _parentObject?: MirabufSceneObject + private _relativeTransformations?: Map + + private _mainCamera: PerspectiveCamera + + private _size: number + + /** @returns the instance of the transform gizmo itself */ + public get gizmo() { + return this._gizmo + } + + /** @returns Object3D that is attached to transform gizmo */ + public get obj() { + return this._obj + } + + /** @returns true if gizmo is currently being dragged */ + public get isDragging() { + return this._gizmo.dragging + } + + /** @returns the id of the parent scene object */ + public get parentObjectId() { + return this._parentObject?.id + } + + public constructor( + mode: GizmoMode, + size: number, + obj?: THREE.Mesh, + parentObject?: MirabufSceneObject, + postGizmoCreation?: (gizmo: GizmoSceneObject) => void + ) { + super() + + this._obj = obj ?? new THREE.Mesh() + this._parentObject = parentObject + this._mainCamera = World.SceneRenderer.mainCamera + + this._size = size + + this._gizmo = new TransformControls(World.SceneRenderer.mainCamera, World.SceneRenderer.renderer.domElement) + this._gizmo.setMode(mode) + + World.SceneRenderer.RegisterGizmoSceneObject(this) + + postGizmoCreation?.(this) + + if (this._parentObject) { + this._relativeTransformations = new Map() + const gizmoTransformInv = this._obj.matrix.clone().invert() + + /** Due to the limited math functionality exposed to JS for Jolt, we need everything in ThreeJS. */ + this._parentObject.mirabufInstance.parser.rigidNodes.forEach(rn => { + const jBodyId = this._parentObject!.mechanism.GetBodyByNodeId(rn.id) + if (!jBodyId) return + + const worldTransform = JoltMat44_ThreeMatrix4(World.PhysicsSystem.GetBody(jBodyId).GetWorldTransform()) + const relativeTransform = worldTransform.premultiply(gizmoTransformInv) + this._relativeTransformations!.set(rn.id, relativeTransform) + }) + } + } + + public Setup(): void { + // adding the mesh and gizmo to the scene + World.SceneRenderer.AddObject(this._obj) + World.SceneRenderer.AddObject(this._gizmo) + + // forcing the gizmo to rotate and transform with the object + this._gizmo.setSpace("local") + this._gizmo.attach(this._obj) + + this._gizmo.addEventListener("dragging-changed", (event: { target: TransformControls; value: unknown }) => { + // disable orbit controls when dragging the transform gizmo + const gizmoDragging = World.SceneRenderer.IsAnyGizmoDragging() + World.SceneRenderer.orbitControls.enabled = !event.value && !gizmoDragging + + const isShift = InputSystem.isKeyPressed("ShiftRight") || InputSystem.isKeyPressed("ShiftLeft") + const isAlt = InputSystem.isKeyPressed("AltRight") || InputSystem.isKeyPressed("AltLeft") + + switch (event.target.mode) { + case "translate": { + // snap if alt is pressed + event.target.translationSnap = isAlt ? 0.1 : null + + // disable other gizmos when translating + const gizmos = [...World.SceneRenderer.gizmosOnMirabuf.values()] + gizmos.forEach(obj => { + if (obj.gizmo.object === event.target.object && obj.gizmo.mode !== "translate") { + obj.gizmo.dragging = false + obj.gizmo.enabled = !event.value + return + } + }) + break + } + case "rotate": { + // snap if alt is pressed + event.target.rotationSnap = isAlt ? Math.PI * (1.0 / 12.0) : null + + // disable scale gizmos added to the same object + const gizmos = [...World.SceneRenderer.gizmosOnMirabuf.values()] + gizmos.forEach(obj => { + if ( + obj.gizmo.mode === "scale" && + event.target !== obj.gizmo && + obj.gizmo.object === event.target.object + ) { + obj.gizmo.dragging = false + obj.gizmo.enabled = !event.value + return + } + }) + break + } + case "scale": { + // snap if alt is pressed + event.target.setScaleSnap(isAlt ? 0.1 : null) + + // scale uniformly if shift is pressed + event.target.axis = isShift ? "XYZE" : null + break + } + default: { + console.error("Invalid gizmo state") + break + } + } + }) + } + + public Update(): void { + this._gizmo.updateMatrixWorld() + + if (!this.gizmo.object) { + console.error("No object added to gizmo") + return + } + + // updating the size of the gizmo based on the distance from the camera + const mainCameraFovRadians = (Math.PI * (this._mainCamera.fov * 0.5)) / 180 + this._gizmo.setSize( + (this._size / this._mainCamera.position.distanceTo(this.gizmo.object!.position)) * + Math.tan(mainCameraFovRadians) * + 1.9 + ) + + /** Translating the obj changes to the mirabuf scene object */ + if (this._parentObject) { + this._parentObject.DisablePhysics() + if (this.isDragging || this._forceUpdate) { + this._forceUpdate = false + this._parentObject.mirabufInstance.parser.rigidNodes.forEach(rn => { + this.UpdateNodeTransform(rn.id) + }) + this._parentObject.UpdateMeshTransforms() + } + } + } + + public Dispose(): void { + this._gizmo.detach() + this._parentObject?.EnablePhysics() + World.SceneRenderer.RemoveObject(this._obj) + World.SceneRenderer.RemoveObject(this._gizmo) + + this._relativeTransformations?.clear() + } + + /** changes the mode of the gizmo */ + public SetMode(mode: GizmoMode) { + this._gizmo.setMode(mode) + } + + /** + * Updates a given node to follow the gizmo. + * + * @param rnId Target node to update. + */ + public UpdateNodeTransform(rnId: RigidNodeId) { + if (!this._parentObject || !this._relativeTransformations || !this._relativeTransformations.has(rnId)) return + + const jBodyId = this._parentObject.mechanism.GetBodyByNodeId(rnId) + if (!jBodyId) return + + const relativeTransform = this._relativeTransformations.get(rnId)! + const worldTransform = relativeTransform.clone().premultiply(this._obj.matrix) + const position = new THREE.Vector3(0, 0, 0) + const rotation = new THREE.Quaternion(0, 0, 0, 1) + worldTransform.decompose(position, rotation, new THREE.Vector3(1, 1, 1)) + + World.PhysicsSystem.SetBodyPositionAndRotation( + jBodyId, + ThreeVector3_JoltVec3(position), + ThreeQuaternion_JoltQuat(rotation) + ) + } + + /** + * Updates the gizmos location. + * + * @param gizmoTransformation Transform for the gizmo to take on. + */ + public SetTransform(gizmoTransformation: THREE.Matrix4) { + // Super hacky, prolly has something to do with how the transform controls update the attached object. + const position = new THREE.Vector3(0, 0, 0) + const rotation = new THREE.Quaternion(0, 0, 0, 1) + const scale = new THREE.Vector3(1, 1, 1) + gizmoTransformation.decompose(position, rotation, scale) + this._obj.matrix.compose(position, rotation, scale) + + this._obj.position.setFromMatrixPosition(gizmoTransformation) + this._obj.rotation.setFromRotationMatrix(gizmoTransformation) + + this._forceUpdate = true + } + + public SetRotation(rotation: THREE.Quaternion) { + const position = new THREE.Vector3(0, 0, 0) + const scale = new THREE.Vector3(1, 1, 1) + this._obj.matrix.decompose(position, new THREE.Quaternion(0, 0, 0, 1), scale) + this._obj.matrix.compose(position, rotation, scale) + + this._obj.rotation.setFromQuaternion(rotation) + + this._forceUpdate = true + } + + /** @return true if gizmo is attached to mirabufSceneObject */ + public HasParent(): boolean { + return this._parentObject !== undefined + } +} + +export default GizmoSceneObject diff --git a/fission/src/systems/scene/SceneRenderer.ts b/fission/src/systems/scene/SceneRenderer.ts index 205aa4434d..716c048fa1 100644 --- a/fission/src/systems/scene/SceneRenderer.ts +++ b/fission/src/systems/scene/SceneRenderer.ts @@ -1,20 +1,20 @@ import * as THREE from "three" -import SceneObject from "./SceneObject" import WorldSystem from "../WorldSystem" +import SceneObject from "./SceneObject" +import GizmoSceneObject from "./GizmoSceneObject" -import { TransformControls } from "three/examples/jsm/controls/TransformControls.js" -import { OrbitControls } from "three/examples/jsm/controls/OrbitControls.js" import { EdgeDetectionMode, EffectComposer, EffectPass, RenderPass, SMAAEffect } from "postprocessing" +import { OrbitControls } from "three/examples/jsm/controls/OrbitControls.js" -import vertexShader from "@/shaders/vertex.glsl" import fragmentShader from "@/shaders/fragment.glsl" +import vertexShader from "@/shaders/vertex.glsl" import { Theme } from "@/ui/ThemeContext" -import InputSystem from "../input/InputSystem" import Jolt from "@barclah/jolt-physics" import { PixelSpaceCoord, SceneOverlayEvent, SceneOverlayEventKey } from "@/ui/components/SceneOverlayEvents" import PreferencesSystem from "../preferences/PreferencesSystem" import { CSM } from "three/examples/jsm/csm/CSM.js" +import MirabufSceneObject from "@/mirabuf/MirabufSceneObject" const CLEAR_COLOR = 0x121212 const GROUND_COLOR = 0x4066c7 @@ -31,9 +31,9 @@ class SceneRenderer extends WorldSystem { private _antiAliasPass: EffectPass private _sceneObjects: Map + private _gizmosOnMirabuf: Map // maps of all the gizmos that are attached to a mirabuf scene object private _orbitControls: OrbitControls - private _transformControls: Map // maps all rendered transform controls to their size private _light: THREE.DirectionalLight | CSM | undefined @@ -53,11 +53,22 @@ class SceneRenderer extends WorldSystem { return this._renderer } + public get orbitControls() { + return this._orbitControls + } + + /** + * Collection that maps Mirabuf objects to active GizmoSceneObjects + */ + public get gizmosOnMirabuf() { + return this._gizmosOnMirabuf + } + public constructor() { super() this._sceneObjects = new Map() - this._transformControls = new Map() + this._gizmosOnMirabuf = new Map() this._mainCamera = new THREE.PerspectiveCamera(75, window.innerWidth / window.innerHeight, 0.1, 1000) this._mainCamera.position.set(-2.5, 2, 2.5) @@ -84,7 +95,7 @@ class SceneRenderer extends WorldSystem { this._scene.add(ambientLight) const ground = new THREE.Mesh(new THREE.BoxGeometry(10, 1, 10), this.CreateToonMaterial(GROUND_COLOR)) - ground.position.set(0.0, -2.0, 0.0) + ground.position.set(0.0, -0.5, 0.0) ground.receiveShadow = true ground.castShadow = true this._scene.add(ground) @@ -139,15 +150,6 @@ class SceneRenderer extends WorldSystem { this._skybox.position.copy(this._mainCamera.position) - const mainCameraFovRadians = (Math.PI * (this._mainCamera.fov * 0.5)) / 180 - this._transformControls.forEach((size, tc) => { - tc.setSize( - (size / this._mainCamera.position.distanceTo(tc.object!.position)) * - Math.tan(mainCameraFovRadians) * - 1.9 - ) - }) - // Update the tags each frame if they are enabled in preferences if (PreferencesSystem.getGlobalPreference("RenderSceneTags")) new SceneOverlayEvent(SceneOverlayEventKey.UPDATE) @@ -236,13 +238,29 @@ class SceneRenderer extends WorldSystem { return id } + /** Registers gizmos that are attached to a parent mirabufsceneobject */ + public RegisterGizmoSceneObject(obj: GizmoSceneObject): number { + if (obj.HasParent()) this._gizmosOnMirabuf.set(obj.parentObjectId!, obj) + return this.RegisterSceneObject(obj) + } + public RemoveAllSceneObjects() { this._sceneObjects.forEach(obj => obj.Dispose()) + this._gizmosOnMirabuf.clear() this._sceneObjects.clear() } public RemoveSceneObject(id: number) { const obj = this._sceneObjects.get(id) + + // If the object is a mirabuf object, remove the gizmo as well + if (obj instanceof MirabufSceneObject) { + const objGizmo = this._gizmosOnMirabuf.get(id) + if (this._gizmosOnMirabuf.delete(id)) objGizmo!.Dispose() + } else if (obj instanceof GizmoSceneObject && obj.HasParent()) { + this._gizmosOnMirabuf.delete(obj.parentObjectId!) + } + if (this._sceneObjects.delete(id)) { obj!.Dispose() } @@ -328,83 +346,9 @@ class SceneRenderer extends WorldSystem { } } - /** - * Attach new transform gizmo to Mesh - * - * @param obj Mesh to attach gizmo to - * @param mode Transform mode (translate, rotate, scale) - * @param size Size of the gizmo - * @returns void - */ - public AddTransformGizmo(obj: THREE.Object3D, mode: "translate" | "rotate" | "scale" = "translate", size: number) { - const transformControl = new TransformControls(this._mainCamera, this._renderer.domElement) - transformControl.setMode(mode) - transformControl.attach(obj) - - // allowing the transform gizmos to rotate with the object - transformControl.space = "local" - - transformControl.addEventListener( - "dragging-changed", - (event: { target: TransformControls; value: unknown }) => { - const isAnyGizmoDragging = Array.from(this._transformControls.keys()).some(gizmo => gizmo.dragging) - if (!event.value && !isAnyGizmoDragging) { - this._orbitControls.enabled = true // enable orbit controls when not dragging another transform gizmo - } else if (!event.value && isAnyGizmoDragging) { - this._orbitControls.enabled = false // disable orbit controls when dragging another transform gizmo - } else { - this._orbitControls.enabled = !event.value // disable orbit controls when dragging transform gizmo - } - - if (event.target.mode === "translate") { - this._transformControls.forEach((_size, tc) => { - // disable other transform gizmos when translating - if (tc.object === event.target.object && tc.mode !== "translate") { - tc.dragging = false - tc.enabled = !event.value - return - } - }) - } else if ( - event.target.mode === "scale" && - (InputSystem.isKeyPressed("ShiftRight") || InputSystem.isKeyPressed("ShiftLeft")) - ) { - // scale uniformly if shift is pressed - transformControl.axis = "XYZE" - } else if (event.target.mode === "rotate") { - // scale on all axes - this._transformControls.forEach((_size, tc) => { - // disable scale transform gizmo when scaling - if (tc.mode === "scale" && tc !== event.target && tc.object === event.target.object) { - tc.dragging = false - tc.enabled = !event.value - return - } - }) - } - } - ) - - this._transformControls.set(transformControl, size) - this._scene.add(transformControl) - - return transformControl - } - - /** - * Remove transform gizmos from Mesh - * - * @param obj Mesh to remove gizmo from - * @returns void - */ - public RemoveTransformGizmos(obj: THREE.Object3D) { - this._transformControls.forEach((_, tc) => { - if (tc.object === obj) { - tc.detach() - this._scene.remove(tc) - this._transformControls.delete(tc) - } - }) + /** returns whether any gizmos are being currently dragged */ + public IsAnyGizmoDragging(): boolean { + return [...this._gizmosOnMirabuf.values()].some(obj => obj.gizmo.dragging) } /** diff --git a/fission/src/systems/simulation/synthesis_brain/SynthesisBrain.ts b/fission/src/systems/simulation/synthesis_brain/SynthesisBrain.ts index a58c1bdb3a..c98225cf58 100644 --- a/fission/src/systems/simulation/synthesis_brain/SynthesisBrain.ts +++ b/fission/src/systems/simulation/synthesis_brain/SynthesisBrain.ts @@ -17,6 +17,7 @@ import GenericElevatorBehavior from "../behavior/synthesis/GenericElevatorBehavi import PreferencesSystem from "@/systems/preferences/PreferencesSystem" import { DefaultSequentialConfig } from "@/systems/preferences/PreferenceTypes" import InputSystem from "@/systems/input/InputSystem" +import MirabufSceneObject from "@/mirabuf/MirabufSceneObject" class SynthesisBrain extends Brain { public static brainIndexMap = new Map() @@ -63,6 +64,7 @@ class SynthesisBrain extends Brain { this._simLayer = World.SimulationSystem.GetSimulationLayer(mechanism)! this._assemblyName = assemblyName + // I'm not fixing this right now, but this is going to become an issue... this._brainIndex = SynthesisBrain.brainIndexMap.size SynthesisBrain.brainIndexMap.set(this._brainIndex, this) @@ -220,6 +222,10 @@ class SynthesisBrain extends Brain { /** Put any field configuration here */ } + + public static GetBrainIndex(assembly: MirabufSceneObject | undefined): number | undefined { + return (assembly?.brain as SynthesisBrain)?.brainIndex + } } export default SynthesisBrain diff --git a/fission/src/ui/components/GlobalUIComponent.tsx b/fission/src/ui/components/GlobalUIComponent.tsx new file mode 100644 index 0000000000..6f38fb8723 --- /dev/null +++ b/fission/src/ui/components/GlobalUIComponent.tsx @@ -0,0 +1,48 @@ +import { useModalControlContext } from "@/ui/ModalContext" +import { usePanelControlContext } from "@/ui/PanelContext" +import { useToastContext } from "@/ui/ToastContext" +import { useEffect } from "react" +import { setAddToast, setOpenModal, setOpenPanel } from "./GlobalUIControls" + +/** + * So this is Hunter's kinda cursed approach to de-react-ifying some of our UI controls. + * Essentially, this component will expose context controls for our UI, which allows + * non-UI components (such as APSDataManagement) to use UI controls (such as addToast). + * + * Stored in a component to ensure a lifetime is followed with this handles. + * + * @returns Global UI Component + */ +function GlobalUIComponent() { + const { openModal } = useModalControlContext() + const { openPanel } = usePanelControlContext() + const { addToast } = useToastContext() + + useEffect(() => { + setOpenModal(openModal) + + return () => { + setOpenModal(undefined) + } + }, [openModal]) + + useEffect(() => { + setOpenPanel(openPanel) + + return () => { + setOpenPanel(undefined) + } + }, [openPanel]) + + useEffect(() => { + setAddToast(addToast) + + return () => { + setAddToast(undefined) + } + }, [addToast]) + + return <> +} + +export default GlobalUIComponent diff --git a/fission/src/ui/components/GlobalUIControls.ts b/fission/src/ui/components/GlobalUIControls.ts new file mode 100644 index 0000000000..e3a459ac9a --- /dev/null +++ b/fission/src/ui/components/GlobalUIControls.ts @@ -0,0 +1,20 @@ +/** + * This is where all the global references to the Global UI controls are located. + * See GlobalUIComponent.tsx for explanation of this madness. + */ + +import { ToastType } from "@/ui/ToastContext" + +export let Global_AddToast: ((type: ToastType, title: string, description: string) => void) | undefined = undefined +export let Global_OpenPanel: ((panelId: string) => void) | undefined = undefined +export let Global_OpenModal: ((modalId: string) => void) | undefined = undefined + +export function setAddToast(func: typeof Global_AddToast) { + Global_AddToast = func +} +export function setOpenPanel(func: typeof Global_OpenPanel) { + Global_OpenPanel = func +} +export function setOpenModal(func: typeof Global_OpenModal) { + Global_OpenModal = func +} diff --git a/fission/src/ui/components/MainHUD.tsx b/fission/src/ui/components/MainHUD.tsx index f7aca002f2..1c015873ed 100644 --- a/fission/src/ui/components/MainHUD.tsx +++ b/fission/src/ui/components/MainHUD.tsx @@ -4,12 +4,13 @@ import { useModalControlContext } from "@/ui/ModalContext" import { usePanelControlContext } from "@/ui/PanelContext" import { motion } from "framer-motion" import logo from "@/assets/autodesk_logo.png" -import { ToastType, useToastContext } from "@/ui/ToastContext" +import { useToastContext } from "@/ui/ToastContext" import APS, { APS_USER_INFO_UPDATE_EVENT } from "@/aps/APS" import { UserIcon } from "./UserIcon" import { ButtonIcon, SynthesisIcons } from "./StyledComponents" import { Button } from "@mui/base" import { Box } from "@mui/material" +import { setAddToast } from "./GlobalUIControls" type ButtonProps = { value: string @@ -44,8 +45,6 @@ const MainHUDButton: React.FC = ({ value, icon, onClick, larger }) ) } -export let MainHUD_AddToast: (type: ToastType, title: string, description: string) => void = (_a, _b, _c) => {} - const variants = { open: { opacity: 1, y: "-50%", x: 0 }, closed: { opacity: 0, y: "-50%", x: "-100%" }, @@ -57,7 +56,7 @@ const MainHUD: React.FC = () => { const { addToast } = useToastContext() const [isOpen, setIsOpen] = useState(false) - MainHUD_AddToast = addToast + setAddToast(addToast) const [userInfo, setUserInfo] = useState(APS.userInfo) diff --git a/fission/src/ui/components/Scene.tsx b/fission/src/ui/components/Scene.tsx index 6ba35ddb6f..034d865503 100644 --- a/fission/src/ui/components/Scene.tsx +++ b/fission/src/ui/components/Scene.tsx @@ -17,8 +17,6 @@ function Scene({ useStats }: SceneProps) { World.InitWorld() if (refContainer.current) { - console.debug("Adding ThreeJs to DOM") - const sr = World.SceneRenderer sr.renderer.domElement.style.width = "100%" sr.renderer.domElement.style.height = "100%" @@ -30,7 +28,6 @@ function Scene({ useStats }: SceneProps) { }) if (useStats && !stats) { - console.log("Adding stat") stats = new Stats() stats.dom.style.position = "absolute" stats.dom.style.top = "0px" diff --git a/fission/src/ui/components/TransformGizmoControl.tsx b/fission/src/ui/components/TransformGizmoControl.tsx new file mode 100644 index 0000000000..3dd5a2d9f7 --- /dev/null +++ b/fission/src/ui/components/TransformGizmoControl.tsx @@ -0,0 +1,138 @@ +import { useEffect, useState } from "react" +import TransformGizmoControlProps from "./TransformGizmoControlProps" +import GizmoSceneObject, { GizmoMode } from "@/systems/scene/GizmoSceneObject" +import { ToggleButton, ToggleButtonGroup } from "./ToggleButtonGroup" +import World from "@/systems/World" +import Button, { ButtonSize } from "./Button" +import InputSystem from "@/systems/input/InputSystem" +import * as THREE from "three" + +/** + * Creates GizmoSceneObject and gives you a toggle button group to control the modes of the gizmo. + * The lifetime of the gizmo is entirely handles within this component and will be recreated depending + * on the updates made to the parameters provided. You can setup initial properties of the gizmo with + * the `postGizmoCreation` handle. + * + * @param param0 Transform Gizmo Controls. + * @returns TransformGizmoControl component. + */ +function TransformGizmoControl({ + defaultMesh, + gizmoRef, + size, + parent, + defaultMode, + translateDisabled, + rotateDisabled, + scaleDisabled, + sx, + postGizmoCreation, + onAccept, + onCancel, +}: TransformGizmoControlProps) { + const [mode, setMode] = useState(defaultMode) + const [gizmo, setGizmo] = useState(undefined) + + useEffect(() => { + const gizmo = new GizmoSceneObject("translate", size, defaultMesh, parent, (gizmo: GizmoSceneObject) => { + parent?.PostGizmoCreation(gizmo) + postGizmoCreation?.(gizmo) + }) + + if (gizmoRef) gizmoRef.current = gizmo + + setGizmo(gizmo) + + return () => { + World.SceneRenderer.RemoveSceneObject(gizmo.id) + } + }, [gizmoRef, defaultMesh, size, parent, postGizmoCreation]) + + useEffect(() => { + return () => { + if (gizmoRef) gizmoRef.current = undefined + } + }, [gizmoRef]) + + const disableOptions = 2 <= (translateDisabled ? 1 : 0) + (rotateDisabled ? 1 : 0) + (scaleDisabled ? 1 : 0) + + const buttons = [] + if (!translateDisabled) + buttons.push( + + Move + + ) + if (!rotateDisabled) + buttons.push( + + Rotate + + ) + if (!scaleDisabled) + buttons.push( + + Scale + + ) + + useEffect(() => { + const func = () => { + // creating enter key and escape key event listeners + if (InputSystem.isKeyPressed("Enter")) { + onAccept?.(gizmo) + } else if (InputSystem.isKeyPressed("Escape")) { + onCancel?.(gizmo) + } + + cancelAnimationFrame(animHandle) + animHandle = requestAnimationFrame(func) + } + let animHandle = requestAnimationFrame(func) + + return () => { + cancelAnimationFrame(animHandle) + } + }, [gizmo, onAccept, onCancel]) + + // If there are no modes enabled, consider the UI pointless. + return disableOptions ? ( + <> + ) : ( + <> + { + if (v == undefined) return + + setMode(v) + gizmo?.SetMode(v) + }} + sx={{ + ...(sx ?? {}), + alignSelf: "center", + }} + > + {/* { translateDisabled ? <> : Move } + { rotateDisabled ? <> : Rotate } + { scaleDisabled ? <> : Scale } */} + {buttons} + + {rotateDisabled ? ( + <> + ) : ( +