diff --git a/.vscode/extensions.json b/.vscode/extensions.json index d4886b7..0b4fb2b 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -4,7 +4,6 @@ "streetsidesoftware.code-spell-checker", "vscjava.vscode-java-debug", "vscjava.vscode-java-pack", - "github.vscode-github-actions", "visualstudioexptteam.vscodeintellicode", "visualstudioexptteam.intellicode-api-usage-examples", "visualstudioexptteam.vscodeintellicode-completions", diff --git a/.vscode/launch.json b/.vscode/launch.json index 7622240..c240627 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,24 +1,17 @@ { "version": "0.2.0", "configurations": [ - { - "type": "java", - "name": "Main", - "request": "launch", - "mainClass": "org.team340.robot.Main", - "projectName": "Crescendo2024-340" - }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true + "desktop": true, }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false + "desktop": false, } ] } diff --git a/.vscode/settings.json b/.vscode/settings.json index 3be1ebd..9b8cea9 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,5 +1,6 @@ { "java.configuration.updateBuildConfiguration": "automatic", + "java.debug.settings.onBuildFailureProceed": true, "java.server.launchMode": "Standard", "files.exclude": { "**/.git": true, @@ -48,8 +49,8 @@ "Desaturated", "Desaturation", "Devs", - "Discretize", "Discretization", + "Discretize", "DTheta", "Falsi", "Feedforward", @@ -61,15 +62,18 @@ "Interpolatable", "JoystickProfiles", "Keepalive", + "Lerp", "Motorcontrol", "Msgpack", "NetworkTables", + "NTURI", "Odometry", "Overcurrent", "PIDF", "Powerup", - "Pubuid", "Protobuf", + "Pubuid", + "Quasistatic", "Ratelimiter", "Ratelimits", "Regula", @@ -82,14 +86,17 @@ "TalonSRX", "Tauri", "Teleop", + "Topicsonly", "Traj", "Unannounce", + "Unsub", "Unsubscriber", "Unsubscribers", "Vbat", "WPIBlue", "WPILib", "WPILibJ", - "WPIRed" + "WPIRed", + "Writables" ] } diff --git a/README.md b/README.md index 417bb37..58eae71 100644 --- a/README.md +++ b/README.md @@ -1,28 +1,2 @@ -# GRRBase -A template repository for Java-based robots in the FIRST Robotics Competition. It contains boilerplate for programming with WPILib's command framework, as well as utilities outlined in the packages section below. Additionally, this repository contains GRRDashboard, a Svelte-based dashboard for interacting with and analyzing functions of the robot via Network Tables. - -## Code Styling -All Java code follows the styling guide of [Prettier](https://prettier.io/). You can apply these rules via [Spotless](https://github.com/diffplug/spotless/tree/main) using the command `./gradlew spotlessApply` - -## Packages - -### `org.team340.lib` -Contains an extended subsystem that automatically logs hardware information, as well as bindings to our custom dashboard. - -### `org.team340.lib.commands` -Contains a builder class for creating inline commands with a similar API to subclassed commands. - -### `org.team340.lib.controller` -Contains a joystick profiler for controllers, as well as a wrapper class for WPILib's `XboxController` class. - -### `org.team340.lib.swerve` -Contains a hardware agnostic swerve library with extra features such as a rate limiter that respects the kinematic constraints of the robot. - -### `org.team340.lib.utils` -Contains various utility classes. - -### `org.team340.lib.utils.config` -Contains various wrapper classes for saving constants. - -### `org.team340.lib.utils.config.rev` -Contains configuration builders that safely apply settings to REV hardware. +# Crescendo2024-340 +FRC 340's code for the 2024 season, Crescendo. diff --git a/Swerve.md b/Swerve.md deleted file mode 100644 index 8d9ba71..0000000 --- a/Swerve.md +++ /dev/null @@ -1,13 +0,0 @@ -# How To Set Up Swerve - -1. Fill in known variables - * Refer to comments in config files -2. Determine motor directions -3. Tune turn motor PID - * Use AdvantageScope to view positions -4. Characterize move motors - * Use the characterization commands -5. Tune move motor PID - * Use AdvantageScope to view speeds -6. Find speed constraint of all motors - * Set max speeds to 1000 and view peaks on AdvantageScope diff --git a/WPILib-License.md b/WPILib-License.md index 43b62ec..645e542 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2023 FIRST and other WPILib contributors +Copyright (c) 2009-2024 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/build.gradle b/build.gradle index 0d24407..2ecbc76 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" id "com.diffplug.spotless" version "6.23.3" } @@ -70,6 +70,8 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + + implementation 'com.google.code.gson:gson:2.10.1' } // Code formatting via spotless diff --git a/dashboard/index.html b/dashboard/index.html index e4f3886..73c2c51 100644 --- a/dashboard/index.html +++ b/dashboard/index.html @@ -25,9 +25,9 @@ --good: #28cd41; --bad: #ff453a; - --auto-spline: #ffffffc9; - --auto-cp-red: #a20010; - --auto-cp-blue: #0048a7; + --auto-line: #ffffffc9; + --auto-waypoint-red: #a20010; + --auto-waypoint-blue: #0048a7; --graph-grid: #ffffff5b; --graph-red: #ff453a; diff --git a/dashboard/package.json b/dashboard/package.json index 1518fda..b7e09a5 100644 --- a/dashboard/package.json +++ b/dashboard/package.json @@ -7,24 +7,24 @@ "check": "svelte-check --tsconfig ./tsconfig.json", "dev": "vite", "dev:tauri": "tauri dev", - "format": "prettier --write --plugin prettier-plugin-svelte .", - "format:check": "prettier --check --plugin prettier-plugin-svelte .", + "format": "prettier --write --plugin prettier-plugin-svelte ./src/", + "format:check": "prettier --check --plugin prettier-plugin-svelte ./src/", "preview": "vite preview", "tauri": "tauri" }, "devDependencies": { - "@sveltejs/vite-plugin-svelte": "^2.4.6", - "@tauri-apps/cli": "^1.5.6", + "@sveltejs/vite-plugin-svelte": "^3.0.1", + "@tauri-apps/cli": "^1.5.9", "@tsconfig/svelte": "^5.0.2", - "chart.js": "^4.4.0", + "chart.js": "^4.4.1", "chartjs-plugin-zoom": "^2.0.1", - "prettier": "^3.0.3", - "prettier-plugin-svelte": "^3.0.3", - "svelte": "^4.2.2", - "svelte-check": "^3.5.2", - "tslib": "^2.6.1", - "typescript": "^5.2.2", - "vite": "^4.5.0" + "prettier": "^3.2.4", + "prettier-plugin-svelte": "^3.1.2", + "svelte": "^4.2.9", + "svelte-check": "^3.6.3", + "tslib": "^2.6.2", + "typescript": "^5.3.3", + "vite": "^5.0.12" }, "private": true } diff --git a/dashboard/src/App.svelte b/dashboard/src/App.svelte index 78bcff5..0c2fca4 100644 --- a/dashboard/src/App.svelte +++ b/dashboard/src/App.svelte @@ -4,11 +4,11 @@ import ConnectingDialog from "./components/ConnectingDialog.svelte"; import NavBar from "./components/NavBar.svelte"; - import Analysis from "./tabs/Analysis.svelte"; import AutoSelection from "./tabs/AutoSelection.svelte"; import DriverView from "./tabs/DriverView.svelte"; - import { NTConnected, RobotEnabled } from "./ntStores"; + import { NTConnected } from "./ntStores"; + import { NTSvelteClientState } from "./lib/NTSvelte"; // Define your tabs here. // The navbar will be automatically populated using this object, and tabs will be dynamically displayed when selected. @@ -17,7 +17,6 @@ const tabs = { "Driver View": DriverView, "Auto Selection": AutoSelection, - Analysis: Analysis, }; // Helpers for tab selection. @@ -28,10 +27,8 @@
- {#if $NTConnected || IGNORE_CONNECTION} - -

{$RobotEnabled}

- + + {#if $NTConnected === NTSvelteClientState.CONNECTED || IGNORE_CONNECTION}
diff --git a/dashboard/src/assets/field22.png b/dashboard/src/assets/field22.png index dd00269..506b2b1 100644 Binary files a/dashboard/src/assets/field22.png and b/dashboard/src/assets/field22.png differ diff --git a/dashboard/src/assets/field23.png b/dashboard/src/assets/field23.png index f9ea214..88aa77b 100644 Binary files a/dashboard/src/assets/field23.png and b/dashboard/src/assets/field23.png differ diff --git a/dashboard/src/assets/field24.png b/dashboard/src/assets/field24.png new file mode 100644 index 0000000..ca499cf Binary files /dev/null and b/dashboard/src/assets/field24.png differ diff --git a/dashboard/src/components/NavBar.svelte b/dashboard/src/components/NavBar.svelte index ea15285..4464848 100644 --- a/dashboard/src/components/NavBar.svelte +++ b/dashboard/src/components/NavBar.svelte @@ -100,7 +100,7 @@ {$NTURI} - {$NTBitrate}kb/s | {$NTLatency}ms + {($NTBitrate / 1000).toFixed(2)}kb/s | {$NTLatency.toFixed(2)}ms
diff --git a/dashboard/src/components/analysis/Field.svelte b/dashboard/src/components/analysis/Field.svelte deleted file mode 100644 index 9a67b08..0000000 --- a/dashboard/src/components/analysis/Field.svelte +++ /dev/null @@ -1,65 +0,0 @@ - - -
- -
- - field - - - - {#each $FieldModules as module} - - {/each} - - - -
-
- - diff --git a/dashboard/src/components/analysis/LineGraph.svelte b/dashboard/src/components/analysis/LineGraph.svelte deleted file mode 100644 index 10f6d62..0000000 --- a/dashboard/src/components/analysis/LineGraph.svelte +++ /dev/null @@ -1,166 +0,0 @@ - - -
-
- -
-
- - diff --git a/dashboard/src/components/analysis/NetworkTablesList.svelte b/dashboard/src/components/analysis/NetworkTablesList.svelte deleted file mode 100644 index 059a57b..0000000 --- a/dashboard/src/components/analysis/NetworkTablesList.svelte +++ /dev/null @@ -1,180 +0,0 @@ - - -
-
- {#each Object.entries(hierarchy).sort((a, b) => a[0].localeCompare(b[0])) as [key, topic]} - {#if Array.isArray(topic)} -
-
- -
-
- ({topic[1]}) -
-
- -
-
- {:else if topLevel} - - {:else} -
- {key} - -
- {/if} - {/each} -
-
- - diff --git a/dashboard/src/components/analysis/Stats.svelte b/dashboard/src/components/analysis/Stats.svelte deleted file mode 100644 index e69de29..0000000 diff --git a/dashboard/src/constants.ts b/dashboard/src/constants.ts index 5112216..9ee51c8 100644 --- a/dashboard/src/constants.ts +++ b/dashboard/src/constants.ts @@ -4,5 +4,5 @@ export const DEFAULT_URI = `localhost`; // Use for simulation // Field size constants. // Set these values to the field's size in meters. -export const FIELD_WIDTH = 16.5417; -export const FIELD_HEIGHT = 8.0136; +export const FIELD_WIDTH = 16.54175; +export const FIELD_HEIGHT = 8.21055; diff --git a/dashboard/src/lib/MessagePack.ts b/dashboard/src/lib/MessagePack.ts index e371fe2..0d93475 100644 --- a/dashboard/src/lib/MessagePack.ts +++ b/dashboard/src/lib/MessagePack.ts @@ -1,6 +1,6 @@ /** * MessagePack serializer and deserializer. - * Adapted from https://github.com/gerth2/NetworkTablesClients (nt4/js/src/msgpack/msgpack.js) + * Adapted from https://github.com/gerth2/NetworkTablesClients/blob/main/nt4/js/src/msgpack/msgpack.js */ export class MessagePack { private constructor() {} @@ -589,8 +589,14 @@ export class MessagePack { }; } - // Decodes a string from UTF-8 bytes. - // Based on: https://gist.github.com/pascaldekloe/62546103a1576803dade9269ccf76330 + /** + * Decodes a string from UTF-8 bytes. + * From https://gist.github.com/pascaldekloe/62546103a1576803dade9269ccf76330 + * @param bytes The bytes to decode. + * @param start The index of the byte to start with. + * @param length The number of bytes to decode. + * @returns The decoded string. + */ function decodeUtf8(bytes: Uint8Array, start: number, length: number): string { let i = start, str = ``; diff --git a/dashboard/src/lib/NTSvelte.ts b/dashboard/src/lib/NTSvelte.ts index 679e299..0e8adeb 100644 --- a/dashboard/src/lib/NTSvelte.ts +++ b/dashboard/src/lib/NTSvelte.ts @@ -1,14 +1,6 @@ -import { readable, writable, type Readable, type Writable, derived } from "svelte/store"; +import { get, writable, type Readable, type Writable, readonly, readable } from "svelte/store"; import { MessagePack } from "./MessagePack"; -export type NTTopicMapNestedValue = string | NTTopicMapNested; -export interface NTTopicMapNested extends Record {} - -/** - * Types supported by Network Tables. - */ -export type NTType = boolean | number | string | Uint8Array | boolean[] | number[] | string[]; - /** * Network tables type codes. */ @@ -30,348 +22,745 @@ export const NTTypeCodes = { "string[]": 20, } as const; +/** + * Types supported by Network Tables. + */ +export type NTType = boolean | number | string | Uint8Array | boolean[] | number[] | string[]; +/** + * A string representing a topic's type. + */ export type NTTypeString = keyof typeof NTTypeCodes; -export type NTReference = [string, NTTypeString]; -export type NTHierarchyValue = NTReference | NTHierarchy; -export interface NTHierarchy extends Record {} /** - * A topic in network tables. + * A parsed text data frame. */ -export class NTTopic { - private readonly _key: string; - private _id: number | null; - private _type: NTTypeString | null; +export interface NTTextFrame { + method: `publish` | `unpublish` | `setproperties` | `subscribe` | `unsubscribe` | `announce` | `unannounce` | `properties`; + params: Record; +} + +/** + * An NT server topic. + */ +export interface NTTopic { + readonly name: string; + readonly id: number; + readonly type: NTTypeString; + readonly pubuid?: number; + readonly properties: Record>; +} + +/** + * NT Subscriber settings. + */ +export interface NTSubscriberSettings { + /** + * If true, the server should send all value changes over the wire. If false, only the most recent value is sent (same as NT 3.0 behavior). + * @default true + */ + readonly all: boolean; + /** + * How frequently the server should send changes. The server may send more frequently than this (e.g. use a combined minimum period for all values) or apply a restricted range to this value. + * Specified in milliseconds. + * @default 100 + */ + readonly periodic: number; + /** + * If topic history should be saved. + * @default false + */ + readonly saveHistory: boolean; +} + +/** + * An NT subscription that can be subscribed to a single topic. + */ +class NTSubscriber { + private readonly _topicName: string; + private readonly _settings: NTSubscriberSettings; + + private _history: Map = new Map(); + private _listeners: Set<(value: NTType | null) => void> = new Set(); + private _subuid: number | null = null; + private _topicId: number | null = null; private _value: NTType | null = null; private _valueTimestamp: number | null = null; - private _valueHistory: Map = new Map(); - private _listeners: Set<(value: NTType | null) => void> = new Set(); - private _pubuid: number | null = null; /** - * Create a topic. - * @param key - * @param id + * Creates the subscriber. + * @param topicName The name of the topic. + * @param settings Subscriber settings. */ - constructor(key: string, id: number | null = null, type: NTTypeString | null = null) { - this._key = key; - this._id = id; - this._type = type; + public constructor(topicName: string, settings: NTSubscriberSettings) { + this._topicName = topicName; + this._settings = settings; } - public getKey(): string { - return this._key; + /** + * Gets the subscribe message JSON and sets the subscriber's internal state to be subscribed. + * @param subuid The subscription UID to use. + */ + public subscribe(subuid: number): NTTextFrame { + this._subuid = subuid; + return { + method: `subscribe`, + params: { + subuid, + topics: [this._topicName], + options: { + all: this._settings.all, + periodic: this._settings.periodic / 1000, + }, + }, + }; + } + + /** + * Gets the unsubscribe message JSON and sets the subscriber's internal state to be unsubscribed. + * Additionally, this sets the subscriber's value to `null`, updates the listeners, and clears the subscriber's history. + */ + public unsubscribe(): NTTextFrame { + const subuid = this._subuid; + this._subuid = null; + this._topicId = null; + if (this._value !== null) this.updateValue(null); + this._history.clear(); + return { + method: `unsubscribe`, + params: { subuid }, + }; } - public setId(id: number | null): void { - this._id = id; + /** + * This should be called when the subscription's topic is announced. + * @param topicId The topic ID sent by the server. + */ + public onAnnounce(topicId: number): void { + this._topicId = topicId; } - public getId(): number | null { - return this._id; + /** + * This should be called when the subscription's topic is unannounced. + * Resets the saved subscription UID and topic ID, sets the subscriber's value to `null`, and updates listeners. + * Topic history is still saved. + */ + public onUnannounce(): void { + this._subuid = null; + this._topicId = null; + if (this._value !== null) this.updateValue(null); } - public setType(type: NTTypeString | null): void { - this._type = type; + /** + * This should be called when the client disconnects. + */ + public onDisconnect(): void { + this._subuid = null; + this._topicId = null; + if (this._value !== null) this.updateValue(null); + this._history.clear(); + } + + /** + * Gets the subscriber's subscription UID sent by the client. + * This returns `null` if the subscriber hasn't been assigned a UID (via {@link NTSubscriber.subscribe()}), or if it was unsubscribed (via {@link NTSubscriber.unsubscribe()}). + */ + public getSubuid(): number | null { + return this._subuid; } - public getType(): NTTypeString | null { - return this._type; + /** + * Gets the subscriber's topic ID sent by the server. + * This returns `null` if the topic hasn't received an `announce` message, or if the topic was unannounced. + */ + public getTopicId(): number | null { + return this._topicId; } - public setValue(value: NTType | null, timestamp: number | null = null): void { + /** + * Updates the subscriber's value and updates its listeners with the new value. + * Also saves the value to the subscriptions's history if enabled. + * @param value The new value. + * @param timestamp The value's timestamp in milliseconds. + */ + public updateValue(value: NTType | null, timestamp: number | null = null): void { if (timestamp === null || timestamp > (this._valueTimestamp ?? 0)) { this._value = value; this._valueTimestamp = timestamp; this._listeners.forEach((listener) => listener(value)); } - if (timestamp !== null) { - this._valueHistory.set(timestamp, value); + if (this._settings.saveHistory && timestamp !== null) { + this._history.set(timestamp, value); } } - public getValue(): NTType | null { - return this._value; - } - - public getValueHistory(): Map { - return this._valueHistory; - } - + /** + * Adds a listener to the subscriber and invokes it with the current value. + * @param listener The listener to add. + */ public addListener(listener: (value: NTType | null) => void): void { this._listeners.add(listener); listener(this._value); } - public removeListener(listener: (value: NTType | null) => void): void { + /** + * Removes a listener. + * @param listener The listener to remove. + * @returns If the subscriber contains any other listeners. + */ + public removeListener(listener: (value: NTType | null) => void): boolean { this._listeners.delete(listener); + return this._listeners.size > 0; + } + + /** + * Gets the subscriber's history. + */ + public getHistory(): Map { + return this._history; } - public setPublished(pubuid: number): void { + /** + * Trims the subscriber's history back to a specified timestamp. + * @param until Keep history as far back as this timestamp in milliseconds. + */ + public trimHistory(until: number) { + if (!this._settings.saveHistory) return; + for (const [timestamp] of this._history) { + if (timestamp < until) this._history.delete(timestamp); + } + } +} + +/** + * NT Publisher settings. + */ +export interface NTPublisherSettings { + /** + * If true, the last set value will be periodically saved to persistent storage on the server and be restored during server startup. Topics with this property set to true will not be deleted by the server when the last publisher stops publishing. + * @default false + */ + readonly persistent: boolean; + /** + * Topics with this property set to true will not be deleted by the server when the last publisher stops publishing. + * @default false + */ + readonly retained: boolean; + /** + * If false, the server and clients will not store the value of the topic. This means that only value updates will be available for the topic. + * @default true + */ + readonly cached: boolean; +} + +/** + * An NT publisher. + */ +class NTPublisher { + private readonly _topicName: string; + private readonly _type: NTTypeString; + private readonly _properties: Record; + private readonly _settings: NTPublisherSettings; + + private _listeners: Set<(value: NTType) => void> = new Set(); + private _receivedAck: boolean = false; + private _pubuid: number | null = null; + private _value: NTType; + + /** + * Creates the publisher. + * @param topicName The name of the topic. + * @param type The topic's type. + * @param initialValue The initial value of the publisher. + * @param settings Publisher settings. + * @param properties Custom topic properties. + */ + public constructor( + topicName: string, + type: NTTypeString, + initialValue: NTType, + settings: NTPublisherSettings, + properties?: Record, + ) { + this._topicName = topicName; + this._type = type; + this._value = initialValue; + this._properties = properties ?? {}; + this._settings = settings; + } + + /** + * Gets the publish message JSON and sets the publisher's internal state to be published. + * @param pubuid The publish UID to use. + */ + public publish(pubuid: number): NTTextFrame { this._pubuid = pubuid; + return { + method: `publish`, + params: { + pubuid, + name: this._topicName, + type: this._type, + properties: { + ...this._properties, + persistent: this._settings.persistent, + retained: this._settings.retained, + cached: this._settings.cached, + }, + }, + }; } - public setUnpublished(): void { + /** + * Gets the unpublish message JSON and sets the publisher's internal state to be unpublished. + * This does not reset the saved value of the publisher. + */ + public unpublish(): NTTextFrame { + const pubuid = this._pubuid; this._pubuid = null; + return { + method: `unpublish`, + params: { pubuid }, + }; + } + + /** + * This should be called when the published topic is acknowledged by the server. + */ + public onAck(): void { + this._receivedAck = true; } - public isPublished(): boolean { - return this._pubuid !== null; + /** + * This should be called when the client disconnects. + */ + public onDisconnect(): void { + this._pubuid = null; + this._receivedAck = false; } + /** + * Gets the publisher's publish UID sent by the client. + * This returns `null` if the publisher hasn't been assigned a UID (via {@link NTPublisher.publish()}), or if it was unpublished (via {@link NTPublisher.unpublish()}). + */ public getPubuid(): number | null { return this._pubuid; } + + /** + * Returns `true` if the published topic was acknowledged by the server. + */ + public getAck(): boolean { + return this._receivedAck; + } + + /** + * Updates the publisher's client-side value and updates its listeners with the new value. + * @param value The new value. + */ + public updateValue(value: NTType): void { + this._value = value; + this._listeners.forEach((listener) => listener(value)); + } + + /** + * Adds a listener to the publisher and invokes it with the current value. + * @param listener The listener to add. + */ + public addListener(listener: (value: NTType) => void): void { + this._listeners.add(listener); + listener(this._value); + } + + /** + * Removes a listener. + * @param listener The listener to remove. + * @returns If the publisher contains any other listeners. + */ + public removeListener(listener: (value: NTType) => void): boolean { + this._listeners.delete(listener); + return this._listeners.size > 0; + } + + /** + * Returns a binary frame representing the publisher's current value. + * Returns `null` if a publisher UID has not been assigned to the publisher, or if it has a value of `null`. + * @param serverTimeUs The current server time (in microseconds). + */ + public getBinaryFrame(serverTimeUs: number): any[] | null { + if (typeof this._pubuid !== `number` || this._value === null) return null; + else return [this._pubuid, serverTimeUs, NTTypeCodes[this._type], this._value]; + } +} + +/** + * NT Client state. + */ +export enum NTSvelteClientState { + IDLE, + DISCONNECTED, + CONNECTING, + CONNECTED, +} + +/** + * NT Client settings. + */ +export interface NTSvelteClientSettings { + /** + * The name for the client. Used to identify the connection in robot logs. + * @default `GRRDashboard` + */ + readonly appName: string; + /** + * The TTL for a value in a topic's history in milliseconds. Has no effect when a subscriber's `saveHistory` is `false`. + * Set to `0` to disable. + * @default 0 + */ + readonly historyTTL: number; + /** + * Default subscriber settings. + */ + readonly subscribers: NTSubscriberSettings; + /** + * Default publisher settings. + */ + readonly publishers: NTPublisherSettings; } /** - * A NetworkTables 4 implementation for svelte stores. - * Automatically subscribes to all topics. + * A Network Tables 4.1 client with bindings for Svelte. */ -export class NTSvelte { - private static readonly TIMESTAMP_INTERVAL = 2500; - - private readonly _appName: string; - private readonly _updateInterval: number; - - private _bitrateStore: Writable = writable(`0.00`); - private _byteLengthCounter: number = 0; - private _connectionStore: Writable = writable(false); - private _lastServerTime: number | null = null; - private _latencyStore: Writable = writable(`0.00`); - private _serverOffset: number | null = null; - private _serverTime: number | null = null; - private _started = false; - private _subuidNonce = 0; - private _topics: Map = new Map(); - private _topicMap: Writable = writable([]); - private _uri: string; - private _uriStore: Writable; +export class NTSvelteClient { + private static readonly _HISTORY_SWEEP_PERIOD = 100; + private static readonly _OPEN_TIMEOUT = 5000; + private static readonly _PORT = 5810; + private static readonly _RECONNECT_DELAY = 1500; + private static readonly _RTT_PERIOD = 500; + private static readonly _SERVER_AVAILABLE_TIMEOUT = 500; + private static readonly _WS_PROTOCOL = `v4.1.networktables.first.wpi.edu`; + private static readonly _WS_RTT_PROTOCOL = `rtt.networktables.first.wpi.edu`; + + private readonly _settings: NTSvelteClientSettings; + + private _bitrate: Writable = writable(0); + private _latency: Writable = writable(0); + private _openTimeout: number | null = null; + private _serverTime: Writable = writable(); + private _state: Writable = writable(NTSvelteClientState.IDLE); + private _uidNonce: number = 0; + private _uri: Writable; + private _ws: WebSocket | null = null; - private _wsListeners: Record = { - open: this._onOpen.bind(this), - close: this._onClose.bind(this), - error: this._onError.bind(this), - message: this._onMessage.bind(this), + private _wsRtt: WebSocket | null = null; + private _wsListeners = { + open: [() => this._onOpen(false), () => this._onOpen(true)], + close: [(event: CloseEvent) => this._onClose(event, false), (event: CloseEvent) => this._onClose(event, true)], + error: [() => this._onError(false), () => this._onError(true)], + message: [(event: MessageEvent) => this._onMessage(event, false), (event: MessageEvent) => this._onMessage(event, true)], }; + private _aliveAck = false; + private _serverOffsetUs: number = 0; + private _usage: number = 0; + + private _topicsOnlySubuid: number | null = null; + private _publishers: Map = new Map(); + private _subscribers: Map = new Map(); + private _serverTopics: Writable> = writable(new Map()); + /** - * Create the NT client. - * @param appName The app name. - * @param uri The initial URI to connect to. - * @param updateInterval The interval at which the client should receive values from network tables at. + * Creates the NT Svelte Client. + * Use {@link NTSvelteClient.connect()} to start the connection. + * @param uri The URI to connect to. + * @param settings Client settings. */ - constructor(appName: string, uri: string, updateInterval: number) { - this._appName = appName; - this._uri = uri; - this._uriStore = writable(uri); - this._updateInterval = updateInterval; + public constructor(uri: string, settings?: Partial) { + this._uri = writable(uri); + this._settings = { + appName: settings?.appName ?? `GRRDashboard`, + historyTTL: settings?.historyTTL ?? 0, + subscribers: { + all: settings?.subscribers?.all ?? true, + periodic: settings?.subscribers?.periodic ?? 100, + saveHistory: settings?.subscribers?.saveHistory ?? false, + }, + publishers: { + persistent: settings?.publishers?.persistent ?? false, + retained: settings?.publishers?.retained ?? false, + cached: settings?.publishers?.cached ?? true, + }, + }; setInterval(() => { - if (this._ws?.readyState === WebSocket.OPEN) { - if (this._lastServerTime !== null && this._lastServerTime === this._serverTime) { - this.disconnect(`Timed Out`); - return; - } else { - this._lastServerTime = this._serverTime; - } - + if (get(this._state) === NTSvelteClientState.CONNECTED) { + if (!this._aliveAck) return this._restart(`Timed out`); + this._aliveAck = false; this._sendTimestamp(); - - const bitrate = ((this._byteLengthCounter * 0.0016) / (NTSvelte.TIMESTAMP_INTERVAL / 1000)).toFixed(2); - this._bitrateStore.set(bitrate); - console.log(`[NT] Bitrate: ${bitrate} kb/s`); - this._byteLengthCounter = 0; + this._bitrate.set(this._usage / (NTSvelteClient._RTT_PERIOD / 1000)); + this._usage = 0; } - }, NTSvelte.TIMESTAMP_INTERVAL); + }, NTSvelteClient._RTT_PERIOD); + + if (this._settings.historyTTL > 0) { + setInterval(() => { + this._subscribers.forEach((subscriber) => { + subscriber.trimHistory(Date.now() - this._settings.historyTTL); + }); + }, NTSvelteClient._HISTORY_SWEEP_PERIOD); + } } /** - * Connect to the robot. + * Sets a new URI and restarts the client. + * @param newURI The new URI. */ - public connect(): void { - if (!this._started) { - this._started = true; - this._connect(); - } + public setURI(newURI: string) { + this._uri.set(newURI); + this._restart(`URI Changed`); } /** - * Disconnect from the robot. + * Connect to the NT server. */ - public disconnect(reason: string = `Client Disconnect`): void { - if (this._started) { - this._started = false; - this._ws?.close(1000, reason); - this._ws = null; - console.log(`[NT] Disconnected: ${reason}`); + public connect(): void { + if (get(this._state) === NTSvelteClientState.IDLE) { + this._state.set(NTSvelteClientState.DISCONNECTED); + this._connect(); } } - public setURI(uri: string): void { - this.disconnect(); - this._uri = uri; - this._uriStore.set(uri); - this.connect(); + /** + * Gets the URI in use by the client, as a readable. + */ + public uriReadable(): Readable { + return readonly(this._uri); } - public getURIStore(): Readable { - return this._uriStore; + /** + * Gets the state of the client, as a readable. + */ + public stateReadable(): Readable { + return readonly(this._state); } - public getConnectionStore(): Readable { - return this._connectionStore; + /** + * Gets the bitrate between the client and server (bits/s), as a readable. + */ + public bitrateReadable(): Readable { + return readonly(this._bitrate); } - public getBitrateStore(): Readable { - return this._bitrateStore; + /** + * Gets the latency between the client and server (ms), as a readable. + */ + public latencyReadable(): Readable { + return readonly(this._latency); } - public getLatencyStore(): Readable { - return this._latencyStore; + /** + * Gets the server time in milliseconds, as a readable. + */ + public serverTimeReadable(): Readable { + return readonly(this._serverTime); } - public getTopicMapStore(): Readable { - return this._topicMap; + /** + * Gets a map of topics announced by the server, as a readable. + */ + public topicsReadable(): Readable> { + return readonly(this._serverTopics); } - public getTopicMapHierarchyStore(): Readable { - return derived(this._topicMap, (value) => { - return value.reduce((p, c) => { - let current = p; - c[0].split(`/`).forEach((topic, i, array) => { - if (i + 1 === array.length) current[topic] = [c[0], c[1]]; - else { - const n = typeof current[topic] === `object` ? (current[topic] as NTHierarchy) : {}; - current[topic] = n; - current = n; - } - }); - return p; - }, {} as NTHierarchy); - }); + /** + * Subscribes to all topics with the `topicsonly` flag set to `true`. + * Useful for observing all topics available on the server without receiving value changes. + */ + public subAllNoValues(): void { + if (this._topicsOnlySubuid === null || this._topicsOnlySubuid < 0) { + this._topicsOnlySubuid = this._genUID(); + const sent = this._sendJSON({ + method: `subscribe`, + params: { + subuid: this._topicsOnlySubuid, + topics: [``], + options: { + periodic: this._settings.subscribers.periodic / 1000, + topicsonly: true, + prefix: true, + }, + }, + }); + if (!sent) this._topicsOnlySubuid = -1; + } } - getTopicHistoryMap(key: string): Map { - return this._topics.get(key)?.getValueHistory() ?? new Map(); + /** + * Removes a subscription created from {@link NTSvelteClient.subAllNoValues()}. + */ + public unsubAllNoValues(): void { + if (this._topicsOnlySubuid !== null && this._topicsOnlySubuid >= 0) { + const subuid = this._topicsOnlySubuid; + this._topicsOnlySubuid = null; + this._sendJSON({ method: `unsubscribe`, params: { subuid } }); + } } - public createSubscription(key: string, defaultValue: T | null): Readable { + /** + * Subscribes to a NT topic. + * @param key The key of the topic to subscribe to. + * @param defaultValue The default value, returned when the topic is unavailable. + * @param settings Subscription settings. Ignored if a subscription with the same key is currently active. + * @returns The value of the topic as a readable. + */ + public subscribe( + key: string, + defaultValue: T | null = null, + settings?: Partial, + ): Readable { return readable(defaultValue, (set) => { - const listener = (value: T | null) => { - set(value ?? defaultValue); - }; + const listener = (value: any) => set(value ?? defaultValue); - const existing = this._topics.get(key); - if (existing) { - existing.addListener(listener as any); + let sub = this._subscribers.get(key); + if (sub) { + sub.addListener(listener); } else { - const newTopic = new NTTopic(key); - newTopic.addListener(listener as any); - this._topics.set(key, newTopic); + sub = new NTSubscriber(key, { + all: settings?.all ?? this._settings.subscribers.all, + periodic: settings?.periodic ?? this._settings.subscribers.periodic, + saveHistory: settings?.saveHistory ?? this._settings.subscribers.saveHistory, + }); + sub.addListener(listener); + sub.updateValue(null); + const existingTopic = get(this._serverTopics).get(key); + if (existingTopic) sub.onAnnounce(existingTopic.id); + this._subscribers.set(key, sub); + this._sendJSON(sub.subscribe(this._genUID())); } return () => { - const existing = this._topics.get(key); - if (existing) { - existing.removeListener(listener as any); + if (!sub) return; + const others = sub.removeListener(listener); + if (!others) { + this._sendJSON(sub.unsubscribe()); + this._subscribers.delete(key); } }; }); } - public createPublisher(key: string, type: keyof typeof NTTypeCodes, firstValue: T): Writable { - let topic: NTTopic; - const existing = this._topics.get(key); - if (existing) topic = existing; - else { - topic = new NTTopic(key, null, type); - this._topics.set(key, topic); - this._topicMap.update((last) => [...last, [key, type]]); - } - - const inst = writable(firstValue, (set) => { - const listener = (value: T | null) => { - set(value); - }; + /** + * Publishes a NT topic. + * To modify the published topic's properties, + * @param key The key of the topic to publish. + * @param type The topic's type. + * @param initialValue The topic's initial value. If the topic is already published, its value is updated to this. + * @param settings Publish settings. Ignored if a publisher with the same key is currently active. + * @param properties Custom topic properties. Ignored if a publisher with the same key is currently active. + * @returns The publisher as a writable. Its value is updated if other writables assigned to the same publisher update their value. + */ + public publish( + key: string, + type: NTTypeString, + initialValue: T, + settings?: Partial, + properties?: Record, + ): Writable { + const store = writable(initialValue, (set) => { + const listener = (value: any) => set(value); + + let pub = this._publishers.get(key); + if (pub) { + pub.addListener(listener); + pub.updateValue(initialValue); + } else { + pub = new NTPublisher( + key, + type, + initialValue, + { + persistent: settings?.persistent ?? this._settings.publishers.persistent, + retained: settings?.retained ?? this._settings.publishers.retained, + cached: settings?.cached ?? this._settings.publishers.cached, + }, + properties, + ); + pub.addListener(listener); + pub.updateValue(initialValue); + if (typeof get(this._serverTopics).get(key)?.pubuid === `number`) pub.onAck(); + this._publishers.set(key, pub); + this._sendJSON(pub.publish(this._genUID())); + } - topic.addListener(listener as any); + if (pub.getAck()) { + const frame = pub.getBinaryFrame(this._getServerTimeUs()); + if (Array.isArray(frame)) this._sendMessagePack(frame, false); + } return () => { - const existing = this._topics.get(key); - if (existing) { - existing.removeListener(listener as any); + if (!pub) return; + const others = pub.removeListener(listener); + if (!others) { + this._sendJSON(pub.unpublish()); + this._publishers.delete(key); } }; }); return { - subscribe: inst.subscribe, - set: (value: T | null) => { - if (value === null) { - if (topic.getPubuid()) { - this._sendJSON(`unpublish`, { pubuid: topic.getPubuid() }); - } - topic.setUnpublished(); - } else { - if (!topic.isPublished()) { - const pubuid = this._getPubuid(); - this._sendJSON(`publish`, { - name: key, - pubuid: pubuid, - type, - }); - - topic.setPublished(pubuid); - } - - this._sendMessagePack([ - topic.getPubuid()!, - (this._serverOffset ?? 0) + Date.now() * 1000, - NTTypeCodes[type], - value as any, - ]); + subscribe: store.subscribe, + update: store.update, + set: (value: T) => { + const pub = this._publishers.get(key); + pub?.updateValue(value); + if (pub?.getAck()) { + const frame = pub.getBinaryFrame(this._getServerTimeUs()); + if (Array.isArray(frame)) this._sendMessagePack(frame, false); } - - topic.setValue(value, this._serverOffset !== null ? this._serverOffset + Date.now() * 1000 : null); }, - update: inst.update, }; } /** - * Connect to the robot. + * Gets the URL used for checking if the server is alive. */ - private _connect(): void { - const url = encodeURI(`ws://${this._uri}:5810/nt/${this._appName}`); - this._ws ??= new WebSocket(url, `networktables.first.wpi.edu`); - this._ws.binaryType = `arraybuffer`; + private get _aliveUrl(): string { + return encodeURI(`http://${get(this._uri)}:${NTSvelteClient._PORT}`); + } - console.log(`[NT] Connecting to ${url}`); + /** + * Gets the server's WebSocket URL. + */ + private get _wsUrl(): string { + return encodeURI(`ws://${get(this._uri)}:${NTSvelteClient._PORT}/nt/${this._settings.appName}`); + } - for (const [type, listener] of Object.entries(this._wsListeners)) { - this._ws.addEventListener(type, listener as any); - } + /** + * Gets the server's current time (in microseconds). + */ + private _getServerTimeUs(): number { + return Date.now() * 1000 + this._serverOffsetUs; } /** - * Send the client's current time. + * Sends the client's timestamp via the RTT connection. */ - private _sendTimestamp(): void { - this._sendMessagePack([-1, 0, NTTypeCodes.int, Date.now() * 1000]); + private _sendTimestamp() { + this._sendMessagePack([-1, 0, NTTypeCodes.int, Date.now() * 1000], true); } /** - * Send MessagePack binary data. + * Sends a message pack packet. * @param data The data to send. + * @param rtt If the packet should be sent to the RTT connection. * @returns If the data was sent. */ - private _sendMessagePack(data: number[]): boolean { - if (this._ws?.readyState === WebSocket.OPEN) { + private _sendMessagePack(data: any[], rtt: boolean): boolean { + const ws = rtt ? this._wsRtt : this._ws; + if (ws?.readyState === WebSocket.OPEN) { const msg = MessagePack.serialize(data); - this._ws.send(msg); - this._byteLengthCounter += msg.byteLength; + ws.send(msg); + this._usage += msg.byteLength * 8; return true; } else { return false; @@ -379,165 +768,289 @@ export class NTSvelte { } /** - * Send JSON data. + * Sends a JSON message. + * Only used for the primary WS connection, not the RTT connection. * @param method The method value to send. * @param params JSON parameters. * @returns If the data was sent. */ - private _sendJSON(method: string, params: any): boolean { + private _sendJSON(data: NTTextFrame): boolean { if (this._ws?.readyState === WebSocket.OPEN) { - const msg = JSON.stringify([{ method, params }]); + const msg = JSON.stringify([data]); this._ws.send(msg); - this._byteLengthCounter += new Blob([msg]).size; + this._usage += this._getStringSize(msg); return true; } else { return false; } } - private _onOpen(): void { - console.log(`[NT] Connected`); - this._sendTimestamp(); - this._sendJSON(`subscribe`, { - topics: [`/`], - subuid: this._getSubuid(), - options: { - all: true, - periodic: this._updateInterval / 1000, - prefix: true, - }, - }); + /** + * Connect to the server. + * @param reconnecting If the client is reconnecting. + */ + private async _connect(reconnecting: boolean = false): Promise { + if (get(this._state) !== NTSvelteClientState.DISCONNECTED) return; + if (this._openTimeout !== null) clearTimeout(this._openTimeout); + this._state.set(NTSvelteClientState.CONNECTING); + + if (reconnecting) { + console.log(`[NTSvelte] Waiting to reconnect...`); + await new Promise((resolve) => setTimeout(resolve, NTSvelteClient._RECONNECT_DELAY)); + } - setTimeout(() => { - if (this._ws?.readyState === WebSocket.OPEN) { - this._connectionStore.set(true); - } - }, 100); + console.log(`[NTSvelte] Connecting...`); + + let aliveResult: Response | null = null; + let length = `0.00`; + + try { + const start = Date.now(); + aliveResult = await fetch(this._aliveUrl, { signal: AbortSignal.timeout(NTSvelteClient._SERVER_AVAILABLE_TIMEOUT) }); + length = ((Date.now() - start) / 1000).toFixed(2); + } catch (_) {} + + if (!aliveResult?.ok) { + this._state.set(NTSvelteClientState.DISCONNECTED); + console.warn(`[NTSvelte] Server not responding after ${length}s while attempting to connect`); + return this._connect(true); + } + + console.log(`[NTSvelte] Server responded after ${length}s`); + + this._openTimeout = setTimeout(() => this._restart(`Timed out while connecting`), NTSvelteClient._OPEN_TIMEOUT); + + this._ws = new WebSocket(this._wsUrl, [NTSvelteClient._WS_PROTOCOL]); + this._ws.binaryType = `arraybuffer`; + + this._wsRtt = new WebSocket(this._wsUrl, [NTSvelteClient._WS_RTT_PROTOCOL]); + this._wsRtt.binaryType = `arraybuffer`; + + for (const [type, listener] of Object.entries(this._wsListeners)) { + this._ws.addEventListener(type, listener[0] as any); + this._wsRtt.addEventListener(type, listener[1] as any); + } } - private _onClose(event: CloseEvent): void { + /** + * Closes and restart the connection. + * @param reason The reason for restarting. + */ + private _restart(reason: string): void { + if (this._openTimeout !== null) clearTimeout(this._openTimeout); + this._openTimeout = null; + for (const [type, listener] of Object.entries(this._wsListeners)) { - this._ws?.removeEventListener(type, listener as any); + this._ws?.removeEventListener(type, listener[0] as any); + this._wsRtt?.removeEventListener(type, listener[1] as any); } + this._ws?.close(); this._ws = null; - this._bitrateStore.set(`0.00`); - this._byteLengthCounter = 0; - this._connectionStore.set(false); - this._lastServerTime = null; - this._latencyStore.set(`0.00`); - this._serverTime = null; - this._topics.forEach((topic) => { - if (topic.isPublished()) { - topic.setUnpublished(); - } else { - topic.setValue(null, this._serverOffset !== null ? this._serverOffset + Date.now() * 1000 : null); - } - }); - this._serverOffset = null; - console.warn(`[NT] Socket closed with code ${event.code}: ${event.reason?.length ? event.reason : `Unknown Reason`}`); + this._wsRtt?.close(); + this._wsRtt = null; - if (this._started) { - setTimeout(() => this._connect(), 1000); - } + this._bitrate.set(0); + this._latency.set(0); + this._serverTime.set(0); + + this._aliveAck = false; + this._serverOffsetUs = 0; + this._usage = 0; + + if (this._topicsOnlySubuid !== null) this._topicsOnlySubuid = -1; + this._subscribers.forEach((subscriber) => subscriber.onDisconnect()); + this._publishers.forEach((publisher) => publisher.onDisconnect()); + this._serverTopics.set(new Map()); + + this._state.set(NTSvelteClientState.DISCONNECTED); + console.warn(`[NTSvelte] Restarting: ${reason}`); + this._connect(true); } - private _onError(): void { - this._ws?.close(); + private _onOpen(rtt: boolean): void { + console.log(`[NTSvelte] ${rtt ? `RTT ` : ``}WebSocket Connected`); + + if (this._ws?.readyState === WebSocket.OPEN && this._wsRtt?.readyState === WebSocket.OPEN) { + if (this._openTimeout !== null) clearTimeout(this._openTimeout); + this._openTimeout = null; + this._aliveAck = true; + this._state.set(NTSvelteClientState.CONNECTED); + console.log(`[NTSvelte] Connection complete`); + + this._sendTimestamp(); + + if (this._topicsOnlySubuid !== null) this.subAllNoValues(); + + this._subscribers.forEach((subscriber) => { + if (subscriber.getSubuid() === null) this._sendJSON(subscriber.subscribe(this._genUID())); + }); + + this._publishers.forEach((publisher) => { + if (publisher.getPubuid() === null) this._sendJSON(publisher.publish(this._genUID())); + }); + } } - private _onMessage(event: MessageEvent): void { - if (typeof event.data === `string`) { - this._byteLengthCounter += event.data.length; - let data: Array<{ method: string; params: any }> = JSON.parse(event.data); - if (!Array.isArray(data)) { - console.warn(`[NT] Ignoring Text: top level array not found`); - return; - } + private _onClose(event: CloseEvent, rtt: boolean): void { + this._restart( + `${rtt ? `RTT ` : ``}WebSocket Closed with code ${event.code}: ${event.reason?.length ? event.reason : `Unknown reason`}`, + ); + } - data.forEach((msg, i) => { - if (typeof msg !== `object`) { - console.warn(`[NT] Ignoring Text Parameter: index ${i} of parsed array is not an object`); - return; - } + private _onError(rtt: boolean): void { + this._restart(`${rtt ? `RTT ` : ``}Websocket encountered an error`); + } - if (typeof msg.method !== `string` || typeof msg.params !== `object`) { - console.warn(`[NT] Ignoring Text Parameter: index ${i} of parsed array schema mismatch`); - return; - } + private _onMessage(event: MessageEvent, rtt: boolean): void { + const now = Date.now() * 1000; - switch (msg.method) { + if (typeof event.data === `string`) { + if (rtt) return console.warn(`[NTSvelte] Received unexpected text frame from RTT connection`); + + this._usage += this._getStringSize(event.data); + let data: NTTextFrame[] = JSON.parse(event.data); + if (!Array.isArray(data)) return console.warn(`[NTSvelte] Received malformed text frame, data is not an array`); + + data.forEach((frame, i) => { + if (typeof frame !== `object`) + return console.warn(`[NTSvelte] Received malformed text frame, frame at index ${i} is not an object`); + + const method = frame.method; + const params = frame.params; + if (typeof method !== `string`) + return console.warn( + `[NTSvelte] Received malformed text frame, frame at index ${i} has invalid method of type ${typeof method}`, + ); + if (typeof params !== `object`) + return console.warn( + `[NTSvelte] Received malformed text frame, frame at index ${i} has invalid params of type ${typeof params}`, + ); + + switch (method) { case `announce`: - const aName: string = msg.params.name; - const aId: number = msg.params.id; - const aType: NTTypeString = msg.params.type; - - const existing = this._topics.get(aName); - if (existing) { - existing.setId(aId); - existing.setType(aType); - } else { - this._topics.set(aName, new NTTopic(aName, aId, aType)); + this._serverTopics.set( + new Map( + get(this._serverTopics).set( + params.name, + this._deepFreeze({ + id: params.id, + name: params.name, + type: params.type, + pubuid: params.pubuid, + properties: params.properties ?? {}, + }), + ), + ), + ); + + this._subscribers.get(params.name)?.onAnnounce(params.id); + if (typeof params.pubuid === `number`) { + const pub = this._publishers.get(params.name); + if (pub?.getPubuid() === params.pubuid && !pub.getAck()) { + pub.onAck(); + const frame = pub.getBinaryFrame(this._getServerTimeUs()); + if (Array.isArray(frame)) this._sendMessagePack(frame, false); + } } - - this._topicMap.update((last) => [...last, [aName, aType]]); break; case `unannounce`: - const unName: string = msg.params.name; - - const topic = this._topics.get(unName); - if (topic) { - topic.setId(null); - topic.setType(null); - topic.setValue(null, this._serverOffset !== null ? this._serverOffset + Date.now() * 1000 : null); - this._topicMap.update((last) => last.filter((v) => v[0] !== unName)); - } else { - console.warn(`[NT] Ignoring Text Parameter: Unannounced unknown topic ID of ${msg.params.id as number}`); - } + const topics = get(this._serverTopics); + topics.delete(params.name); + this._serverTopics.set(new Map(topics)); + + this._subscribers.get(params.name)?.onUnannounce(); break; case `properties`: + const topic = get(this._serverTopics).get(params.name); + if (!topic) return console.warn(`[NTSvelte] Ignoring properties update, topic was not announced`); + + const properties = structuredClone(topic.properties); + for (const [k, v] of Object.entries((params.update ?? {}) as NTTopic[`properties`])) { + if (v !== null) properties[k] = v; + else delete properties[k]; + } + + this._serverTopics.set( + new Map(get(this._serverTopics).set(params.name, this._deepFreeze({ ...topic, properties }))), + ); break; default: - console.warn(`[NT] Ignoring Text Parameter: index ${i} of parsed array has unknown method`); + console.warn(`[NTSvelte] Received unknown method "${method}" in text frame at index ${i}`); break; } }); - } else { + } else if (event.data instanceof ArrayBuffer) { + this._usage += event.data.byteLength * 8; + MessagePack.deserialize(event.data, { multiple: true }).forEach((unpacked: any[]) => { const topicId: number = unpacked[0]; const timestamp: number = unpacked[1]; - const value = unpacked[3]; + const type: (typeof NTTypeCodes)[NTTypeString] = unpacked[2]; + const value: NTType = unpacked[3]; + + if (typeof topicId !== `number`) + return console.warn(`[NTSvelte] Received malformed binary frame, topic ID is not a number`); + if (typeof timestamp !== `number`) + return console.warn(`[NTSvelte] Received malformed binary frame, timestamp is not a number`); + if (typeof type !== `number`) return console.warn(`[NTSvelte] Received malformed binary frame, type index is not a number`); if (topicId === -1) { - const localTime = Date.now() * 1000; - const offset = localTime - value; - this._serverOffset = timestamp + offset - localTime; - - const latency = (offset / 2 / 1000).toFixed(2); - this._serverTime = (this._serverOffset + localTime) / 1000000; - this._latencyStore.set(latency); - console.log(`[NT] Server time is ${this._serverTime.toFixed(3)}s with ${latency}ms latency`); - } else if (topicId >= 0) { - for (const [_, topic] of this._topics) { - if (topic.getId() === topicId) { - topic.setValue(value, timestamp); + this._aliveAck = true; + const usLatency = (now - (value as number)) / 2; + const serverTimeUs = timestamp + usLatency; + this._latency.set(usLatency / 1000); + this._serverTime.set(serverTimeUs / 1000); + this._serverOffsetUs = serverTimeUs - now; + return; + } + + if (rtt) return console.warn(`[NTSvelte] Received unexpected binary frame with topic ID ${topicId} on RTT connection`); + + if (topicId >= 0) { + let found = false; + this._subscribers.forEach((subscriber) => { + if (subscriber.getTopicId() === topicId) { + subscriber.updateValue(value, timestamp / 1000); + found = true; } - } + }); + if (!found) console.warn(`[NTSvelte] Received binary frame with unknown topic ID ${topicId}`); } else { - console.warn(`[NT] Ignoring Binary Data: Invalid topic ID of ${topicId}`); + return console.warn(`[NTSvelte] Received malformed binary frame, topic ID "${topicId}" out of range`); } }); - - this._byteLengthCounter += event.data.byteLength; + } else { + console.warn(`[NTSvelte] Received unknown message type`); } } - private _getSubuid(): number { - this._subuidNonce++; - return this._subuidNonce; + /** + * Generates a new UID. + */ + private _genUID(): number { + return this._uidNonce++; } - private _getPubuid() { - return Math.floor(Math.random() * 99999999) + 10000; + /** + * Gets the number of bits in a string. + * @param str The string. + */ + private _getStringSize(str: string): number { + return (encodeURI(str).split(/%..|./).length - 1) * 8; + } + + /** + * Deep freezes an object in place. + * Supports circular references. + * @param obj The object to freeze. + * @returns The object. + */ + private _deepFreeze>(obj: T): T { + Object.freeze(obj); + Object.values(obj) + .filter((o) => typeof o === `object` && !Object.isFrozen(o)) + .forEach((o) => this._deepFreeze(o)); + return obj; } } diff --git a/dashboard/src/ntStores.ts b/dashboard/src/ntStores.ts index 8e08472..f78df61 100644 --- a/dashboard/src/ntStores.ts +++ b/dashboard/src/ntStores.ts @@ -1,31 +1,20 @@ -import { derived } from "svelte/store"; import { DEFAULT_URI } from "./constants"; -import { NTSvelte } from "./lib/NTSvelte"; +import { NTSvelteClient } from "./lib/NTSvelte"; -export const nt = new NTSvelte(`GRRDashboard`, DEFAULT_URI, 40); +export const nt = new NTSvelteClient(DEFAULT_URI, { appName: `GRRDashboard` }); -export const NTURI = nt.getURIStore(); -export const NTConnected = nt.getConnectionStore(); -export const NTBitrate = nt.getBitrateStore(); -export const NTLatency = nt.getLatencyStore(); -export const NTTopicMap = nt.getTopicMapStore(); -export const NTTopicMapHierarchy = nt.getTopicMapHierarchyStore(); +export const NTURI = nt.uriReadable(); +export const NTConnected = nt.stateReadable(); +export const NTBitrate = nt.bitrateReadable(); +export const NTLatency = nt.latencyReadable(); -export const RobotEnabled = nt.createSubscription(`/GRRDashboard/Robot/enabled`, false); -export const RobotMatchTime = nt.createSubscription(`/GRRDashboard/Robot/matchTime`, 0); -export const RobotBlueAlliance = nt.createSubscription(`/GRRDashboard/Robot/blueAlliance`, true); -export const RobotVoltage = nt.createSubscription(`/GRRDashboard/Robot/voltage`, 0); +export const RobotEnabled = nt.subscribe(`/GRRDashboard/Robot/enabled`, false); +export const RobotMatchTime = nt.subscribe(`/GRRDashboard/Robot/matchTime`, 0); +export const RobotBlueAlliance = nt.subscribe(`/GRRDashboard/Robot/blueAlliance`, true); +export const RobotVoltage = nt.subscribe(`/GRRDashboard/Robot/voltage`, 0); -export const AutosActive = nt.createSubscription(`/GRRDashboard/Autos/active`, ``); -export const AutosOptions = nt.createSubscription(`/GRRDashboard/Autos/options`, []); -export const AutosSelected = nt.createPublisher(`/GRRDashboard/Autos/selected`, `string`, ``); - -export const FieldRobot = nt.createSubscription<[number, number, number]>(`/GRRDashboard/Subsystems/Swerve/Field/Robot`, [0, 0, 0]); -export const FieldModules = derived( - nt.createSubscription(`/GRRDashboard/Subsystems/Swerve/Field/Modules`, new Array(12).fill(-1)), - ($v) => { - return $v?.map((_, i) => (i % 3 === 0 ? $v.slice(i, i + 3) : null)).filter((v) => v) ?? ([] as number[][]); - }, -); +export const AutosActive = nt.subscribe(`/GRRDashboard/Autos/active`, ``); +export const AutosOptions = nt.subscribe(`/GRRDashboard/Autos/options`, []); +export const AutosSelected = nt.publish(`/GRRDashboard/Autos/selected`, `string`, ``); nt.connect(); diff --git a/dashboard/src/tabs/Analysis.svelte b/dashboard/src/tabs/Analysis.svelte deleted file mode 100644 index 7a088c3..0000000 --- a/dashboard/src/tabs/Analysis.svelte +++ /dev/null @@ -1,133 +0,0 @@ - - -
- -
-
- -
-
-
- - -
- - diff --git a/dashboard/src/tabs/AutoSelection.svelte b/dashboard/src/tabs/AutoSelection.svelte index dae294c..b6a7be0 100644 --- a/dashboard/src/tabs/AutoSelection.svelte +++ b/dashboard/src/tabs/AutoSelection.svelte @@ -1,54 +1,81 @@
- {#if options.length} - {#each options as { label, splines, raw }} + {#if $options.length} + {#each $options as { id, label, points }, i}
diff --git a/dashboard/tsconfig.json b/dashboard/tsconfig.json index 2e0efd1..be3a172 100644 --- a/dashboard/tsconfig.json +++ b/dashboard/tsconfig.json @@ -6,8 +6,8 @@ "useDefineForClassFields": true, "module": "ESNext", "checkJs": true, - "isolatedModules": true + "isolatedModules": true, }, "include": ["src/**/*.d.ts", "src/**/*.ts", "src/**/*.js", "src/**/*.svelte"], - "references": [{ "path": "./tsconfig.node.json" }] + "references": [{ "path": "./tsconfig.node.json" }], } diff --git a/src/main/deploy/choreo/README.md b/src/main/deploy/choreo/README.md index b335b71..a55b8e9 100644 --- a/src/main/deploy/choreo/README.md +++ b/src/main/deploy/choreo/README.md @@ -1,3 +1,3 @@ ## deploy/choreo -Place trajectory files generated by [Choreo](https://github.com/SleipnirGroup/Choreo) in this directory. +This directory contains trajectory files generated by [Choreo](https://github.com/SleipnirGroup/Choreo). diff --git a/src/main/java/com/choreo/lib/Choreo.java b/src/main/java/com/choreo/lib/Choreo.java new file mode 100644 index 0000000..dd0944f --- /dev/null +++ b/src/main/java/com/choreo/lib/Choreo.java @@ -0,0 +1,217 @@ +// Copyright (c) Choreo contributors +// From Choreolib@https://github.com/SleipnirGroup/Choreo/commit/09b94f7b24969b4ca3d910439f0be1fb562834d5 + +package com.choreo.lib; + +import com.google.gson.Gson; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; +import java.io.BufferedReader; +import java.io.File; +import java.io.FileReader; +import java.util.ArrayList; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import java.util.function.Supplier; + +/** Utilities to load and follow ChoreoTrajectories */ +public class Choreo { + + private static final Gson gson = new Gson(); + + /** Default constructor. */ + public Choreo() {} + + /** + * Load a trajectory from the deploy directory. Choreolib expects .traj files to be placed in + * src/main/deploy/choreo/[trajName].traj . + * + * @param trajName the path name in Choreo, which matches the file name in the deploy directory. + * Do not include ".traj" here. + * @return the loaded trajectory, or null if the trajectory could not be loaded. + */ + public static ChoreoTrajectory getTrajectory(String trajName) { + var traj_dir = new File(Filesystem.getDeployDirectory(), "choreo"); + var traj_file = new File(traj_dir, trajName + ".traj"); + + return loadFile(traj_file); + } + + /** + * Loads the split parts of the specified trajectory. Fails and returns null if any of the parts + * could not be loaded. + * + *

This method determines the number of parts to load by counting the files that match the + * pattern "trajName.X.traj", where X is a string of digits. Let this count be N. It then attempts + * to load "trajName.1.traj" through "trajName.N.traj", consecutively counting up. If any of these + * files cannot be loaded, the method returns null. + * + * @param trajName The path name in Choreo for this trajectory. + * @return The ArrayList of segments, in order, or null. + */ + public static ArrayList getTrajectoryGroup(String trajName) { + // Count files matching the pattern for split parts. + var traj_dir = new File(Filesystem.getDeployDirectory(), "choreo"); + File[] files = traj_dir.listFiles(file -> file.getName().matches(trajName + "\\.\\d+\\.traj")); + int segmentCount = files.length; + // Try to load the segments. + var trajs = new ArrayList(); + for (int i = 1; i <= segmentCount; ++i) { + File traj = new File(traj_dir, String.format("%s.%d.traj", trajName, i)); + ChoreoTrajectory trajectory = loadFile(traj); + if (trajectory == null) { + DriverStation.reportError("ChoreoLib: Missing segments for path group " + trajName, false); + return null; + } + trajs.add(trajectory); + } + + return trajs; + } + + private static ChoreoTrajectory loadFile(File path) { + try { + var reader = new BufferedReader(new FileReader(path)); + ChoreoTrajectory traj = gson.fromJson(reader, ChoreoTrajectory.class); + + return traj; + } catch (Exception ex) { + DriverStation.reportError(ex.getMessage(), ex.getStackTrace()); + } + return null; + } + + /** + * Create a command to follow a Choreo path. + * + * @param trajectory The trajectory to follow. Use Choreo.getTrajectory(String trajName) to load + * this from the deploy directory. + * @param poseSupplier A function that returns the current field-relative pose of the robot. + * @param xController A PIDController for field-relative X translation (input: X error in meters, + * output: m/s). + * @param yController A PIDController for field-relative Y translation (input: Y error in meters, + * output: m/s). + * @param rotationController A PIDController for robot rotation (input: heading error in radians, + * output: rad/s). This controller will have its continuous input range set to -pi..pi by + * ChoreoLib. + * @param outputChassisSpeeds A function that consumes the target robot-relative chassis speeds + * and commands them to the robot. + * @param mirrorTrajectory If this returns true, the path will be mirrored to the opposite side, + * while keeping the same coordinate system origin. This will be called every loop during the + * command. + * @param requirements The subsystem(s) to require, typically your drive subsystem only. + * @return A command that follows a Choreo path. + */ + public static Command choreoSwerveCommand( + ChoreoTrajectory trajectory, + Supplier poseSupplier, + PIDController xController, + PIDController yController, + PIDController rotationController, + Consumer outputChassisSpeeds, + BooleanSupplier mirrorTrajectory, + Subsystem... requirements + ) { + return choreoSwerveCommand( + trajectory, + poseSupplier, + choreoSwerveController(xController, yController, rotationController), + outputChassisSpeeds, + mirrorTrajectory, + requirements + ); + } + + /** + * Create a command to follow a Choreo path. + * + * @param trajectory The trajectory to follow. Use Choreo.getTrajectory(String trajName) to load + * this from the deploy directory. + * @param poseSupplier A function that returns the current field-relative pose of the robot. + * @param controller A ChoreoControlFunction to follow the current trajectory state. Use + * ChoreoCommands.choreoSwerveController(PIDController xController, PIDController yController, + * PIDController rotationController) to create one using PID controllers for each degree of + * freedom. You can also pass in a function with the signature (Pose2d currentPose, + * ChoreoTrajectoryState referenceState) -> ChassisSpeeds to implement a custom follower + * (i.e. for logging). + * @param outputChassisSpeeds A function that consumes the target robot-relative chassis speeds + * and commands them to the robot. + * @param mirrorTrajectory If this returns true, the path will be mirrored to the opposite side, + * while keeping the same coordinate system origin. This will be called every loop during the + * command. + * @param requirements The subsystem(s) to require, typically your drive subsystem only. + * @return A command that follows a Choreo path. + */ + public static Command choreoSwerveCommand( + ChoreoTrajectory trajectory, + Supplier poseSupplier, + ChoreoControlFunction controller, + Consumer outputChassisSpeeds, + BooleanSupplier mirrorTrajectory, + Subsystem... requirements + ) { + var timer = new Timer(); + return new FunctionalCommand( + timer::restart, + () -> { + outputChassisSpeeds.accept( + controller.apply(poseSupplier.get(), trajectory.sample(timer.get(), mirrorTrajectory.getAsBoolean())) + ); + }, + interrupted -> { + timer.stop(); + if (interrupted) { + outputChassisSpeeds.accept(new ChassisSpeeds()); + } else { + outputChassisSpeeds.accept(trajectory.getFinalState().getChassisSpeeds()); + } + }, + () -> timer.hasElapsed(trajectory.getTotalTime()), + requirements + ); + } + + /** + * Creates a control function for following a ChoreoTrajectoryState. + * + * @param xController A PIDController for field-relative X translation (input: X error in meters, + * output: m/s). + * @param yController A PIDController for field-relative Y translation (input: Y error in meters, + * output: m/s). + * @param rotationController A PIDController for robot rotation (input: heading error in radians, + * output: rad/s). This controller will have its continuous input range set to -pi..pi by + * ChoreoLib. + * @return A ChoreoControlFunction to track ChoreoTrajectoryStates. This function returns + * robot-relative ChassisSpeeds. + */ + public static ChoreoControlFunction choreoSwerveController( + PIDController xController, + PIDController yController, + PIDController rotationController + ) { + rotationController.enableContinuousInput(-Math.PI, Math.PI); + return (pose, referenceState) -> { + double xFF = referenceState.velocityX; + double yFF = referenceState.velocityY; + double rotationFF = referenceState.angularVelocity; + + double xFeedback = xController.calculate(pose.getX(), referenceState.x); + double yFeedback = yController.calculate(pose.getY(), referenceState.y); + double rotationFeedback = rotationController.calculate(pose.getRotation().getRadians(), referenceState.heading); + + return ChassisSpeeds.fromFieldRelativeSpeeds( + xFF + xFeedback, + yFF + yFeedback, + rotationFF + rotationFeedback, + pose.getRotation() + ); + }; + } +} diff --git a/src/main/java/com/choreo/lib/ChoreoControlFunction.java b/src/main/java/com/choreo/lib/ChoreoControlFunction.java new file mode 100644 index 0000000..3495c5c --- /dev/null +++ b/src/main/java/com/choreo/lib/ChoreoControlFunction.java @@ -0,0 +1,15 @@ +// Copyright (c) Choreo contributors +// From Choreolib@https://github.com/SleipnirGroup/Choreo/commit/09b94f7b24969b4ca3d910439f0be1fb562834d5 + +package com.choreo.lib; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import java.util.function.BiFunction; + +/** + * This interface exists as a type alias. A ChoreoControlFunction has signature (Pose2d currentPose, + * ChoreoTrajectoryState referenceState)->ChassisSpeeds, where the function returns + * robot-relative ChassisSpeeds for the robot. + */ +public interface ChoreoControlFunction extends BiFunction {} diff --git a/src/main/java/com/choreo/lib/ChoreoTrajectory.java b/src/main/java/com/choreo/lib/ChoreoTrajectory.java new file mode 100644 index 0000000..bd473c2 --- /dev/null +++ b/src/main/java/com/choreo/lib/ChoreoTrajectory.java @@ -0,0 +1,161 @@ +// Copyright (c) Choreo contributors +// From Choreolib@https://github.com/SleipnirGroup/Choreo/commit/09b94f7b24969b4ca3d910439f0be1fb562834d5 + +package com.choreo.lib; + +import edu.wpi.first.math.geometry.Pose2d; +import java.util.ArrayList; +import java.util.List; + +/** A trajectory loaded from Choreo. */ +public class ChoreoTrajectory { + + private final List samples; + + /** Create an empty ChoreoTrajectory. */ + public ChoreoTrajectory() { + samples = List.of(); + } + + /** + * Constructs a new trajectory from a list of trajectory states + * + * @param samples a vector containing a list of ChoreoTrajectoryStates + */ + public ChoreoTrajectory(List samples) { + this.samples = samples; + } + + /** + * Returns the first ChoreoTrajectoryState in the trajectory. + * + * @return The first ChoreoTrajectoryState in the trajectory. + */ + public ChoreoTrajectoryState getInitialState() { + return samples.get(0); + } + + /** + * Returns the last ChoreoTrajectoryState in the trajectory. + * + * @return The last ChoreoTrajectoryState in the trajectory. + */ + public ChoreoTrajectoryState getFinalState() { + return samples.get(samples.size() - 1); + } + + private ChoreoTrajectoryState sampleInternal(double timestamp) { + if (timestamp < samples.get(0).timestamp) { + return samples.get(0); + } + if (timestamp >= getTotalTime()) { + return samples.get(samples.size() - 1); + } + + int low = 0; + int high = samples.size() - 1; + + while (low != high) { + int mid = (low + high) / 2; + if (samples.get(mid).timestamp < timestamp) { + low = mid + 1; + } else { + high = mid; + } + } + + if (low == 0) { + return samples.get(low); + } + + var behindState = samples.get(low - 1); + var aheadState = samples.get(low); + + if ((aheadState.timestamp - behindState.timestamp) < 1e-6) { + return aheadState; + } + + return behindState.interpolate(aheadState, timestamp); + } + + /** + * Return an interpolated, non-mirrored sample of the trajectory at the given timestamp. + * + * @param timestamp The timestamp of this sample relative to the beginning of the trajectory. + * @return The ChoreoTrajectoryState at the given time. + */ + public ChoreoTrajectoryState sample(double timestamp) { + return sample(timestamp, false); + } + + /** + * Return an interpolated sample of the trajectory at the given timestamp. + * + * @param timestamp The timestamp of this sample relative to the beginning of the trajectory. + * @param mirrorForRedAlliance whether or not to return the sample as mirrored across the field + * midline (as in 2023). + * @return The ChoreoTrajectoryState at the given time. + */ + public ChoreoTrajectoryState sample(double timestamp, boolean mirrorForRedAlliance) { + var state = sampleInternal(timestamp); + return mirrorForRedAlliance ? state.flipped() : state; + } + + /** + * Returns the initial, non-mirrored pose of the trajectory. + * + * @return the initial, non-mirrored pose of the trajectory. + */ + public Pose2d getInitialPose() { + return samples.get(0).getPose(); + } + + /** + * Returns the final, non-mirrored pose of the trajectory. + * + * @return the final, non-mirrored pose of the trajectory. + */ + public Pose2d getFinalPose() { + return samples.get(samples.size() - 1).getPose(); + } + + /** + * Returns the total time of the trajectory (the timestamp of the last sample) + * + * @return the total time of the trajectory (the timestamp of the last sample) + */ + public double getTotalTime() { + return samples.get(samples.size() - 1).timestamp; + } + + /** + * Returns the array of poses corresponding to the trajectory. + * + * @return the array of poses corresponding to the trajectory. + */ + public Pose2d[] getPoses() { + return samples.stream().map(ChoreoTrajectoryState::getPose).toArray(Pose2d[]::new); + } + + /** + * Returns the array of samples corresponding to the trajectory. + * + * @return the array of samples corresponding to the trajectory. + */ + public ChoreoTrajectoryState[] getStates() { + return samples.stream().toArray(ChoreoTrajectoryState[]::new); + } + + /** + * Returns this trajectory, mirrored across the field midline. + * + * @return this trajectory, mirrored across the field midline. + */ + public ChoreoTrajectory flipped() { + var flippedStates = new ArrayList(); + for (var state : samples) { + flippedStates.add(state.flipped()); + } + return new ChoreoTrajectory(flippedStates); + } +} diff --git a/src/main/java/com/choreo/lib/ChoreoTrajectoryState.java b/src/main/java/com/choreo/lib/ChoreoTrajectoryState.java new file mode 100644 index 0000000..d3b70c8 --- /dev/null +++ b/src/main/java/com/choreo/lib/ChoreoTrajectoryState.java @@ -0,0 +1,135 @@ +// Copyright (c) Choreo contributors +// From Choreolib@https://github.com/SleipnirGroup/Choreo/commit/09b94f7b24969b4ca3d910439f0be1fb562834d5 + +package com.choreo.lib; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** A single robot state in a ChoreoTrajectory. */ +public class ChoreoTrajectoryState implements Interpolatable { + + private static final double FIELD_WIDTH_METERS = 16.55445; + + /** The timestamp of this state, relative to the beginning of the trajectory. */ + public final double timestamp; + + /** The X position of the state in meters. */ + public final double x; + + /** The Y position of the state in meters. */ + public final double y; + + /** The heading of the state in radians, with 0 being in the +X direction. */ + public final double heading; + + /** The velocity of the state in the X direction in m/s. */ + public final double velocityX; + + /** The velocity of the state in the X direction in m/s. */ + public final double velocityY; + + /** The angular velocity of the state in rad/s. */ + public final double angularVelocity; + + /** + * Constructs a ChoreoTrajectoryState with the specified parameters. + * + * @param timestamp The timestamp of this state, relative to the beginning of the trajectory. + * @param x The X position of the state in meters. + * @param y The Y position of the state in meters. + * @param heading The heading of the state in radians, with 0 being in the +X direction. + * @param velocityX The velocity of the state in the X direction in m/s. + * @param velocityY The velocity of the state in the X direction in m/s. + * @param angularVelocity The angular velocity of the state in rad/s. + */ + public ChoreoTrajectoryState( + double timestamp, + double x, + double y, + double heading, + double velocityX, + double velocityY, + double angularVelocity + ) { + this.timestamp = timestamp; + this.x = x; + this.y = y; + this.heading = heading; + this.velocityX = velocityX; + this.velocityY = velocityY; + this.angularVelocity = angularVelocity; + } + + /** + * Returns the pose at this state. + * + * @return the pose at this state. + */ + public Pose2d getPose() { + return new Pose2d(x, y, Rotation2d.fromRadians(heading)); + } + + /** + * Returns the field-relative chassis speeds of this state. + * + * @return the field-relative chassis speeds of this state. + */ + public ChassisSpeeds getChassisSpeeds() { + return new ChassisSpeeds(velocityX, velocityY, angularVelocity); + } + + /** + * Interpolate between this state and the provided state. + * + * @param endValue The next state. It should have a timestamp after this state. + * @param t the timestamp of the interpolated state. It should be between this state and endValue. + * @return the interpolated state. + */ + @Override + public ChoreoTrajectoryState interpolate(ChoreoTrajectoryState endValue, double t) { + double scale = (t - this.timestamp) / (endValue.timestamp - this.timestamp); + var interp_pose = getPose().interpolate(endValue.getPose(), scale); + + return new ChoreoTrajectoryState( + MathUtil.interpolate(this.timestamp, endValue.timestamp, scale), + interp_pose.getX(), + interp_pose.getY(), + interp_pose.getRotation().getRadians(), + MathUtil.interpolate(this.velocityX, endValue.velocityX, scale), + MathUtil.interpolate(this.velocityY, endValue.velocityY, scale), + MathUtil.interpolate(this.angularVelocity, endValue.angularVelocity, scale) + ); + } + + /** + * Returns this state as a double array: {timestamp, x, y, heading, velocityX, velocityY, + * angularVelocity}. + * + * @return This state as a double array: {timestamp, x, y, heading, velocityX, velocityY, + * angularVelocity}. + */ + public double[] asArray() { + return new double[] { timestamp, x, y, heading, velocityX, velocityY, angularVelocity }; + } + + /** + * Returns this state, mirrored across the field midline. + * + * @return this state, mirrored across the field midline. + */ + public ChoreoTrajectoryState flipped() { + return new ChoreoTrajectoryState( + this.timestamp, + FIELD_WIDTH_METERS - this.x, + this.y, + Math.PI - this.heading, + -this.velocityX, + this.velocityY, + -this.angularVelocity + ); + } +} diff --git a/src/main/java/org/team340/lib/GRRDashboard.java b/src/main/java/org/team340/lib/GRRDashboard.java index 788b9a1..a43ca23 100644 --- a/src/main/java/org/team340/lib/GRRDashboard.java +++ b/src/main/java/org/team340/lib/GRRDashboard.java @@ -1,5 +1,8 @@ package org.team340.lib; +import com.choreo.lib.ChoreoTrajectory; +import com.choreo.lib.ChoreoTrajectoryState; +import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.util.sendable.Sendable; @@ -12,13 +15,14 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Watchdog; import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import java.util.ArrayList; import java.util.HashMap; +import java.util.LinkedHashMap; import java.util.List; import java.util.Map; +import java.util.UUID; import java.util.concurrent.locks.ReentrantLock; import java.util.function.Supplier; import org.team340.lib.controller.Controller2; @@ -51,7 +55,7 @@ private GRRDashboard() { /** * The auto chooser. */ - private static final SendableChooser autoChooser = new SendableChooser<>(); + private static final AutoChooserSendable autoChooser = new AutoChooserSendable(); /** * The notifier that runs telemetry updates on a separate thread. */ @@ -95,10 +99,7 @@ private static synchronized void init(TimedRobot robot, double telemetryPeriod, initialized = true; publish("Robot", new RobotSendable()); - - autoChooser.setDefaultOption(serializeAuto("Do Nothing"), Commands.none().withName("Do Nothing")); publish("Autos", autoChooser); - publish("SystemsCheck", Commands.none().withName("SystemsCheck")); watchdog.setTimeout(telemetryPeriod); @@ -159,17 +160,37 @@ public static void addCommand(String label, Command command) { * @param command The command to add to the dashboard. */ public static void addAutoCommand(String label, Command command) { - addAutoCommand(label, command, ""); + addAutoCommand(label, new ChoreoTrajectory(), command); + } + + /** + * Adds an auto command to the dashboard. + * @param label The label for the command. + * @param trajectory The trajectory used by the auto. + * @param command The command to add to the dashboard. + */ + public static void addAutoCommand(String label, ChoreoTrajectory trajectory, Command command) { + addAutoCommand(label, new ChoreoTrajectory[] { trajectory }, command); + } + + /** + * Adds an auto command to the dashboard. + * @param label The label for the command. + * @param trajectories Trajectories used by the auto. + * @param command The command to add to the dashboard. + */ + public static void addAutoCommand(String label, List trajectories, Command command) { + autoChooser.addOption(label, trajectories.stream().toArray(ChoreoTrajectory[]::new), command); } /** * Adds an auto command to the dashboard. * @param label The label for the command. + * @param trajectories Trajectories used by the auto. * @param command The command to add to the dashboard. - * @param trajFile The name of trajectory file generated by Choreo used by the command. */ - public static void addAutoCommand(String label, Command command, String trajFile) { - autoChooser.addOption(serializeAuto(label), command.withName(label)); + public static void addAutoCommand(String label, ChoreoTrajectory[] trajectories, Command command) { + autoChooser.addOption(label, trajectories, command); } /** @@ -261,45 +282,116 @@ private static void reportOverrun() { } /** - * Serializes an auto into JSON. - * @param label The label for the command. + * A sendable for the auto chooser. */ - private static String serializeAuto(String label) { - // TODO - - // List> parsedSplines = new ArrayList<>(); - - // if (!pathFile.isEmpty()) { - // try { - // JSONArray rawWaypoints = (JSONArray) ( - // (JSONObject) new JSONParser() - // .parse(new FileReader(new File(Filesystem.getDeployDirectory(), "choreo/" + trajFile))) - // ).get("waypoints"); - - // if (rawWaypoints != null) { - // for (int i = 0; i < rawWaypoints.size(); i++) { - // JSONObject rawWaypoint = (JSONObject) rawWaypoints.get(i); - // JSONObject nextRawWaypoint = i < rawWaypoints.size() - 1 ? (JSONObject) rawWaypoints.get(i + 1) : null; - // if (rawWaypoint == null) continue; - - // parsedSplines.add( - // new HashMap<>() { - // { - // put("p0", (JSONObject) rawWaypoint.get("anchorPoint")); - // put("p1", (JSONObject) rawWaypoint.get("nextControl")); - // put("p2", nextRawWaypoint != null ? (JSONObject) nextRawWaypoint.get("prevControl") : null); - // put("p3", nextRawWaypoint != null ? (JSONObject) nextRawWaypoint.get("anchorPoint") : null); - // } - // } - // ); - // } - // } - // } catch (Exception e) { - // e.printStackTrace(); - // } - // } - - return "{ \"label\": " + label + ", \"splines\": [] }"; + private static class AutoChooserSendable implements Sendable { + + private final ReentrantLock selectedMutex = new ReentrantLock(); + + private String defaultChoice; + private String selected; + private Command choice = Commands.none(); + private Map> options = new LinkedHashMap<>(); // { [id]: [json, command] } + + /** + * Creates the auto chooser. Automatically adds a default option that does nothing. + */ + public AutoChooserSendable() { + defaultChoice = addOption("Do Nothing", new ChoreoTrajectory[] { new ChoreoTrajectory() }, Commands.none()); + } + + /** + * Gets the command of the selected auto. + */ + public Command getSelected() { + return choice; + } + + /** + * Adds an option. + * @param label The option's label. + * @param trajectories Trajectories used by the auto. + * @param command The option's command. + * @return The option's ID. + */ + public String addOption(String label, ChoreoTrajectory[] trajectories, Command command) { + String id = UUID.randomUUID().toString(); + String json = ""; + + List points = new ArrayList<>(); + double lastTimestamp = 0.0; + + for (int i = 0; i < trajectories.length; i++) { + ChoreoTrajectoryState[] states = trajectories[i].getStates(); + if (i > 0 && trajectories[i - 1].getStates().length > 0) lastTimestamp += trajectories[i - 1].getFinalState().timestamp; + for (ChoreoTrajectoryState state : states) { + points.add(new double[] { state.x, state.y, state.heading, state.timestamp + lastTimestamp }); + } + } + + ChoreoTrajectory lastTrajectory = trajectories.length > 0 ? trajectories[trajectories.length - 1] : new ChoreoTrajectory(); + double time = lastTimestamp + (lastTrajectory.getStates().length > 0 ? lastTrajectory.getFinalState().timestamp : 0.0); + + try { + json = + new ObjectMapper() + .writeValueAsString( + new HashMap<>() { + { + put("id", id); + put("label", label); + put("points", points); + put("time", time); + } + } + ); + } catch (Exception e) { + e.printStackTrace(); + json = ""; + } + + if (json.isEmpty()) json = "{ \"id\": \"" + id + "\", \"label\": \"" + label + "\", \"points\": [] }"; + options.put(id, Map.entry(json, command)); + return id; + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.addStringProperty("default", () -> defaultChoice, null); + builder.addStringArrayProperty( + "options", + () -> options.values().stream().map(entry -> entry.getKey()).toArray(String[]::new), + null + ); + builder.addStringProperty( + "active", + () -> { + selectedMutex.lock(); + try { + if (selected != null) return selected; else return defaultChoice; + } finally { + selectedMutex.unlock(); + } + }, + null + ); + builder.addStringProperty( + "selected", + null, + (String value) -> { + selectedMutex.lock(); + try { + Map.Entry entry = options.get(value); + if (entry != null) { + selected = value; + choice = entry.getValue(); + } + } finally { + selectedMutex.unlock(); + } + } + ); + } } /** @@ -307,6 +399,7 @@ private static String serializeAuto(String label) { */ private static class RobotSendable implements Sendable { + @Override public void initSendable(SendableBuilder builder) { builder.addBooleanProperty("blueAlliance", () -> DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue), null); builder.addDoubleProperty("cpuTemperature", () -> Math2.toFixed(RobotController.getCPUTemp()), null); diff --git a/src/main/java/org/team340/lib/swerve/SwerveBase.java b/src/main/java/org/team340/lib/swerve/SwerveBase.java index ff43d17..eeed6ae 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveBase.java +++ b/src/main/java/org/team340/lib/swerve/SwerveBase.java @@ -1,9 +1,15 @@ package org.team340.lib.swerve; +import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder; +import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; @@ -16,12 +22,25 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.ADIS16470_IMU; +import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; +import java.util.ArrayDeque; import java.util.ArrayList; +import java.util.Deque; +import java.util.Iterator; import java.util.List; +import java.util.concurrent.locks.ReentrantLock; import org.team340.lib.GRRDashboard; import org.team340.lib.GRRSubsystem; import org.team340.lib.blacklight.Blacklight; @@ -111,6 +130,17 @@ public static enum SwerveEncoderType { protected final SwerveDrivePoseEstimator poseEstimator; protected final List blacklights = new ArrayList<>(); + protected final Notifier odometryThread; + protected final ReentrantLock odometryMutex = new ReentrantLock(); + protected final Deque timestampQueue = new ArrayDeque<>(); + protected final Deque imuYawQueue = new ArrayDeque<>(); + + protected final MutableMeasure sysIdAppliedVoltage = mutable(Volts.of(0)); + protected final MutableMeasure sysIdDistance = mutable(Meters.of(0)); + protected final MutableMeasure> sysIdVelocity = mutable(MetersPerSecond.of(0)); + + protected final SysIdRoutine sysIdRoutine; + /** * Create the GRRSwerve subsystem. * @param label The label for the subsystem. Shown in the dashboard. @@ -154,6 +184,35 @@ public SwerveBase(String label, SwerveConfig config) { blacklights.add(blacklight); } + odometryThread = new Notifier(this::sampleOdometry); + odometryThread.setName("Swerve Odometry"); + odometryThread.startPeriodic(config.getOdometryPeriod()); + + sysIdRoutine = + new SysIdRoutine( + // Default config, rampRate is 1V/sec, stepVoltage is 7V, and timeout is 10secs + config.getSysIdConfig(), + new Mechanism( + // Defines how drive command should be sent to motors + (Measure volts) -> { + driveVoltage(volts.in(Volts), Math2.ROTATION2D_0); + }, + log -> { + for (SwerveModule module : modules) { + log + .motor("module-" + StringUtil.toCamelCase(module.getLabel())) + .voltage( + sysIdAppliedVoltage.mut_replace(module.getMoveDutyCycle() * RobotController.getBatteryVoltage(), Volts) + ) + .linearPosition(sysIdDistance.mut_replace(module.getDistance(), Meters)) + .linearVelocity(sysIdVelocity.mut_replace(module.getVelocity(), MetersPerSecond)); + } + }, + this, + "Swerve" + ) + ); + imu.setZero(Math2.ROTATION2D_0); System.out.println( @@ -248,6 +307,22 @@ protected Pose2d getPosition() { return poseEstimator.getEstimatedPosition(); } + /** + * Samples a timestamp, IMU yaw, and module positions to odometry sample queues. + */ + protected void sampleOdometry() { + try { + odometryMutex.lock(); + timestampQueue.add(MathSharedStore.getTimestamp()); + imuYawQueue.add(imu.getYaw()); + for (SwerveModule module : modules) { + module.recordSample(); + } + } finally { + odometryMutex.unlock(); + } + } + /** * Resets odometry. * @param newPose The new pose. @@ -262,10 +337,36 @@ protected void resetOdometry(Pose2d newPose) { * Should be ran periodically. */ protected void updateOdometry() { - for (Blacklight blacklight : blacklights) blacklight.update(poseEstimator); - SwerveModulePosition[] modulePositions = getModulePositions(); - Pose2d newPose = poseEstimator.update(imu.getYaw(), modulePositions); - field.update(newPose, modulePositions); + try { + odometryMutex.lock(); + for (Blacklight blacklight : blacklights) blacklight.update(poseEstimator); + Iterator timestampIterator = timestampQueue.iterator(); + while (timestampIterator.hasNext()) { + SwerveModulePosition[] modulePositions = new SwerveModulePosition[modules.length]; + boolean missingModule = false; + for (int i = 0; i < modulePositions.length; i++) { + double distance = modules[i].pollDistanceSample(); + double heading = modules[i].pollHeadingSample(); + if (distance == Double.MIN_VALUE || heading == Double.MIN_VALUE) { + missingModule = true; + continue; + } + modulePositions[i] = new SwerveModulePosition(distance, new Rotation2d(heading)); + } + double timestamp = timestampIterator.next(); + if (missingModule) continue; + try { + Rotation2d yaw = imuYawQueue.pop(); + poseEstimator.updateWithTime(timestamp, yaw, modulePositions); + } catch (Exception e) { + continue; + } + } + timestampQueue.clear(); + field.update(getPosition(), getModulePositions()); + } finally { + odometryMutex.unlock(); + } } /** diff --git a/src/main/java/org/team340/lib/swerve/SwerveModule.java b/src/main/java/org/team340/lib/swerve/SwerveModule.java index 8016ab4..31056b5 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveModule.java +++ b/src/main/java/org/team340/lib/swerve/SwerveModule.java @@ -6,6 +6,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; +import java.util.ArrayDeque; +import java.util.Deque; import org.team340.lib.swerve.config.SwerveConfig; import org.team340.lib.swerve.config.SwerveModuleConfig; import org.team340.lib.swerve.hardware.encoders.SwerveEncoder; @@ -25,6 +27,9 @@ class SwerveModule { private final SimpleMotorFeedforward moveFFController; private final Timer controlTimer = new Timer(); + private final Deque distanceQueue = new ArrayDeque<>(); + private final Deque headingQueue = new ArrayDeque<>(); + private double lastMoveSpeed = 0.0; private double simDistance = 0.0; private double simHeading = 0.0; @@ -83,6 +88,17 @@ public double getDistance() { } } + /** + * Gets current duty cycle of move motor. + */ + public double getMoveDutyCycle() { + if (RobotBase.isSimulation()) { + return (simVelocity / config.getMaxV()) * 12.0; + } else { + return moveMotor.getDutyCycle(); + } + } + /** * Gets the heading of the swerve module in radians. */ @@ -168,4 +184,34 @@ public void setVoltage(double voltage, Rotation2d heading) { lastMoveSpeed = RobotBase.isSimulation() ? simVelocity : getVelocity(); controlTimer.restart(); } + + /** + * Adds the module's current distance and heading to their respective queues for odometry. + */ + public void recordSample() { + distanceQueue.add(getDistance()); + headingQueue.add(getHeading()); + } + + /** + * Polls first distance value in the odometry sample queue. + */ + public double pollDistanceSample() { + try { + return distanceQueue.pop(); + } catch (Exception e) { + return Double.MIN_VALUE; + } + } + + /** + * Polls first heading value in the odometry sample queue. + */ + public double pollHeadingSample() { + try { + return headingQueue.pop(); + } catch (Exception e) { + return Double.MIN_VALUE; + } + } } diff --git a/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java b/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java index 34863d0..886e179 100644 --- a/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java +++ b/src/main/java/org/team340/lib/swerve/config/SwerveConfig.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj.ADIS16470_IMU; import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import java.util.ArrayList; import java.util.List; import java.util.MissingResourceException; @@ -13,8 +14,6 @@ import org.team340.lib.util.config.FeedForwardConfig; import org.team340.lib.util.config.PIDConfig; -// TODO Documentation (startup and tuning) - /** * Config builder for {@link SwerveBase}. */ @@ -41,7 +40,9 @@ public class SwerveConfig { private SwerveMotorType moveMotorType; private SwerveMotorType turnMotorType; private double discretizationLookahead = -1.0; + private double odometryPeriod = -1.0; private double[] standardDeviations; + private Config sysIdConfig = null; private double fieldLength = -1.0; private double fieldWidth = -1.0; private List modules = new ArrayList<>(); @@ -375,6 +376,22 @@ public double getDiscretizationLookahead() { return discretizationLookahead; } + /** + * Sets period in seconds between odometry samples. + * @param odometryPeriod Period in seconds. + */ + public SwerveConfig setOdometryPeriod(double odometryPeriod) { + this.odometryPeriod = odometryPeriod; + return this; + } + + /** + * Gets period in seconds between odometry samples. + */ + public double getOdometryPeriod() { + return odometryPeriod; + } + /** * Sets the standard deviations for pose estimation from module odometry. * A good starting configuration is all axis with a magnitude of {@code 0.1}. @@ -394,6 +411,21 @@ public double[] getStandardDeviations() { return standardDeviations; } + /** + * Sets config for SysId. + */ + public SwerveConfig setSysIdConfig(Config sysIdConfig) { + this.sysIdConfig = sysIdConfig; + return this; + } + + /** + * Gets config for SysId. + */ + public Config getSysIdConfig() { + return sysIdConfig; + } + /** * Sets the field size. * @param fieldLength The field's length in meters. Typically {@code 16.5417}. @@ -503,7 +535,9 @@ public void verify() { if (moveMotorType == null) throwMissing("Move Motor Type"); if (turnMotorType == null) throwMissing("Turn Motor Type"); if (discretizationLookahead == -1) throwMissing("Discretization Lookahead"); + if (odometryPeriod == -1) throwMissing("Odometry Period"); if (standardDeviations == null) throwMissing("Standard Deviations"); + if (sysIdConfig == null) throwMissing("SysId Config"); if (fieldLength == -1) throwMissing("Field Length"); if (fieldWidth == -1) throwMissing("Field Width"); if (modules.size() == 0) throwMissing("Modules"); diff --git a/src/main/java/org/team340/lib/swerve/hardware/encoders/vendors/SwerveCANcoder.java b/src/main/java/org/team340/lib/swerve/hardware/encoders/vendors/SwerveCANcoder.java index a2d186f..66eff3f 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/encoders/vendors/SwerveCANcoder.java +++ b/src/main/java/org/team340/lib/swerve/hardware/encoders/vendors/SwerveCANcoder.java @@ -26,8 +26,9 @@ public class SwerveCANcoder implements SwerveEncoder { public SwerveCANcoder(CANcoder canCoder, SwerveConfig config, SwerveModuleConfig moduleConfig) { absolutePositionSignal = canCoder.getAbsolutePosition(); - double hz = 1.0 / config.getPeriod(); + double hz = 1.0 / config.getOdometryPeriod(); absolutePositionSignal.setUpdateFrequency(hz); + canCoder.optimizeBusUtilization(); CANcoderConfiguration canCoderConfig = new CANcoderConfiguration(); diff --git a/src/main/java/org/team340/lib/swerve/hardware/imu/vendors/SwervePigeon2.java b/src/main/java/org/team340/lib/swerve/hardware/imu/vendors/SwervePigeon2.java index 70b0eac..84145cd 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/imu/vendors/SwervePigeon2.java +++ b/src/main/java/org/team340/lib/swerve/hardware/imu/vendors/SwervePigeon2.java @@ -1,5 +1,6 @@ package org.team340.lib.swerve.hardware.imu.vendors; +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; @@ -28,10 +29,9 @@ public SwervePigeon2(Pigeon2 pigeon2, SwerveConfig config) { pitchSignal = pigeon2.getPitch(); rollSignal = pigeon2.getRoll(); - double hz = 1.0 / config.getPeriod(); - yawSignal.setUpdateFrequency(hz); - pitchSignal.setUpdateFrequency(hz); - rollSignal.setUpdateFrequency(hz); + double hz = 1.0 / config.getOdometryPeriod(); + BaseStatusSignal.setUpdateFrequencyForAll(hz, yawSignal, pitchSignal, rollSignal); + pigeon2.optimizeBusUtilization(); } @Override diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/SwerveMotor.java b/src/main/java/org/team340/lib/swerve/hardware/motors/SwerveMotor.java index 819b645..ebfdca1 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/motors/SwerveMotor.java +++ b/src/main/java/org/team340/lib/swerve/hardware/motors/SwerveMotor.java @@ -15,6 +15,11 @@ public interface SwerveMotor { */ public abstract double getPosition(); + /** + * Gets the motor's applied duty cycle. + */ + public abstract double getDutyCycle(); + /** * Sets the motor's closed loop target. * If the motor is a move motor, the target is in meters/second. diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java index 99adfff..383c6b6 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java +++ b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkFlex.java @@ -56,6 +56,7 @@ public SwerveSparkFlex( SwerveConversions conversions = new SwerveConversions(config); int periodMs = (int) (config.getPeriod() * 1000.0); + int periodOdometryMs = (int) (config.getOdometryPeriod() * 1000.0); boolean usingAttachedEncoder = SwerveEncoderType.SPARK_ENCODER.equals(moduleConfig.getEncoderType()); double conversionFactor = 1.0 / (isMoveMotor ? conversions.moveRotationsPerMeter() : conversions.turnRotationsPerRadian()); PIDConfig pidConfig = isMoveMotor ? config.getMovePID() : config.getTurnPID(); @@ -72,11 +73,11 @@ public SwerveSparkFlex( .setOpenLoopRampRate(isMoveMotor ? config.getMoveRampRate() : config.getTurnRampRate()) .setClosedLoopRampRate(isMoveMotor ? config.getMoveRampRate() : config.getTurnRampRate()) .setPeriodicFramePeriod(Frame.S0, periodMs) - .setPeriodicFramePeriod(Frame.S1, periodMs) - .setPeriodicFramePeriod(Frame.S2, periodMs) + .setPeriodicFramePeriod(Frame.S1, periodOdometryMs) + .setPeriodicFramePeriod(Frame.S2, periodOdometryMs) .setPeriodicFramePeriod(Frame.S3, 10000) - .setPeriodicFramePeriod(Frame.S4, usingAttachedEncoder ? periodMs : 10000) - .setPeriodicFramePeriod(Frame.S5, usingAttachedEncoder ? periodMs : 10000) + .setPeriodicFramePeriod(Frame.S4, usingAttachedEncoder ? periodOdometryMs : 10000) + .setPeriodicFramePeriod(Frame.S5, usingAttachedEncoder ? periodOdometryMs : 10000) .apply(sparkFlex); new SparkPIDControllerConfig() @@ -113,6 +114,11 @@ public double getPosition() { return relativeEncoder.getPosition(); } + @Override + public double getDutyCycle() { + return sparkFlex.getAppliedOutput(); + } + @Override public void setReference(double target, double ff) { pidController.setReference( diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java index 2142d9d..2d6e0fa 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java +++ b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveSparkMax.java @@ -56,6 +56,7 @@ public SwerveSparkMax( SwerveConversions conversions = new SwerveConversions(config); int periodMs = (int) (config.getPeriod() * 1000.0); + int periodOdometryMs = (int) (config.getOdometryPeriod() * 1000.0); boolean usingAttachedEncoder = SwerveEncoderType.SPARK_ENCODER.equals(moduleConfig.getEncoderType()); double conversionFactor = 1.0 / (isMoveMotor ? conversions.moveRotationsPerMeter() : conversions.turnRotationsPerRadian()); PIDConfig pidConfig = isMoveMotor ? config.getMovePID() : config.getTurnPID(); @@ -72,11 +73,11 @@ public SwerveSparkMax( .setOpenLoopRampRate(isMoveMotor ? config.getMoveRampRate() : config.getTurnRampRate()) .setClosedLoopRampRate(isMoveMotor ? config.getMoveRampRate() : config.getTurnRampRate()) .setPeriodicFramePeriod(Frame.S0, periodMs) - .setPeriodicFramePeriod(Frame.S1, periodMs) - .setPeriodicFramePeriod(Frame.S2, periodMs) + .setPeriodicFramePeriod(Frame.S1, periodOdometryMs) + .setPeriodicFramePeriod(Frame.S2, periodOdometryMs) .setPeriodicFramePeriod(Frame.S3, 10000) - .setPeriodicFramePeriod(Frame.S4, usingAttachedEncoder ? periodMs : 10000) - .setPeriodicFramePeriod(Frame.S5, usingAttachedEncoder ? periodMs : 10000) + .setPeriodicFramePeriod(Frame.S4, usingAttachedEncoder ? periodOdometryMs : 10000) + .setPeriodicFramePeriod(Frame.S5, usingAttachedEncoder ? periodOdometryMs : 10000) .apply(sparkMax); new SparkPIDControllerConfig() @@ -113,6 +114,11 @@ public double getPosition() { return relativeEncoder.getPosition(); } + @Override + public double getDutyCycle() { + return sparkMax.getAppliedOutput(); + } + @Override public void setReference(double target, double ff) { pidController.setReference( diff --git a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveTalonFX.java b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveTalonFX.java index 8c0120c..ecf99ef 100644 --- a/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveTalonFX.java +++ b/src/main/java/org/team340/lib/swerve/hardware/motors/vendors/SwerveTalonFX.java @@ -1,5 +1,6 @@ package org.team340.lib.swerve.hardware.motors.vendors; +import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; @@ -27,6 +28,7 @@ public class SwerveTalonFX implements SwerveMotor { private final boolean isMoveMotor; private final StatusSignal velocitySignal; private final StatusSignal positionSignal; + private final StatusSignal dutyCycleSignal; private final double conversionFactor; /** @@ -71,10 +73,11 @@ public SwerveTalonFX(boolean isMoveMotor, TalonFX talonFX, SwerveConfig config, velocitySignal = talonFX.getVelocity(); positionSignal = talonFX.getPosition(); + dutyCycleSignal = talonFX.getDutyCycle(); - double hz = 1.0 / config.getPeriod(); - velocitySignal.setUpdateFrequency(hz); - positionSignal.setUpdateFrequency(hz); + double hz = 1.0 / config.getOdometryPeriod(); + BaseStatusSignal.setUpdateFrequencyForAll(hz, velocitySignal, positionSignal, dutyCycleSignal); + talonFX.optimizeBusUtilization(); talonFX.clearStickyFaults(); talonFX.getConfigurator().apply(fxConfig); @@ -93,6 +96,11 @@ public double getPosition() { return positionSignal.refresh().getValue() / conversionFactor; } + @Override + public double getDutyCycle() { + return dutyCycleSignal.refresh().getValue(); + } + @Override public void setReference(double target, double ff) { if (isMoveMotor) { diff --git a/src/main/java/org/team340/lib/util/SendableFactory.java b/src/main/java/org/team340/lib/util/SendableFactory.java index e1ada10..432654f 100644 --- a/src/main/java/org/team340/lib/util/SendableFactory.java +++ b/src/main/java/org/team340/lib/util/SendableFactory.java @@ -30,6 +30,7 @@ public SendableImpl(Consumer initSendable) { initSendableConsumer = initSendable; } + @Override public void initSendable(SendableBuilder builder) { initSendableConsumer.accept(builder); } diff --git a/src/main/java/org/team340/lib/util/config/rev/AbsoluteEncoderConfig.java b/src/main/java/org/team340/lib/util/config/rev/AbsoluteEncoderConfig.java index 22d2f37..a26faef 100644 --- a/src/main/java/org/team340/lib/util/config/rev/AbsoluteEncoderConfig.java +++ b/src/main/java/org/team340/lib/util/config/rev/AbsoluteEncoderConfig.java @@ -41,6 +41,9 @@ public void apply(CANSparkMax sparkMax, AbsoluteEncoder absoluteEncoder) { RevConfigUtils.burnFlashSleep(); return sparkMax.burnFlash(); }, + ae -> true, + false, + 1, "Burn Flash" ); super.applySteps(absoluteEncoder, "Spark Max (ID " + sparkMax.getDeviceId() + ") Absolute Encoder"); @@ -57,6 +60,9 @@ public void apply(CANSparkFlex sparkFlex, AbsoluteEncoder absoluteEncoder) { RevConfigUtils.burnFlashSleep(); return sparkFlex.burnFlash(); }, + ae -> true, + false, + 1, "Burn Flash" ); super.applySteps(absoluteEncoder, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") Absolute Encoder"); diff --git a/src/main/java/org/team340/lib/util/config/rev/RelativeEncoderConfig.java b/src/main/java/org/team340/lib/util/config/rev/RelativeEncoderConfig.java index 408a64b..ab9091b 100644 --- a/src/main/java/org/team340/lib/util/config/rev/RelativeEncoderConfig.java +++ b/src/main/java/org/team340/lib/util/config/rev/RelativeEncoderConfig.java @@ -37,10 +37,13 @@ public RelativeEncoderConfig clone() { */ public void apply(CANSparkMax sparkMax, RelativeEncoder relativeEncoder) { addStep( - ae -> { + re -> { RevConfigUtils.burnFlashSleep(); return sparkMax.burnFlash(); }, + re -> true, + false, + 1, "Burn Flash" ); super.applySteps(relativeEncoder, "Spark Max (ID " + sparkMax.getDeviceId() + ") Relative Encoder"); @@ -53,10 +56,13 @@ public void apply(CANSparkMax sparkMax, RelativeEncoder relativeEncoder) { */ public void apply(CANSparkFlex sparkFlex, RelativeEncoder relativeEncoder) { addStep( - ae -> { + re -> { RevConfigUtils.burnFlashSleep(); return sparkFlex.burnFlash(); }, + re -> true, + false, + 1, "Burn Flash" ); super.applySteps(relativeEncoder, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") Relative Encoder"); diff --git a/src/main/java/org/team340/lib/util/config/rev/RevConfigBase.java b/src/main/java/org/team340/lib/util/config/rev/RevConfigBase.java index 06c90ea..bb9cd9a 100644 --- a/src/main/java/org/team340/lib/util/config/rev/RevConfigBase.java +++ b/src/main/java/org/team340/lib/util/config/rev/RevConfigBase.java @@ -16,6 +16,7 @@ private static final record RevConfigStep( Function applier, Function checker, boolean trustCheck, + int setIterations, String name ) { public RevConfigStep(Function applier, String name) { @@ -25,6 +26,10 @@ public RevConfigStep(Function applier, String name) { public RevConfigStep(Function applier, Function checker, String name) { this(applier, checker, true, name); } + + public RevConfigStep(Function applier, Function checker, boolean trustCheck, String name) { + this(applier, checker, trustCheck, RevConfigUtils.DEFAULT_SET_ITERATIONS, name); + } } final List> configSteps = new ArrayList<>(); @@ -65,6 +70,13 @@ void addStep(Function applier, Function checker, boo configSteps.add(new RevConfigStep(applier, checker, trustCheck, name)); } + /** + * Stores a configuration step. + */ + void addStep(Function applier, Function checker, boolean trustCheck, int setIterations, String name) { + configSteps.add(new RevConfigStep(applier, checker, trustCheck, setIterations, name)); + } + /** * Applies the config. * @param hardware The hardware to apply the config to. @@ -72,7 +84,7 @@ void addStep(Function applier, Function checker, boo */ void applySteps(T hardware, String identifier) { for (RevConfigStep step : configSteps) { - applyStep(hardware, identifier, step, new String[RevConfigUtils.SET_ITERATIONS], RevConfigUtils.SET_ITERATIONS); + applyStep(hardware, identifier, step, new String[RevConfigUtils.DEFAULT_SET_ITERATIONS], RevConfigUtils.DEFAULT_SET_ITERATIONS); } } @@ -111,23 +123,19 @@ private void applyStep(T hardware, String identifier, RevConfigStep step, Str iterationsLeft--; if (iterationsLeft <= 0) { String resultsString = ""; - boolean hadFailure = false; + boolean hadSuccess = false; for (int i = 0; i < results.length; i++) { if ( - !(results[i].equals(REVLibError.kOk.name()) || results[i].equals("Skipped")) && - !(RobotBase.isSimulation() && results[i].startsWith(REVLibError.kParamMismatchType.name())) - ) hadFailure = true; + results[i].equals(REVLibError.kOk.name()) || + results[i].equals("Skipped") || + (RobotBase.isSimulation() && results[i].startsWith(REVLibError.kParamMismatchType.name())) + ) hadSuccess = true; resultsString += results[i]; if (i != results.length - 1) resultsString += ", "; } - if (hadFailure) { - DriverStation.reportWarning( - "Failure(s) encountered while configuring \"" + step.name() + "\" on " + identifier + ": " + resultsString, - true - ); - } else { - RevConfigUtils.addSuccess(identifier + " \"" + step.name() + "\": " + resultsString); + if (!hadSuccess) { + RevConfigUtils.addError(identifier + " \"" + step.name() + "\": " + resultsString); } } else { applyStep(hardware, identifier, step, results, iterationsLeft); diff --git a/src/main/java/org/team340/lib/util/config/rev/RevConfigUtils.java b/src/main/java/org/team340/lib/util/config/rev/RevConfigUtils.java index d770761..adf12c0 100644 --- a/src/main/java/org/team340/lib/util/config/rev/RevConfigUtils.java +++ b/src/main/java/org/team340/lib/util/config/rev/RevConfigUtils.java @@ -1,5 +1,6 @@ package org.team340.lib.util.config.rev; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import java.text.Collator; import java.util.Collection; @@ -12,11 +13,11 @@ public final class RevConfigUtils { public static final double EPSILON = 1e-4; - public static final double BURN_FLASH_SLEEP = 250.0; - public static final double CHECK_SLEEP = 25; - public static final int SET_ITERATIONS = 3; + public static final double BURN_FLASH_SLEEP = 100.0; + public static final double CHECK_SLEEP = 25.0; + public static final int DEFAULT_SET_ITERATIONS = 3; - private static final Collection success = new TreeSet<>(SuccessComparator.getInstance()); + private static final Collection errors = new TreeSet<>(ErrorComparator.getInstance()); private RevConfigUtils() { throw new UnsupportedOperationException("This is a utility class!"); @@ -34,37 +35,42 @@ static void burnFlashSleep() { } /** - * Saves a configuration success string to be logged. - * @param successString The success string. + * Saves a configuration error string to be logged. + * @param errorString The error string. */ - static void addSuccess(String successString) { - success.add(successString); + static void addError(String errorString) { + errors.add(errorString); } /** - * Prints successful configurations to stdout. + * Prints unsuccessful configurations to stdout. * Useful for debugging, should be ran after initializing all hardware. */ - public static void printSuccess() { - System.out.println("\nSuccessfully configured " + success.size() + " options on REV hardware:"); - for (String successString : success) { - System.out.println("\t" + successString); + public static void printError() { + if (errors.size() <= 0) { + System.out.println("\nAll REV hardware configured successfully\n"); + } else { + DriverStation.reportWarning("\nErrors while configuring " + errors.size() + " options on REV hardware:", false); + + for (String errorString : errors) { + DriverStation.reportWarning("\t" + errorString, false); + } + DriverStation.reportWarning("\n", false); + errors.clear(); } - System.out.println("\n"); - success.clear(); } - private static final class SuccessComparator implements Comparator { + private static final class ErrorComparator implements Comparator { - private static SuccessComparator instance; + private static ErrorComparator instance; private final Collator localeComparator = Collator.getInstance(); - public static SuccessComparator getInstance() { - if (instance == null) instance = new SuccessComparator(); + public static ErrorComparator getInstance() { + if (instance == null) instance = new ErrorComparator(); return instance; } - private SuccessComparator() {} + private ErrorComparator() {} @Override public int compare(String arg0, String arg1) { diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkFlexConfig.java b/src/main/java/org/team340/lib/util/config/rev/SparkFlexConfig.java index 09bac40..38a7996 100644 --- a/src/main/java/org/team340/lib/util/config/rev/SparkFlexConfig.java +++ b/src/main/java/org/team340/lib/util/config/rev/SparkFlexConfig.java @@ -11,7 +11,7 @@ */ public final class SparkFlexConfig extends RevConfigBase { - private static final double FACTORY_DEFAULTS_SLEEP = 50.0; + private static final double FACTORY_DEFAULTS_SLEEP = 25.0; /** * Creates an empty config. @@ -39,10 +39,13 @@ public SparkFlexConfig clone() { */ public void apply(CANSparkFlex sparkFlex) { addStep( - sm -> { + sf -> { RevConfigUtils.burnFlashSleep(); - return sm.burnFlash(); + return sf.burnFlash(); }, + sf -> true, + false, + 1, "Burn Flash" ); super.applySteps(sparkFlex, "Spark Flex (ID " + sparkFlex.getDeviceId() + ")"); diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkLimitSwitchConfig.java b/src/main/java/org/team340/lib/util/config/rev/SparkLimitSwitchConfig.java index 8f74469..cbd3d2b 100644 --- a/src/main/java/org/team340/lib/util/config/rev/SparkLimitSwitchConfig.java +++ b/src/main/java/org/team340/lib/util/config/rev/SparkLimitSwitchConfig.java @@ -36,10 +36,13 @@ public SparkLimitSwitchConfig clone() { */ public void apply(CANSparkMax sparkMax, SparkLimitSwitch limitSwitch) { addStep( - ae -> { + ls -> { RevConfigUtils.burnFlashSleep(); return sparkMax.burnFlash(); }, + ls -> true, + false, + 1, "Burn Flash" ); super.applySteps(limitSwitch, "Spark Max (ID " + sparkMax.getDeviceId() + ") Limit Switch"); @@ -52,10 +55,13 @@ public void apply(CANSparkMax sparkMax, SparkLimitSwitch limitSwitch) { */ public void apply(CANSparkFlex sparkFlex, SparkLimitSwitch limitSwitch) { addStep( - ae -> { + ls -> { RevConfigUtils.burnFlashSleep(); return sparkFlex.burnFlash(); }, + ls -> true, + false, + 1, "Burn Flash" ); super.applySteps(limitSwitch, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") Limit Switch"); diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java b/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java index a2b2a71..34c3e30 100644 --- a/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java +++ b/src/main/java/org/team340/lib/util/config/rev/SparkMaxConfig.java @@ -43,6 +43,9 @@ public void apply(CANSparkMax sparkMax) { RevConfigUtils.burnFlashSleep(); return sm.burnFlash(); }, + sm -> true, + false, + 1, "Burn Flash" ); super.applySteps(sparkMax, "Spark Max (ID " + sparkMax.getDeviceId() + ")"); diff --git a/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java b/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java index 8842b7c..847399e 100644 --- a/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java +++ b/src/main/java/org/team340/lib/util/config/rev/SparkPIDControllerConfig.java @@ -5,6 +5,8 @@ import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.SparkPIDController; import org.team340.lib.util.Math2; +import org.team340.lib.util.config.PIDConfig; +import org.team340.lib.util.config.PIDFConfig; /** * Config builder for {@link SparkPIDController}. @@ -42,6 +44,9 @@ public void apply(CANSparkMax sparkMax, SparkPIDController pidController) { RevConfigUtils.burnFlashSleep(); return sparkMax.burnFlash(); }, + pc -> true, + false, + 1, "Burn Flash" ); super.applySteps(pidController, "Spark Max (ID " + sparkMax.getDeviceId() + ") PID Controller"); @@ -58,6 +63,9 @@ public void apply(CANSparkFlex sparkFlex, SparkPIDController pidController) { RevConfigUtils.burnFlashSleep(); return sparkFlex.burnFlash(); }, + pc -> true, + false, + 1, "Burn Flash" ); super.applySteps(pidController, "Spark Flex (ID " + sparkFlex.getDeviceId() + ") PID Controller"); @@ -351,6 +359,27 @@ public SparkPIDControllerConfig setPID(double pGain, double iGain, double dGain, return this; } + /** + * Sets PIDF gains on the Spark Max. + * @param pidConfig The PID config object to apply. + */ + public SparkPIDControllerConfig setPID(PIDConfig pidfConfig) { + setPID(pidfConfig.p(), pidfConfig.i(), pidfConfig.d()); + setIZone(pidfConfig.iZone()); + return this; + } + + /** + * Sets PIDF gains on the Spark Max. + * @param pidConfig The PID config object to apply. + * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + */ + public SparkPIDControllerConfig setPID(PIDConfig pidfConfig, int slotId) { + setPID(pidfConfig.p(), pidfConfig.i(), pidfConfig.d(), slotId); + setIZone(pidfConfig.iZone(), slotId); + return this; + } + /** * Sets PIDF gains on the Spark Max. * @param pGain The proportional gain value, must be positive. @@ -381,4 +410,25 @@ public SparkPIDControllerConfig setPIDF(double pGain, double iGain, double dGain setFF(ffGain, slotId); return this; } + + /** + * Sets PIDF gains on the Spark Max. + * @param pidfConfig The PIDF config object to apply. + */ + public SparkPIDControllerConfig setPIDF(PIDFConfig pidfConfig) { + setPIDF(pidfConfig.p(), pidfConfig.i(), pidfConfig.d(), pidfConfig.ff()); + setIZone(pidfConfig.iZone()); + return this; + } + + /** + * Sets PIDF gains on the Spark Max. + * @param pidfConfig The PIDF config object to apply. + * @param slotId The gain schedule slot, the value is a number between {@code 0.0} and {@code 3}. + */ + public SparkPIDControllerConfig setPIDF(PIDFConfig pidfConfig, int slotId) { + setPIDF(pidfConfig.p(), pidfConfig.i(), pidfConfig.d(), pidfConfig.ff(), slotId); + setIZone(pidfConfig.iZone(), slotId); + return this; + } } diff --git a/src/main/java/org/team340/robot/Constants.java b/src/main/java/org/team340/robot/Constants.java index 9a7db40..ff65545 100644 --- a/src/main/java/org/team340/robot/Constants.java +++ b/src/main/java/org/team340/robot/Constants.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.ADIS16470_IMU.CalibrationTime; import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.SPI.Port; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import org.team340.lib.controller.Controller2Config; import org.team340.lib.swerve.SwerveBase.SwerveMotorType; import org.team340.lib.swerve.config.SwerveConfig; @@ -180,7 +181,9 @@ public static final class SwerveConstants { .setSpeedConstraints(5.0, 10.0, 17.5, 30.0) .setMotorTypes(SwerveMotorType.SPARK_FLEX_BRUSHLESS, SwerveMotorType.SPARK_FLEX_BRUSHLESS) .setDiscretizationLookahead(0.020) + .setOdometryPeriod(0.004) .setStandardDeviations(0.1, 0.1, 0.1) + .setSysIdConfig(new SysIdRoutine.Config()) .setFieldSize(FIELD_LENGTH, FIELD_WIDTH) .addModule(FRONT_LEFT) .addModule(BACK_LEFT) diff --git a/src/main/java/org/team340/robot/RobotContainer.java b/src/main/java/org/team340/robot/RobotContainer.java index 1ab1e0f..77bdcb0 100644 --- a/src/main/java/org/team340/robot/RobotContainer.java +++ b/src/main/java/org/team340/robot/RobotContainer.java @@ -2,15 +2,11 @@ import static edu.wpi.first.wpilibj2.command.Commands.*; -import edu.wpi.first.wpilibj.XboxController.Axis; -import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import org.team340.lib.GRRDashboard; import org.team340.lib.controller.Controller2; -import org.team340.lib.controller.JoystickProfiler; import org.team340.lib.util.Math2; import org.team340.lib.util.config.rev.RevConfigUtils; import org.team340.robot.Constants.ControllerConstants; -import org.team340.robot.commands.Autos; import org.team340.robot.commands.SystemsCheck; import org.team340.robot.subsystems.Intake; import org.team340.robot.subsystems.Shooter; @@ -28,9 +24,9 @@ private RobotContainer() { private static Controller2 driver; private static Controller2 coDriver; - public static Swerve swerve; public static Intake intake; public static Shooter shooter; + public static Swerve swerve; /** * Entry to initializing subsystems and command execution. @@ -45,20 +41,18 @@ public static void init() { coDriver.addToDashboard(); // Initialize subsystems. - swerve = new Swerve(); intake = new Intake(); shooter = new Shooter(); + swerve = new Swerve(); // Add subsystems to the dashboard. swerve.addToDashboard(); - intake.addToDashboard(); - shooter.addToDashboard(); // Set systems check command. GRRDashboard.setSystemsCheck(SystemsCheck.command()); - // Print successful REV hardware initialization. - RevConfigUtils.printSuccess(); + // Print errors from REV hardware initialization. + RevConfigUtils.printError(); // Configure bindings and autos. configBindings(); @@ -92,39 +86,13 @@ private static void configBindings() { // A => Do nothing coDriver.a().onTrue(none()); - - /** - * Joystick profiling. - */ - driver - .start() - .and(driver.leftBumper()) - .and(RobotModeTriggers.disabled()) - .whileTrue(JoystickProfiler.run(driver.getHID(), Axis.kLeftX.value, Axis.kLeftY.value, 100)); - driver - .start() - .and(driver.rightBumper()) - .and(RobotModeTriggers.disabled()) - .whileTrue(JoystickProfiler.run(driver.getHID(), Axis.kRightX.value, Axis.kRightY.value, 100)); - coDriver - .start() - .and(coDriver.leftBumper()) - .and(RobotModeTriggers.disabled()) - .whileTrue(JoystickProfiler.run(coDriver.getHID(), Axis.kLeftX.value, Axis.kLeftY.value, 100)); - coDriver - .start() - .and(coDriver.rightBumper()) - .and(RobotModeTriggers.disabled()) - .whileTrue(JoystickProfiler.run(coDriver.getHID(), Axis.kRightX.value, Axis.kRightY.value, 100)); } /** * Autonomous commands should be declared here and * added to {@link GRRDashboard}. */ - private static void configAutos() { - GRRDashboard.addAutoCommand("Example", Autos.example()); - } + private static void configAutos() {} /** * Gets the X axis drive speed from the driver's controller. diff --git a/src/main/java/org/team340/robot/subsystems/Swerve.java b/src/main/java/org/team340/robot/subsystems/Swerve.java index f1071bf..dc61171 100644 --- a/src/main/java/org/team340/robot/subsystems/Swerve.java +++ b/src/main/java/org/team340/robot/subsystems/Swerve.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import java.util.function.Supplier; import org.team340.lib.swerve.SwerveBase; import org.team340.lib.util.Math2; @@ -88,4 +89,20 @@ public Command driveSnap180(Supplier x, Supplier y) { public Command lock() { return commandBuilder("swerve.lock()").onExecute(this::lockWheels); } + + /** + * Runs a SysId quasistatic test. + * @param direction The direction to run the test in. + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction); + } + + /** + * Runs a SysId dynamic test. + * @param direction The direction to run the test in. + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction); + } }