From 88c551b7b3bbcd0f95fe9632d52954799577ec42 Mon Sep 17 00:00:00 2001 From: PepperLola Date: Mon, 26 Aug 2024 16:36:37 -0700 Subject: [PATCH] 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 badb33a28..7052b887d 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 a4ce35d7f..4a64f221f 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 a6495ac20..7eaad915e 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 1584ba3d5..075eda2d2 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 000000000..d093a510c --- /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 000000000..46b66b05b --- /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 000000000..e4df20931 --- /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 000000000..ea7667948 --- /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 891e77651..3f85f59bc 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);