Accelerometer Implementation [AARD-1924]

fission/src/systems/simulation/wpilib_brain/SimInput.ts

@@ -1,3 +1,16 @@
+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) {}
 
@@ -7,3 +20,150 @@ export abstract class SimInput {
         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())
+    }
+}

fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts

@@ -93,7 +93,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))

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"
 

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();
+    }
+
+}

simulation/SyntheSimJava/src/main/java/com/autodesk/synthesis/wpilibj/ADXL362.java

@@ -0,0 +1,87 @@
+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;
+                break;
+            case k4G:
+                rangeValue = 4.0;
+                break;
+            case k8G:
+                rangeValue = 8.0;
+                break;
+            default:
+                rangeValue = 2.0;
+                break;
+        }
+
+        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();
+    }
+}

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<String> 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);