Skip to content

Commit

Permalink
digital and analog inputs work but can't be configured
Browse files Browse the repository at this point in the history
show joint name in CAN config panel
update robot code to use all the sim things
  • Loading branch information
PepperLola committed Aug 26, 2024
1 parent 9f9948d commit 88c551b
Show file tree
Hide file tree
Showing 9 changed files with 212 additions and 52 deletions.
6 changes: 0 additions & 6 deletions fission/src/Synthesis.tsx
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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<Worker>(() => new WPILibWSWorker())

function Synthesis() {
const { openModal, closeModal, getActiveModalElement } = useModalManager(initialModals)
const { openPanel, closePanel, closeAllPanels, getActivePanelElements } = usePanelManager(initialPanels)
Expand Down Expand Up @@ -93,8 +89,6 @@ function Synthesis() {
setConsentPopupDisable(false)
}

worker.getValue()

let mainLoopHandle = 0
const mainLoop = () => {
mainLoopHandle = requestAnimationFrame(mainLoop)
Expand Down
1 change: 0 additions & 1 deletion fission/src/systems/simulation/wpilib_brain/SimOutput.ts
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
23 changes: 12 additions & 11 deletions fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts
Original file line number Diff line number Diff line change
@@ -1,14 +1,16 @@
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"

import { SimAnalogOutput, SimOutput, SimOutputGroup } from "./SimOutput"

Check warning on line 9 in fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts

View workflow job for this annotation

GitHub Actions / ESLint Format Validation

'SimAnalogOutput' is defined but never used. Allowed unused vars must match /^_/u

Check warning on line 9 in fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts

View workflow job for this annotation

GitHub Actions / ESLint Format Validation

'SimOutputGroup' is defined but never used. Allowed unused vars must match /^_/u
import { SimAccelInput, SimInput, SimDIO as SimDIOIn, SimAnalogInput } from "./SimInput"

Check warning on line 10 in fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts

View workflow job for this annotation

GitHub Actions / ESLint Format Validation

'SimAccelInput' is defined but never used. Allowed unused vars must match /^_/u

Check warning on line 10 in fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts

View workflow job for this annotation

GitHub Actions / ESLint Format Validation

'SimDIOIn' is defined but never used. Allowed unused vars must match /^_/u

Check warning on line 10 in fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts

View workflow job for this annotation

GitHub Actions / ESLint Format Validation

'SimAnalogInput' is defined but never used. Allowed unused vars must match /^_/u

const worker = new WPILibWSWorker()
const worker: Lazy<Worker> = new Lazy<Worker>(() => new WPILibWSWorker())

const PWM_SPEED = "<speed"
const PWM_POSITION = "<position"

Expand Down Expand Up @@ -113,7 +115,7 @@ export class SimGeneric {
selectedData[field] = value
data.set(field, value)

worker.postMessage({
worker.getValue().postMessage({
command: "update",
data: {
type: simType,
Expand Down Expand Up @@ -324,7 +326,7 @@ type WSMessage = {
data: Map<string, number>
}

worker.addEventListener("message", (eventData: MessageEvent) => {
worker.getValue().addEventListener("message", (eventData: MessageEvent) => {
let data: WSMessage | undefined

if (typeof eventData.data == "object") {
Expand All @@ -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)
Expand Down Expand Up @@ -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) {
Expand All @@ -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" })
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,13 +25,11 @@ const RCConfigCANGroupModal: React.FC<ModalPropsImpl> = ({ 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<string, Map<string, number>>()
Expand All @@ -49,14 +47,6 @@ const RCConfigCANGroupModal: React.FC<ModalPropsImpl> = ({ 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")
Expand Down Expand Up @@ -92,7 +82,7 @@ const RCConfigCANGroupModal: React.FC<ModalPropsImpl> = ({ modalId }) => {
{drivers.map((driver, idx) => (
<Checkbox
key={`${driver.constructor.name}-${idx}`}
label={driver.constructor.name}
label={`${driver.constructor.name} ${driver.info?.name && '(' + driver.info!.name + ')'}`}
defaultState={false}
onClick={checked => {
if (checked && !checkedDrivers.includes(driver)) {
Expand Down
Original file line number Diff line number Diff line change
@@ -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);
}
}
Original file line number Diff line number Diff line change
@@ -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();
}
}
Original file line number Diff line number Diff line change
@@ -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();
}
}
Original file line number Diff line number Diff line change
@@ -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);
}
}
Loading

0 comments on commit 88c551b

Please sign in to comment.