From 7875e39513c548ac216990a24616b620e898e0e1 Mon Sep 17 00:00:00 2001 From: Ryan Zhang Date: Tue, 22 Jul 2025 14:06:59 -0700 Subject: [PATCH 1/9] fix: z-axis bug --- 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 34296c3ea6..2b763bb8fc 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -114,7 +114,7 @@ export class SimAccelInput extends SimInput { const x = (newVel.x - this._prevVel.x) / deltaT const y = (newVel.y - this._prevVel.y) / deltaT - const z = (newVel.y - this._prevVel.y) / deltaT + const z = (newVel.z - this._prevVel.z) / deltaT SimAccel.setX(this._device, x) SimAccel.setY(this._device, y) From 77f21a734081ef086c8d4182c3b59b3d56a5fa1c Mon Sep 17 00:00:00 2001 From: Ryan Zhang Date: Wed, 23 Jul 2025 14:27:59 -0700 Subject: [PATCH 2/9] Add accelerometer implementation --- .../java/com/autodesk/synthesis/Accel.java | 80 +++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/Accel.java diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/Accel.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/Accel.java new file mode 100644 index 0000000000..ab57af2c6b --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/Accel.java @@ -0,0 +1,80 @@ +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; + +/** + * Accelerometer 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 Accel { + private SimDevice m_device; + private SimDouble m_range; + private SimBoolean m_connected; + private SimDouble m_x, m_y, m_z; + + /** + * Creates a CANMotor sim device in accordance with the WebSocket API Specification. + * + * @param name Name of the Accel. This is generally the class name of the originating gyro (i.e. "ADXRS450"). + * @param deviceId ID of the Gyro. + */ + public Accel(String name, int deviceId) { + m_device = SimDevice.create("Accel:" + name, deviceId); + + m_range = m_device.createDouble("range", Direction.kOutput, 0.0); + m_connected = m_device.createBoolean("connected", Direction.kOutput, false); + m_x = m_device.createDouble("x", Direction.kInput, 0); + m_y = m_device.createDouble("y", Direction.kInput, 0); + m_z = m_device.createDouble("z", Direction.kInput, 0); + } + + /** + * Set the range of the accel. + * + * @param range Range of the accel + */ + public void setRange(double range) { + if (Double.isNaN(range) || Double.isInfinite(range)) { + range = 0.0; + } + + m_range.set(range); + } + + public void setConnected(boolean connected) { + m_connected.set(connected); + } + + /** + * Get the x position of the accel. + * + * @return x + */ + public double getX() { + return m_x.get(); + } + + /** + * Get the y position of the accel. + * + * @return y + */ + public double getY() { + return m_y.get(); + } + + /** + * Get the z position of the accel. + * + * @return z + */ + public double getZ() { + return m_z.get(); + } + +} From 2b39aa7391f87e77cb6b1e0a33acca5970d8cb49 Mon Sep 17 00:00:00 2001 From: Ryan Zhang Date: Wed, 23 Jul 2025 14:28:18 -0700 Subject: [PATCH 3/9] Add ADXL362 accelerometer --- .../autodesk/synthesis/wpilibj/ADXL362.java | 81 +++++++++++++++++++ 1 file changed, 81 insertions(+) create mode 100644 simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/wpilibj/ADXL362.java diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/wpilibj/ADXL362.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/wpilibj/ADXL362.java new file mode 100644 index 0000000000..5477f7ab6b --- /dev/null +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/wpilibj/ADXL362.java @@ -0,0 +1,81 @@ +package com.autodesk.synthesis.wpilibj; + +import com.autodesk.synthesis.Accel; + +import edu.wpi.first.wpilibj.SPI; + +/** + * ADXL362 wrapper for simulation support. + * Extends the original WPILib ADXL362 accelerometer class to provide + * simulated accelerometer data during simulation. + */ +public class ADXL362 extends edu.wpi.first.wpilibj.ADXL362 { + private Accel m_Accel; + + /** + * Constructor for ADXL362 accelerometer. + * + * @param port SPI port the accelerometer is connected to + * @param range The range of the accelerometer + */ + public ADXL362(SPI.Port port, Range range) { + super(port, range); + init("SPI", port.value, range); + } + + /** + * Constructor with default range. + * + * @param port SPI port the accelerometer is connected to + */ + public ADXL362(SPI.Port port) { + this(port, Range.k2G); + } + + private void init(String commType, int port, Range range) { + this.m_Accel = new Accel("Accel: " + commType, port); + this.m_Accel.setConnected(true); + + double rangeValue; + switch (range) { + case k2G: + rangeValue = 2.0; + case k4G: + rangeValue = 4.0; + case k8G: + rangeValue = 8.0; + } + + this.m_Accel.setRange(rangeValue); + } + + /** + * Get the acceleration in the X-axis. + * + * @return X-axis acceleration in g-forces + */ + @Override + public double getX() { + return m_Accel.getX(); + } + + /** + * Get the acceleration in the Y-axis. + * + * @return Y-axis acceleration in g-forces + */ + @Override + public double getY() { + return m_Accel.getY(); + } + + /** + * Get the acceleration in the Z-axis. + * + * @return Z-axis acceleration in g-forces + */ + @Override + public double getZ() { + return m_Accel.getZ(); + } +} From 33820b6cb151fb54a485b3325048b9d91030b3d0 Mon Sep 17 00:00:00 2001 From: Ryan Zhang Date: Wed, 23 Jul 2025 14:29:40 -0700 Subject: [PATCH 4/9] Update accelerometer implementation --- fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts | 2 +- simulation/SyntheSimJava/build.gradle | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts index 7df056ced4..ff570cd34d 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -577,7 +577,7 @@ class WPILibBrain extends Brain { } this.addSimInput(new SimGyroInput("Test Gyro[1]", this._mechanism)) - this.addSimInput(new SimAccelInput("ADXL362[4]", this._mechanism)) + this.addSimInput(new SimAccelInput("Test Accel[4]", this._mechanism)) this.addSimInput(new SimDigitalInput("SYN DI[0]", () => random() > 0.5)) this.addSimOutput(new SimDigitalOutput("SYN DO[1]")) this.addSimInput(new SimAnalogInput("SYN AI[0]", () => random() * 12)) diff --git a/simulation/SyntheSimJava/build.gradle b/simulation/SyntheSimJava/build.gradle index e8631a287e..c1c5b71838 100644 --- a/simulation/SyntheSimJava/build.gradle +++ b/simulation/SyntheSimJava/build.gradle @@ -54,6 +54,9 @@ dependencies { implementation "edu.wpi.first.wpiutil:wpiutil-java:$WPI_Version" implementation "edu.wpi.first.hal:hal-java:$WPI_Version" + // NetworkTables + implementation "edu.wpi.first.ntcore:ntcore-java:$WPI_Version" + // REVRobotics implementation "com.revrobotics.frc:REVLib-java:$REV_Version" From 9beef152f0a9483069918e21a485b60640a686ca Mon Sep 17 00:00:00 2001 From: Ryan Zhang Date: Wed, 23 Jul 2025 14:32:07 -0700 Subject: [PATCH 5/9] Update java sim example imports --- .../samples/JavaAutoSample/src/main/java/frc/robot/Robot.java | 4 ++-- .../samples/JavaSample/src/main/java/frc/robot/Robot.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/simulation/samples/JavaAutoSample/src/main/java/frc/robot/Robot.java b/simulation/samples/JavaAutoSample/src/main/java/frc/robot/Robot.java index 9452a56343..0eb9932d8e 100644 --- a/simulation/samples/JavaAutoSample/src/main/java/frc/robot/Robot.java +++ b/simulation/samples/JavaAutoSample/src/main/java/frc/robot/Robot.java @@ -10,7 +10,6 @@ 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; @@ -21,6 +20,7 @@ import com.autodesk.synthesis.revrobotics.RelativeEncoder; import com.autodesk.synthesis.revrobotics.SparkAbsoluteEncoder; import com.kauailabs.navx.frc.AHRS; +import com.autodesk.synthesis.wpilibj.ADXL362; import com.autodesk.synthesis.CANEncoder; import com.autodesk.synthesis.ctre.TalonFX; @@ -39,7 +39,7 @@ public class Robot extends TimedRobot { private String m_autoSelected; private final SendableChooser m_chooser = new SendableChooser<>(); - private ADXL362 m_Accelerometer = new ADXL362(SPI.Port.kMXP, ADXL362.Range.k8G); + private ADXL362 m_Accel = new ADXL362(SPI.Port.kMXP, ADXL362.Range.k8G); private AHRS m_Gyro = new AHRS(); private CANSparkMax m_sparkLeft = new CANSparkMax(1, MotorType.kBrushless); 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 f1b05386fc..a009ef8806 100644 --- a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java +++ b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java @@ -10,7 +10,6 @@ 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; @@ -20,6 +19,7 @@ import com.autodesk.synthesis.revrobotics.CANSparkMax; import com.kauailabs.navx.frc.AHRS; import com.autodesk.synthesis.ctre.TalonFX; +import com.autodesk.synthesis.wpilibj.ADXL362; /** * The VM is configured to automatically run this class, and to call the @@ -41,7 +41,7 @@ public class Robot extends TimedRobot { 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 ADXL362 m_Accel = new ADXL362(SPI.Port.kMXP, ADXL362.Range.k8G); private AHRS m_Gyro = new AHRS(); private DigitalInput m_DI = new DigitalInput(0); From 08dd270397241074a9385ddf7c2004fa74baf26f Mon Sep 17 00:00:00 2001 From: Ryan Zhang Date: Wed, 23 Jul 2025 14:32:55 -0700 Subject: [PATCH 6/9] Update rangeValue switch case --- .../main/java/com/autodesk/synthesis/wpilibj/ADXL362.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/wpilibj/ADXL362.java b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/wpilibj/ADXL362.java index 5477f7ab6b..a1f2b4e086 100644 --- a/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/wpilibj/ADXL362.java +++ b/simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/wpilibj/ADXL362.java @@ -40,10 +40,16 @@ private void init(String commType, int port, Range range) { switch (range) { case k2G: rangeValue = 2.0; + break; case k4G: rangeValue = 4.0; + break; case k8G: rangeValue = 8.0; + break; + default: + rangeValue = 2.0; + break; } this.m_Accel.setRange(rangeValue); From 30d5ba268997372509a484e3242a14e05fd4066d Mon Sep 17 00:00:00 2001 From: Ryan Zhang Date: Wed, 6 Aug 2025 10:23:29 -0700 Subject: [PATCH 7/9] Update imports --- .../src/systems/simulation/wpilib_brain/SimInput.ts | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index 4259c09a1d..724a7e744d 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -1,3 +1,16 @@ +import World from "@/systems/World" +import EncoderStimulus from "../stimulus/EncoderStimulus" +import SimCANEncoder from "./sim/SimCANEncoder" +import SimGyro from "./sim/SimGyro" +import SimAccel from "./sim/SimAccel" +import SimDIO from "./sim/SimDIO" +import SimAI from "./sim/SimAI" +import Mechanism from "@/systems/physics/Mechanism" +import Jolt from "@azaleacolburn/jolt-physics" +import JOLT from "@/util/loading/JoltSyncLoader" +import { convertJoltQuatToThreeQuaternion, convertJoltVec3ToThreeVector3 } from "@/util/TypeConversions" +import * as THREE from "three" + export abstract class SimInput { constructor(protected _device: string) {} From e8b21dc9acf4624ae40ad7d3f61fe49ab2e3e50c Mon Sep 17 00:00:00 2001 From: Ryan Zhang Date: Wed, 6 Aug 2025 10:24:25 -0700 Subject: [PATCH 8/9] chore: biome --- .../simulation/wpilib_brain/SimInput.ts | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index 724a7e744d..e57de6582c 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -1,15 +1,15 @@ +import type Jolt from "@azaleacolburn/jolt-physics" +import * as THREE from "three" +import type Mechanism from "@/systems/physics/Mechanism" import World from "@/systems/World" -import EncoderStimulus from "../stimulus/EncoderStimulus" -import SimCANEncoder from "./sim/SimCANEncoder" -import SimGyro from "./sim/SimGyro" -import SimAccel from "./sim/SimAccel" -import SimDIO from "./sim/SimDIO" -import SimAI from "./sim/SimAI" -import Mechanism from "@/systems/physics/Mechanism" -import Jolt from "@azaleacolburn/jolt-physics" import JOLT from "@/util/loading/JoltSyncLoader" import { convertJoltQuatToThreeQuaternion, convertJoltVec3ToThreeVector3 } from "@/util/TypeConversions" -import * as THREE from "three" +import type EncoderStimulus from "../stimulus/EncoderStimulus" +import SimAccel from "./sim/SimAccel" +import SimAI from "./sim/SimAI" +import SimCANEncoder from "./sim/SimCANEncoder" +import SimDIO from "./sim/SimDIO" +import SimGyro from "./sim/SimGyro" export abstract class SimInput { constructor(protected _device: string) {} From 3ec6ea040a03ff113922c51779ec9545bf97897a Mon Sep 17 00:00:00 2001 From: Ryan Zhang Date: Fri, 15 Aug 2025 09:56:10 -0700 Subject: [PATCH 9/9] Resolve circular dependency issue --- .../simulation/wpilib_brain/SimInput.ts | 162 +----------------- 1 file changed, 1 insertion(+), 161 deletions(-) diff --git a/fission/src/systems/simulation/wpilib_brain/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index e57de6582c..9d37df3661 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -1,16 +1,3 @@ -import type Jolt from "@azaleacolburn/jolt-physics" -import * as THREE from "three" -import type Mechanism from "@/systems/physics/Mechanism" -import World from "@/systems/World" -import JOLT from "@/util/loading/JoltSyncLoader" -import { convertJoltQuatToThreeQuaternion, convertJoltVec3ToThreeVector3 } from "@/util/TypeConversions" -import type EncoderStimulus from "../stimulus/EncoderStimulus" -import SimAccel from "./sim/SimAccel" -import SimAI from "./sim/SimAI" -import SimCANEncoder from "./sim/SimCANEncoder" -import SimDIO from "./sim/SimDIO" -import SimGyro from "./sim/SimGyro" - export abstract class SimInput { constructor(protected _device: string) {} @@ -19,151 +6,4 @@ export abstract class SimInput { public get device(): string { return this._device } -} - -export class SimEncoderInput extends SimInput { - private _stimulus: EncoderStimulus - - constructor(device: string, stimulus: EncoderStimulus) { - super(device) - this._stimulus = stimulus - } - - public update(_deltaT: number) { - SimCANEncoder.setPosition(this._device, this._stimulus.positionValue) - SimCANEncoder.setVelocity(this._device, this._stimulus.velocityValue) - } -} - -export class SimGyroInput extends SimInput { - private _robot: Mechanism - private _joltID?: Jolt.BodyID - private _joltBody?: Jolt.Body - - private static readonly AXIS_X: Jolt.Vec3 = new JOLT.Vec3(1, 0, 0) - private static readonly AXIS_Y: Jolt.Vec3 = new JOLT.Vec3(0, 1, 0) - private static readonly AXIS_Z: Jolt.Vec3 = new JOLT.Vec3(0, 0, 1) - - constructor(device: string, robot: Mechanism) { - super(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: Jolt.Vec3): number { - return ((this._joltBody?.GetRotation().GetRotationAngle(axis) ?? 0) * 180) / Math.PI - } - - private getX(): number { - return this.getAxis(SimGyroInput.AXIS_X) - } - - private getY(): number { - return this.getAxis(SimGyroInput.AXIS_Y) - } - - private getZ(): number { - 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() - const z = this.getZ() - - 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")) - } -} - -export class SimAccelInput extends SimInput { - private _robot: Mechanism - private _joltID?: Jolt.BodyID - 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 THREE.Vector3(0, 0, 0) - } - - public update(deltaT: number) { - if (!this._joltID) return - const body = World.physicsSystem.getBody(this._joltID) - - const rot = convertJoltQuatToThreeQuaternion(body.GetRotation()) - const mat = new THREE.Matrix4().makeRotationFromQuaternion(rot).transpose() - const newVel = convertJoltVec3ToThreeVector3(body.GetLinearVelocity()).applyMatrix4(mat) - - const x = (newVel.x - this._prevVel.x) / deltaT - const y = (newVel.y - this._prevVel.y) / deltaT - const z = (newVel.z - this._prevVel.z) / deltaT - - SimAccel.setX(this._device, x) - SimAccel.setY(this._device, y) - SimAccel.setZ(this._device, z) - - this._prevVel = newVel - } -} - -export class SimDigitalInput extends SimInput { - private _valueSupplier: () => boolean - - /** - * Creates a Simulation Digital Input object. - * - * @param device Device ID - * @param valueSupplier Called each frame and returns what the value should be set to - */ - constructor(device: string, valueSupplier: () => boolean) { - super(device) - this._valueSupplier = valueSupplier - } - - private setValue(value: boolean) { - SimDIO.setValue(this._device, value) - } - - public getValue(): boolean { - return SimDIO.getValue(this._device) - } - - public update(_deltaT: number) { - if (this._valueSupplier) this.setValue(this._valueSupplier()) - } -} - -export class SimAnalogInput extends SimInput { - private _valueSupplier: () => number - - constructor(device: string, valueSupplier: () => number) { - super(device) - this._valueSupplier = valueSupplier - } - - public update(_deltaT: number) { - SimAI.setValue(this._device, this._valueSupplier()) - } -} +} \ No newline at end of file