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/SimInput.ts b/fission/src/systems/simulation/wpilib_brain/SimInput.ts index 973f6b9c70..b4776f03a3 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimInput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimInput.ts @@ -1,20 +1,27 @@ import World from "@/systems/World" import EncoderStimulus from "../stimulus/EncoderStimulus" -import { SimCANEncoder, SimGyro } 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" +import { JoltQuat_ThreeQuaternion, JoltVec3_ThreeVector3 } from "@/util/TypeConversions" +import * as THREE from "three" -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 +31,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 +41,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) @@ -58,12 +64,102 @@ 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() 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 = JoltQuat_ThreeQuaternion(body.GetRotation()) + const mat = new THREE.Matrix4().makeRotationFromQuaternion(rot).transpose() + const newVel = JoltVec3_ThreeVector3(body.GetLinearVelocity()).applyMatrix4(mat) + + 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 = 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()) } } diff --git a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts index 40fe5494e9..04e9205c7c 100644 --- a/fission/src/systems/simulation/wpilib_brain/SimOutput.ts +++ b/fission/src/systems/simulation/wpilib_brain/SimOutput.ts @@ -2,22 +2,25 @@ 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" - -// 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 -export abstract class SimOutputGroup { - public name: string +import { SimAO, SimCAN, SimDIO, SimPWM, SimType } from "./WPILibBrain" + +export abstract class SimOutput { + constructor(protected _name: string) {} + + public abstract Update(deltaT: number): void + + public get name(): string { + return this._name + } +} + +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 @@ -35,7 +38,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 @@ -59,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(" { @@ -72,3 +74,36 @@ 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) + } + + 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 a00034d1a4..7fa35eddaf 100644 --- a/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts +++ b/fission/src/systems/simulation/wpilib_brain/WPILibBrain.ts @@ -1,14 +1,17 @@ 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 { SimOutputGroup } from "./SimOutput" -import { SimInput } from "./SimInput" +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()) -const worker = new WPILibWSWorker() const PWM_SPEED = " +type DeviceData = Map export const simMap = new Map>() 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) { @@ -83,7 +93,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`) @@ -102,11 +112,11 @@ export class SimGeneric { return false } - const selectedData: { [key: string]: number } = {} + const selectedData: { [key: string]: number | boolean } = {} selectedData[field] = value data.set(field, value) - worker.postMessage({ + worker.getValue().postMessage({ command: "update", data: { type: simType, @@ -136,7 +146,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()) { @@ -204,6 +214,111 @@ export class SimGyro { public static SetAngleZ(device: string, angle: number): boolean { return SimGeneric.Set(SimType.Gyro, device, ">angle_z", angle) } + + public static SetRateX(device: string, rate: number): boolean { + return SimGeneric.Set(SimType.Gyro, device, ">rate_x", rate) + } + + public static SetRateY(device: string, rate: number): boolean { + return SimGeneric.Set(SimType.Gyro, device, ">rate_y", rate) + } + + public static SetRateZ(device: string, rate: number): boolean { + return SimGeneric.Set(SimType.Gyro, device, ">rate_z", rate) + } +} + +export class SimAccel { + private constructor() {} + + public static SetX(device: string, accel: number): boolean { + return SimGeneric.Set(SimType.Accel, device, ">x", accel) + } + + public static SetY(device: string, accel: number): boolean { + return SimGeneric.Set(SimType.Accel, device, ">y", accel) + } + + public static SetZ(device: string, accel: number): boolean { + 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) + } } type WSMessage = { @@ -212,7 +327,7 @@ type WSMessage = { data: Map } -worker.addEventListener("message", (eventData: MessageEvent) => { +worker.getValue().addEventListener("message", (eventData: MessageEvent) => { let data: WSMessage | undefined if (typeof eventData.data == "object") { @@ -226,8 +341,7 @@ worker.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) }) @@ -253,7 +367,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) { @@ -265,9 +379,16 @@ class WPILibBrain extends Brain { console.warn("SimulationLayer is undefined") return } + + this.addSimInput(new SimGyroInput("Test Gyro[1]", mechanism)) + this.addSimInput(new SimAccelInput("ADXL362[4]", 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)) + this.addSimOutput(new SimAnalogOutput("SYN AO[1]")) } - public addSimOutputGroup(device: SimOutputGroup) { + public addSimOutput(device: SimOutput) { this._simOutputs.push(device) } @@ -281,11 +402,11 @@ class WPILibBrain extends Brain { } 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 624e133708..b2a1641d08 100644 --- a/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx +++ b/fission/src/ui/modals/configuring/rio-config/RCConfigCANGroupModal.tsx @@ -25,17 +25,15 @@ 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>() - const devices: [string, Map][] = [...cans.entries()] + const devices: [string, Map][] = [...cans.entries()] .filter(([_, data]) => data.get(" = ({ 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) { - 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/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)} /> = ({ 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 a481ceae71..693db7c25c 100644 --- a/fission/src/ui/panels/WSViewPanel.tsx +++ b/fission/src/ui/panels/WSViewPanel.tsx @@ -1,5 +1,5 @@ import Panel, { PanelPropsImpl } from "@/components/Panel" -import { SimMapUpdateEvent, SimGeneric, simMap, SimType } from "@/systems/simulation/wpilib_brain/WPILibBrain" +import { SimGeneric, simMap, SimType } from "@/systems/simulation/wpilib_brain/WPILibBrain" import { Box, Stack, @@ -12,12 +12,14 @@ import { TableRow, Typography, } from "@mui/material" -import { useCallback, useEffect, useMemo, useState } from "react" +import { useEffect, useMemo, useReducer, useState } from "react" import Dropdown from "../components/Dropdown" import Input from "../components/Input" import Button from "../components/Button" import { SynthesisIcons } from "../components/StyledComponents" +const TABLE_UPDATE_INTERVAL = 250 + type ValueType = "string" | "number" | "object" | "boolean" const TypoStyled = styled(Typography)({ @@ -26,7 +28,7 @@ const TypoStyled = styled(Typography)({ color: "white", }) -function formatMap(map: Map): string { +function formatMap(map: Map): string { let entries: string = "" map.forEach((value, key) => { entries += `${key} : ${value}` @@ -35,16 +37,29 @@ 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, + ] + return ( - {simMap.has(SimType.PWM) ? ( - [...simMap.get(SimType.PWM)!.entries()] - .filter(x => x[1].get(" { - 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]} @@ -53,67 +68,10 @@ function generateTableBody() { {formatMap(x[1])} - ) - }) - ) : ( - <> - )} - {simMap.has(SimType.CANMotor) ? ( - [...simMap.get(SimType.CANMotor)!.entries()].map(x => { - return ( - - - CAN Motor - - - {x[0]} - - - {formatMap(x[1])} - - - ) - }) - ) : ( - <> - )} - {simMap.has(SimType.CANEncoder) ? ( - [...simMap.get(SimType.CANEncoder)!.entries()].map(x => { - return ( - - - CAN Encoder - - - {x[0]} - - - {formatMap(x[1])} - - - ) - }) - ) : ( - <> - )} - {simMap.has(SimType.Gyro) ? ( - [...simMap.get(SimType.Gyro)!.entries()].map(x => { - return ( - - - Gyro - - - {x[0]} - - - {formatMap(x[1])} - - - ) - }) - ) : ( - <> + )) + ) : ( + <> + ) )} ) @@ -137,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() @@ -157,17 +117,16 @@ 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} 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..1c75298499 100644 --- a/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureSubsystemsInterface.tsx +++ b/fission/src/ui/panels/configuring/assembly-config/interfaces/ConfigureSubsystemsInterface.tsx @@ -55,7 +55,7 @@ const ConfigureSubsystemsInterface: React.FC = ({ 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(() => { 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 40e6f3c167..0000000000 --- a/simulation/SyntheSimJava/.settings/org.eclipse.buildship.core.prefs +++ /dev/null @@ -1,13 +0,0 @@ -arguments=--init-script /Users/colbura/wpilib/2024/vscode/code-portable-data/user-data/User/globalStorage/redhat.java/1.26.2023121408/config_mac/org.eclipse.osgi/53/0/.cp/gradle/init/init.gradle --init-script /Users/colbura/wpilib/2024/vscode/code-portable-data/user-data/User/globalStorage/redhat.java/1.26.2023121408/config_mac/org.eclipse.osgi/53/0/.cp/gradle/protobuf/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=/Users/colbura/wpilib/2024/jdk -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/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/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/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/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/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 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 bc1ff1181e..0000000000 --- a/simulation/samples/JavaSample/.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 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 232b79aec1..302f80f47c 100644 --- a/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java +++ b/simulation/samples/JavaSample/src/main/java/frc/robot/Robot.java @@ -6,12 +6,19 @@ import com.revrobotics.CANSparkLowLevel.MotorType; +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; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.XboxController; import com.autodesk.synthesis.revrobotics.CANSparkMax; +import com.kauailabs.navx.frc.AHRS; import com.autodesk.synthesis.ctre.TalonFX; /** @@ -24,151 +31,170 @@ * project. */ public class Robot extends TimedRobot { - private static final String kDefaultAuto = "Default"; - private static final String kCustomAuto = "My Auto"; - private String m_autoSelected; - private final SendableChooser m_chooser = new SendableChooser<>(); - - private CANSparkMax m_SparkMax1 = new CANSparkMax(1, MotorType.kBrushless); - private CANSparkMax m_SparkMax2 = new CANSparkMax(2, MotorType.kBrushless); - private CANSparkMax m_SparkMax3 = new CANSparkMax(3, MotorType.kBrushless); - private CANSparkMax m_SparkMax4 = new CANSparkMax(4, MotorType.kBrushless); - private CANSparkMax m_SparkMax5 = new CANSparkMax(5, MotorType.kBrushless); - private CANSparkMax m_SparkMax6 = new CANSparkMax(6, MotorType.kBrushless); - //private com.autodesk.synthesis.ctre.TalonFX m_Talon = new com.autodesk.synthesis.ctre.TalonFX(2); - - /** - * This function is run when the robot is first started up and should be used - * for any - * initialization code. - */ - @Override - public void robotInit() { - m_chooser.setDefaultOption("Default Auto", kDefaultAuto); - m_chooser.addOption("My Auto", kCustomAuto); - SmartDashboard.putData("Auto choices", m_chooser); - } - - /** - * This function is called every 20 ms, no matter the mode. Use this for items - * like diagnostics - * that you want ran during disabled, autonomous, teleoperated and test. - * - *

- * This runs after the mode specific periodic functions, but before LiveWindow - * and - * SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - } - - /** - * This autonomous (along with the chooser code above) shows how to select - * between different - * autonomous modes using the dashboard. The sendable chooser code works with - * the Java - * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the - * chooser code and - * uncomment the getString line to get the auto name from the text box below the - * Gyro - * - *

- * You can add additional auto modes by adding additional comparisons to the - * switch structure - * below with additional strings. If using the SendableChooser make sure to add - * them to the - * chooser code above as well. - */ - @Override - public void autonomousInit() { - m_autoSelected = m_chooser.getSelected(); - //m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto); - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() { - - double position = m_SparkMax1.getAbsoluteEncoderSim().getPosition(); - - if (position >= 20) { - m_SparkMax1.set(0.0); - m_SparkMax2.set(0.0); - m_SparkMax3.set(0.0); - m_SparkMax4.set(0.0); - m_SparkMax5.set(0.0); - m_SparkMax6.set(0.0); - } else { - m_SparkMax1.set(1.0); - m_SparkMax2.set(1.0); - m_SparkMax3.set(1.0); - m_SparkMax4.set(1.0); - m_SparkMax5.set(1.0); - m_SparkMax6.set(1.0); - } - - - switch (m_autoSelected) { - case kCustomAuto: - // Put custom auto code here - break; - case kDefaultAuto: - default: - // Put default auto code here - break; - } - } - - /** This function is called once when teleop is enabled. */ - @Override - public void teleopInit() { - } - - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() { - 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); - } - - /** This function is called once when the robot is disabled. */ - @Override - public void disabledInit() { + private static final String kDefaultAuto = "Default"; + private static final String kCustomAuto = "My Auto"; + private String m_autoSelected; + private final SendableChooser m_chooser = new SendableChooser<>(); + + private Spark m_Spark1 = new Spark(0); + private Spark m_Spark2 = new Spark(1); + 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); + 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); + private CANSparkMax m_SparkMax3 = new CANSparkMax(3, MotorType.kBrushless); + private CANSparkMax m_SparkMax4 = new CANSparkMax(4, MotorType.kBrushless); + private CANSparkMax m_SparkMax5 = new CANSparkMax(5, MotorType.kBrushless); + private CANSparkMax m_SparkMax6 = new CANSparkMax(6, MotorType.kBrushless); + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + m_chooser.setDefaultOption("Default Auto", kDefaultAuto); + m_chooser.addOption("My Auto", kCustomAuto); + SmartDashboard.putData("Auto choices", m_chooser); + } + + /** + * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics + * that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before LiveWindow and + * SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() {} + + /** + * This autonomous (along with the chooser code above) shows how to select between different + * autonomous modes using the dashboard. The sendable chooser code works with the Java + * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and + * uncomment the getString line to get the auto name from the text box below the Gyro + * + *

You can add additional auto modes by adding additional comparisons to the switch structure + * below with additional strings. If using the SendableChooser make sure to add them to the + * chooser code above as well. + */ + @Override + 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. */ + @Override + public void autonomousPeriodic() { + + m_Spark1.set(0.5); + m_Spark2.set(-0.5); + m_Talon.set(-1.0); + + double position = m_SparkMax1.getAbsoluteEncoderSim().getPosition(); + + if (position >= 20) { m_SparkMax1.set(0.0); m_SparkMax2.set(0.0); m_SparkMax3.set(0.0); m_SparkMax4.set(0.0); m_SparkMax5.set(0.0); m_SparkMax6.set(0.0); + } else { + m_SparkMax1.set(1.0); + m_SparkMax2.set(1.0); + m_SparkMax3.set(1.0); + m_SparkMax4.set(1.0); + m_SparkMax5.set(1.0); + m_SparkMax6.set(1.0); } - /** This function is called periodically when disabled. */ - @Override - public void disabledPeriodic() { - } - - /** This function is called once when test mode is enabled. */ - @Override - public void testInit() { - } - - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() { - } - - /** This function is called once when the robot is first started up. */ - @Override - public void simulationInit() { - } - - /** This function is called periodically whilst in simulation. */ - @Override - public void simulationPeriodic() { + switch (m_autoSelected) { + case kCustomAuto: + // Put custom auto code here + break; + case kDefaultAuto: + default: + // Put default auto code here + break; } + } + + /** This function is called once when teleop is enabled. */ + @Override + 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_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); + } + + + /** This function is called once when the robot is disabled. */ + @Override + public void disabledInit() { + m_SparkMax1.set(0.0); + m_SparkMax2.set(0.0); + m_SparkMax3.set(0.0); + 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. */ + @Override + public void disabledPeriodic() { + } + + /** This function is called once when test mode is enabled. */ + @Override + public void testInit() { + } + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() { + } + + /** This function is called once when the robot is first started up. */ + @Override + public void simulationInit() { + } + + /** This function is called periodically whilst in simulation. */ + @Override + public void simulationPeriodic() { + } } diff --git a/simulation/samples/JavaSample/vendordeps/NavX.json b/simulation/samples/JavaSample/vendordeps/NavX.json new file mode 100644 index 0000000000..e978a5f745 --- /dev/null +++ b/simulation/samples/JavaSample/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