diff --git a/build.gradle b/build.gradle index 2c5a21b..fc18d7e 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" id "idea" } @@ -72,6 +72,8 @@ dependencies { implementation 'org.projectlombok:lombok:1.18.36' annotationProcessor 'org.projectlombok:lombok:1.18.36' + implementation 'com.google.code.gson:gson:2.11.0' + testImplementation platform("org.junit:junit-bom:5.10.1") testImplementation "org.junit.jupiter:junit-jupiter-api" testImplementation "org.junit.jupiter:junit-jupiter-params" diff --git a/ctre_sim/CANCoder vers. H - 012 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 012 - 0 - ext.dat index b5bbc9a..e5e60cc 100644 Binary files a/ctre_sim/CANCoder vers. H - 012 - 0 - ext.dat and b/ctre_sim/CANCoder vers. H - 012 - 0 - ext.dat differ diff --git a/ctre_sim/CANCoder vers. H - 03 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 03 - 0 - ext.dat index e9780cc..142c29d 100644 Binary files a/ctre_sim/CANCoder vers. H - 03 - 0 - ext.dat and b/ctre_sim/CANCoder vers. H - 03 - 0 - ext.dat differ diff --git a/ctre_sim/CANCoder vers. H - 06 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 06 - 0 - ext.dat index c4d7863..3db2e15 100644 Binary files a/ctre_sim/CANCoder vers. H - 06 - 0 - ext.dat and b/ctre_sim/CANCoder vers. H - 06 - 0 - ext.dat differ diff --git a/ctre_sim/CANCoder vers. H - 09 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 09 - 0 - ext.dat index 7500340..f3b2389 100644 Binary files a/ctre_sim/CANCoder vers. H - 09 - 0 - ext.dat and b/ctre_sim/CANCoder vers. H - 09 - 0 - ext.dat differ diff --git a/ctre_sim/Pigeon 2 - 00 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 00 - 0 - ext.dat index 8e078d2..7b090d0 100644 Binary files a/ctre_sim/Pigeon 2 - 00 - 0 - ext.dat and b/ctre_sim/Pigeon 2 - 00 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat index 47dddf1..292d4d8 100644 Binary files a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat index 4aeac8c..84250c9 100644 Binary files a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat index 5475aad..a802c39 100644 Binary files a/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 011 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat index 9cbd467..56d38ad 100644 Binary files a/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat index 7747eed..04fd610 100644 Binary files a/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 016 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 016 - 0 - ext.dat index 77d5687..ac41688 100644 Binary files a/ctre_sim/Talon FX vers. C - 016 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 016 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat index 9638254..abae529 100644 Binary files a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat index 3e1c4b9..ece2580 100644 Binary files a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat index e1663b8..a3048d6 100644 Binary files a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat index 23772e2..ef2eddf 100644 Binary files a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 08 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 08 - 0 - ext.dat index 4f9652d..1cee9c8 100644 Binary files a/ctre_sim/Talon FX vers. C - 08 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 08 - 0 - ext.dat differ diff --git a/logs/sim_2025-02-03_16-51-10.hoot b/logs/sim_2025-02-03_16-51-10.hoot deleted file mode 100644 index 9bbb108..0000000 Binary files a/logs/sim_2025-02-03_16-51-10.hoot and /dev/null differ diff --git a/reefcontrols-site-old/NT4.js b/reefcontrols-site-old/NT4.js new file mode 100644 index 0000000..cd8f09a --- /dev/null +++ b/reefcontrols-site-old/NT4.js @@ -0,0 +1,555 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +import { serialize, deserialize } from "./msgpack.js"; +const typestrIdxLookup = { + boolean: 0, + double: 1, + int: 2, + float: 3, + string: 4, + json: 4, + raw: 5, + rpc: 5, + msgpack: 5, + protobuf: 5, + "boolean[]": 16, + "double[]": 17, + "int[]": 18, + "float[]": 19, + "string[]": 20, +}; +class NT4_Subscription { + uid = -1; + topics = new Set(); + options = new NT4_SubscriptionOptions(); + toSubscribeObj() { + return { + topics: Array.from(this.topics), + subuid: this.uid, + options: this.options.toObj(), + }; + } + toUnsubscribeObj() { + return { + subuid: this.uid, + }; + } +} +class NT4_SubscriptionOptions { + periodic = 0.1; + all = false; + topicsOnly = false; + prefix = false; + toObj() { + return { + periodic: this.periodic, + all: this.all, + topicsonly: this.topicsOnly, + prefix: this.prefix, + }; + } +} +export class NT4_Topic { + uid = -1; // "id" if server topic, "pubuid" if published + name = ""; + type = ""; + properties = {}; + toPublishObj() { + return { + name: this.name, + type: this.type, + pubuid: this.uid, + properties: this.properties, + }; + } + toUnpublishObj() { + return { + pubuid: this.uid, + }; + } + getTypeIdx() { + if (this.type in typestrIdxLookup) { + return typestrIdxLookup[this.type]; + } else { + return 5; // Default to binary + } + } +} +export class NT4_Client { + appName; + onTopicAnnounce; + onTopicUnannounce; + onNewTopicData; + onConnect; + onDisconnect; + serverBaseAddr; + ws = null; + serverAddr = ""; + serverConnectionActive = false; + serverConnectionRequested = false; + serverTimeOffset_us = null; + networkLatency_us = 0; + rxLengthCounter = 0; + subscriptions = new Map(); + publishedTopics = new Map(); + serverTopics = new Map(); + /** + * Creates a new NT4 client without connecting. + * @param serverAddr Network address of NT4 server + * @param appName Identifier for this client (does not need to be unique). + * @param onTopicAnnounce Gets called when server announces enough topics to form a new signal + * @param onTopicUnannounce Gets called when server unannounces any part of a signal + * @param onNewTopicData Gets called when any new data is available + * @param onConnect Gets called once client completes initial handshake with server + * @param onDisconnect Gets called once client detects server has disconnected + */ + constructor( + serverAddr, + appName, + onTopicAnnounce, + onTopicUnannounce, + onNewTopicData, + onConnect, + onDisconnect + ) { + this.serverBaseAddr = serverAddr; + this.appName = appName; + this.onTopicAnnounce = onTopicAnnounce; + this.onTopicUnannounce = onTopicUnannounce; + this.onNewTopicData = onNewTopicData; + this.onConnect = onConnect; + this.onDisconnect = onDisconnect; + setInterval(() => { + // Update timestamp + this.ws_sendTimestamp(); + // Log bitrate + let bitrateKbPerSec = ((this.rxLengthCounter / 1000) * 8) / 5; + this.rxLengthCounter = 0; + console.log( + "[NT4] Bitrate: " + Math.round(bitrateKbPerSec).toString() + " kb/s" + ); + }, 5000); + } + ////////////////////////////////////////////////////////////// + // PUBLIC API + /** Starts the connection. The client will reconnect automatically when disconnected. */ + connect() { + if (!this.serverConnectionRequested) { + this.serverConnectionRequested = true; + this.ws_connect(); + } + } + /** Terminates the connection. */ + disconnect() { + if (this.serverConnectionRequested) { + this.serverConnectionRequested = false; + if (this.serverConnectionActive && this.ws) { + this.ws.close(); + } + } + } + /** + * Add a new subscription, reading value updates + * @param topicPatterns A list of topics or prefixes to include in the subscription. + * @param prefixMode If true, use patterns as prefixes. If false, only subscribe to topics that are an exact match. + * @param sendAll If true, send all values. If false, only send the most recent value. + * @param periodic How frequently to send updates (applies regardless of "sendAll" option) + * @returns A subscription ID that can be used to unsubscribe. + */ + subscribe(topicPatterns, prefixMode, sendAll = false, periodic = 0.1) { + let newSub = new NT4_Subscription(); + newSub.uid = this.getNewUID(); + newSub.topics = new Set(topicPatterns); + newSub.options.prefix = prefixMode; + newSub.options.all = sendAll; + newSub.options.periodic = periodic; + this.subscriptions.set(newSub.uid, newSub); + if (this.serverConnectionActive) { + this.ws_subscribe(newSub); + } + return newSub.uid; + } + /** + * Add a new subscription, reading only topic announcements (not values). + * @param topicPatterns A list of topics or prefixes to include in the subscription. + * @param prefixMode If true, use patterns as prefixes. If false, only subscribe to topics that are an exact match. + * @returns A subscription ID that can be used to unsubscribe. + */ + subscribeTopicsOnly(topicPatterns, prefixMode) { + let newSub = new NT4_Subscription(); + newSub.uid = this.getNewUID(); + newSub.topics = new Set(topicPatterns); + newSub.options.prefix = prefixMode; + newSub.options.topicsOnly = true; + this.subscriptions.set(newSub.uid, newSub); + if (this.serverConnectionActive) { + this.ws_subscribe(newSub); + } + return newSub.uid; + } + /** Given an existing subscription, unsubscribe from it. */ + unsubscribe(subscriptionId) { + let subscription = this.subscriptions.get(subscriptionId); + if (!subscription) { + throw 'Unknown subscription ID "' + subscriptionId + '"'; + } + this.subscriptions.delete(subscriptionId); + if (this.serverConnectionActive) { + this.ws_unsubscribe(subscription); + } + } + /** Unsubscribe from all current subscriptions. */ + clearAllSubscriptions() { + for (const subscriptionId of this.subscriptions.keys()) { + this.unsubscribe(subscriptionId); + } + } + /** + * Set the properties of a particular topic. + * @param topic The topic to update + * @param properties The set of new properties + */ + setProperties(topic, properties) { + // Update local topics + let updateTopic = (toUpdate) => { + for (const key of Object.keys(properties)) { + let value = properties[key]; + if (value === null) { + delete toUpdate.properties[key]; + } else { + toUpdate.properties[key] = value; + } + } + }; + let publishedTopic = this.publishedTopics.get(topic); + if (publishedTopic) updateTopic(publishedTopic); + let serverTopic = this.serverTopics.get(topic); + if (serverTopic) updateTopic(serverTopic); + // Send new properties to server + if (this.serverConnectionActive) { + this.ws_setproperties(topic, properties); + } + } + /** Set whether a topic is persistent. + * + * 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. + */ + setPersistent(topic, isPersistent) { + this.setProperties(topic, { persistent: isPersistent }); + } + /** Set whether a topic is retained. + * + * Topics with this property set to true will not be deleted by + * the server when the last publisher stops publishing. + */ + setRetained(topic, isRetained) { + this.setProperties(topic, { retained: isRetained }); + } + /** Publish a topic from this client with the provided name and type. Can be a new or existing. */ + publishTopic(topic, type) { + if (this.publishedTopics.has(topic)) { + return; + } + let newTopic = new NT4_Topic(); + newTopic.name = topic; + newTopic.uid = this.getNewUID(); + newTopic.type = type; + this.publishedTopics.set(topic, newTopic); + if (this.serverConnectionActive) { + this.ws_publish(newTopic); + } + return; + } + /** Unpublish a previously-published topic from this client. */ + unpublishTopic(topic) { + let topicObj = this.publishedTopics.get(topic); + if (!topicObj) { + throw 'Topic "' + topic + '" not found'; + } + this.publishedTopics.delete(topic); + if (this.serverConnectionActive) { + this.ws_unpublish(topicObj); + } + } + /** Send some new value to the server. The timestamp is whatever the current time is. */ + addSample(topic, value) { + let timestamp = this.getServerTime_us(); + if (timestamp === null) timestamp = 0; + this.addTimestampedSample(topic, timestamp, value); + } + /** Send some new timestamped value to the server. */ + addTimestampedSample(topic, timestamp, value) { + let topicObj = this.publishedTopics.get(topic); + if (!topicObj) { + throw 'Topic "' + topic + '" not found'; + } + let txData = serialize([ + topicObj.uid, + timestamp, + topicObj.getTypeIdx(), + value, + ]); + this.ws_sendBinary(txData); + } + ////////////////////////////////////////////////////////////// + // Server/Client Time Sync Handling + /** Returns the current client time in microseconds. */ + getClientTime_us() { + return new Date().getTime() * 1000; + } + /** Returns the current server time in microseconds (or null if unknown). */ + getServerTime_us(clientTime) { + if (this.serverTimeOffset_us === null) { + return null; + } else { + return ( + (clientTime === undefined ? this.getClientTime_us() : clientTime) + + this.serverTimeOffset_us + ); + } + } + /** Returns the current network latency in microseconds */ + getNetworkLatency_us() { + return this.networkLatency_us; + } + ws_sendTimestamp() { + let timeToSend = this.getClientTime_us(); + let txData = serialize([-1, 0, typestrIdxLookup["int"], timeToSend]); + this.ws_sendBinary(txData); + } + ws_handleReceiveTimestamp(serverTimestamp, clientTimestamp) { + let rxTime = this.getClientTime_us(); + // Recalculate server/client offset based on round trip time + let rtt = rxTime - clientTimestamp; + this.networkLatency_us = rtt / 2.0; + let serverTimeAtRx = serverTimestamp + this.networkLatency_us; + this.serverTimeOffset_us = serverTimeAtRx - rxTime; + console.log( + "[NT4] New server time: " + + (this.getServerTime_us() / 1000000.0).toString() + + "s with " + + (this.networkLatency_us / 1000.0).toString() + + "ms latency" + ); + } + ////////////////////////////////////////////////////////////// + // Websocket Message Send Handlers + ws_subscribe(subscription) { + this.ws_sendJSON("subscribe", subscription.toSubscribeObj()); + } + ws_unsubscribe(subscription) { + this.ws_sendJSON("unsubscribe", subscription.toUnsubscribeObj()); + } + ws_publish(topic) { + this.ws_sendJSON("publish", topic.toPublishObj()); + } + ws_unpublish(topic) { + this.ws_sendJSON("unpublish", topic.toUnpublishObj()); + } + ws_setproperties(topic, newProperties) { + this.ws_sendJSON("setproperties", { + name: topic, + update: newProperties, + }); + } + ws_sendJSON(method, params) { + if (this.ws && this.ws.readyState === WebSocket.OPEN) { + this.ws.send( + JSON.stringify([ + { + method: method, + params: params, + }, + ]) + ); + } + } + ws_sendBinary(data) { + if (this.ws && this.ws.readyState === WebSocket.OPEN) { + this.ws.send(data); + } + } + ////////////////////////////////////////////////////////////// + // Websocket connection Maintenance + ws_onOpen() { + // Set the flag allowing general server communication + this.serverConnectionActive = true; + console.log('[NT4] Connected with identity "' + this.appName + '"'); + // Sync timestamps + this.ws_sendTimestamp(); + // Publish any existing topics + for (const topic of this.publishedTopics.values()) { + this.ws_publish(topic); + } + // Subscribe to existing subscriptions + for (const subscription of this.subscriptions.values()) { + this.ws_subscribe(subscription); + } + // User connection-opened hook + this.onConnect(); + } + ws_onClose(event) { + // Clear flags to stop server communication + this.ws = null; + this.serverConnectionActive = false; + // User connection-closed hook + this.onDisconnect(); + // Clear out any local cache of server state + this.serverTopics.clear(); + if (event.reason !== "") { + console.log("[NT4] Socket is closed: ", event.reason); + } + if (this.serverConnectionRequested) { + setTimeout(() => this.ws_connect(), 500); + } + } + ws_onError() { + if (this.ws) this.ws.close(); + } + ws_onMessage(event) { + if (typeof event.data === "string") { + // JSON array + this.rxLengthCounter += event.data.length; + let msgData = JSON.parse(event.data); + if (!Array.isArray(msgData)) { + console.warn( + "[NT4] Ignoring text message, JSON parsing did not produce an array at the top level." + ); + return; + } + msgData.forEach((msg) => { + // Validate proper format of message + if (typeof msg !== "object") { + console.warn( + "[NT4] Ignoring text message, JSON parsing did not produce an object." + ); + return; + } + if (!("method" in msg) || !("params" in msg)) { + console.warn( + "[NT4] Ignoring text message, JSON parsing did not find all required fields." + ); + return; + } + let method = msg["method"]; + let params = msg["params"]; + if (typeof method !== "string") { + console.warn( + '[NT4] Ignoring text message, JSON parsing found "method", but it wasn\'t a string.' + ); + return; + } + if (typeof params !== "object") { + console.warn( + '[NT4] Ignoring text message, JSON parsing found "params", but it wasn\'t an object.' + ); + return; + } + // Message validates reasonably, switch based on supported methods + if (method === "announce") { + let newTopic = new NT4_Topic(); + newTopic.uid = params.id; + newTopic.name = params.name; + newTopic.type = params.type; + newTopic.properties = params.properties; + this.serverTopics.set(newTopic.name, newTopic); + this.onTopicAnnounce(newTopic); + } else if (method === "unannounce") { + let removedTopic = this.serverTopics.get(params.name); + if (!removedTopic) { + console.warn( + "[NT4] Ignoring unannounce, topic was not previously announced." + ); + return; + } + this.serverTopics.delete(removedTopic.name); + this.onTopicUnannounce(removedTopic); + } else if (method === "properties") { + let topic = this.serverTopics.get(params.name); + if (!topic) { + console.warn( + "[NT4] Ignoring set properties, topic was not previously announced." + ); + return; + } + for (const key of Object.keys(params.update)) { + let value = params.update[key]; + if (value === null) { + delete topic.properties[key]; + } else { + topic.properties[key] = value; + } + } + } else { + console.warn( + "[NT4] Ignoring text message - unknown method " + method + ); + return; + } + }); + } else { + // MSGPack + this.rxLengthCounter += event.data.byteLength; + deserialize(event.data, { multiple: true }).forEach((unpackedData) => { + let topicID = unpackedData[0]; + let timestamp_us = unpackedData[1]; + let typeIdx = unpackedData[2]; + let value = unpackedData[3]; + if (topicID >= 0) { + let topic = null; + for (let serverTopic of this.serverTopics.values()) { + if (serverTopic.uid === topicID) { + topic = serverTopic; + break; + } + } + if (!topic) { + console.warn( + "[NT4] Ignoring binary data - unknown topic ID " + + topicID.toString() + ); + return; + } + this.onNewTopicData(topic, timestamp_us, value); + } else if (topicID === -1) { + this.ws_handleReceiveTimestamp(timestamp_us, value); + } else { + console.warn( + "[NT4] Ignoring binary data - invalid topic ID " + + topicID.toString() + ); + } + }); + } + } + ws_connect() { + let port = 5810; + let prefix = "ws://"; + this.serverAddr = + prefix + + this.serverBaseAddr + + ":" + + port.toString() + + "/nt/" + + this.appName; + this.ws = new WebSocket(this.serverAddr, "networktables.first.wpi.edu"); + this.ws.binaryType = "arraybuffer"; + this.ws.addEventListener("open", () => this.ws_onOpen()); + this.ws.addEventListener("message", (event) => this.ws_onMessage(event)); + this.ws.addEventListener("close", (event) => this.ws_onClose(event)); + this.ws.addEventListener("error", () => this.ws_onError()); + } + ////////////////////////////////////////////////////////////// + // General utilities + getNewUID() { + return Math.floor(Math.random() * 99999999); + } +} diff --git a/reefcontrols-site-old/index.css b/reefcontrols-site-old/index.css new file mode 100644 index 0000000..cfcc378 --- /dev/null +++ b/reefcontrols-site-old/index.css @@ -0,0 +1,315 @@ +/* + Copyright (c) 2025 FRC 6328 + http://github.com/Mechanical-Advantage + + Use of this source code is governed by an MIT-style + license that can be found in the LICENSE file at + the root directory of this project. +*/ + +body { + overscroll-behavior: none; + overflow: hidden; + font-family: Helvetica, sans-serif; + user-select: none; + -webkit-user-select: none; + cursor: default; + color: #000; + + /* Show disconnected state by default */ + background-color: red; +} + +div { + position: absolute; +} + +div.overview-section { + top: 0%; + left: 0%; + height: 100%; + width: 15%; +} + +div.overview-section div.counter-container { + left: 0%; + width: 100%; + height: 25%; +} + +div.overview-section div.counter-container:nth-child(1) { + top: 75%; +} + +div.overview-section div.counter-container:nth-child(2) { + top: 50%; +} + +div.overview-section div.counter-container:nth-child(3) { + top: 25%; +} + +div.overview-section div.counter-container:nth-child(4) { + top: 0%; +} + +div.overview-section div.counter-container div.counter-area { + top: 10%; + height: 80%; + left: 10%; + width: 80%; + border-radius: 5vh; + border: 1px solid #333; +} + +div.overview-section div.counter-container div.counter-area.active { + background-color: #ffffff; +} + +div.overview-section div.counter-container div.counter-area div { + left: 50%; + top: 50%; + transform: translate(-50%, -50%); + font-size: 10vh; +} + +div.reef-section { + top: 0%; + left: 15%; + height: 80%; + width: 85%; +} + +div.reef-section canvas { + position: absolute; + left: 0%; + top: 0%; + width: 100%; + height: 100%; + z-index: 0; +} + +div.reef-section div.branch, +div.reef-section div.algae { + border-radius: 100%; + transform: translate(-50%, -50%); + border: 1px solid #333; + z-index: 1; +} + +div.reef-section div.branch { + height: 12vh; + width: 12vh; +} + +div.reef-section div.algae { + height: 14vh; + width: 14vh; +} + +div.reef-section div.branch { + background-color: #f4c9ff; +} + +div.reef-section div.branch.active { + background-color: #cc00ff; +} + +div.reef-section div.branch img { + display: none; + position: absolute; + left: 50%; + top: 50%; + transform: translate(-50%, -50%); + width: 10vh; +} + +div.reef-section div.branch.active img { + display: initial; +} + +div.reef-section div.algae { + background-color: #c6efff; +} + +div.reef-section div.algae.active { + background-color: #00b7ff; +} + +div.reef-section div.algae img { + display: none; + position: absolute; + left: 50%; + top: 50%; + transform: translate(-50%, -50%); + width: 10vh; +} + +div.reef-section div.algae.active img { + display: initial; +} + +div.reef-section div.branch:nth-child(1) { + left: calc(50% + 10%); + top: calc(50% + 40%); +} + +div.reef-section div.branch:nth-child(2) { + left: calc(50% - 10%); + top: calc(50% + 40%); +} + +div.reef-section div.branch:nth-child(3) { + left: calc(50% - 30%); + top: calc(50% + 30%); +} + +div.reef-section div.branch:nth-child(4) { + left: calc(50% - 40%); + top: calc(50% + 12%); +} + +div.reef-section div.branch:nth-child(5) { + left: calc(50% - 40%); + top: calc(50% - 12%); +} + +div.reef-section div.branch:nth-child(6) { + left: calc(50% - 30%); + top: calc(50% - 30%); +} + +div.reef-section div.branch:nth-child(7) { + left: calc(50% - 10%); + top: calc(50% - 40%); +} + +div.reef-section div.branch:nth-child(8) { + left: calc(50% + 10%); + top: calc(50% - 40%); +} + +div.reef-section div.branch:nth-child(9) { + left: calc(50% + 30%); + top: calc(50% - 30%); +} + +div.reef-section div.branch:nth-child(10) { + left: calc(50% + 40%); + top: calc(50% - 12%); +} + +div.reef-section div.branch:nth-child(11) { + left: calc(50% + 40%); + top: calc(50% + 12%); +} + +div.reef-section div.branch:nth-child(12) { + left: calc(50% + 30%); + top: calc(50% + 30%); +} + +div.reef-section div.algae:nth-child(13) { + left: calc(50% + 0%); + top: calc(50% + 20%); +} + +div.reef-section div.algae:nth-child(14) { + left: calc(50% - 18%); + top: calc(50% + 10%); +} + +div.reef-section div.algae:nth-child(15) { + left: calc(50% - 18%); + top: calc(50% - 10%); +} + +div.reef-section div.algae:nth-child(16) { + left: calc(50% + 0%); + top: calc(50% - 20%); +} + +div.reef-section div.algae:nth-child(17) { + left: calc(50% + 18%); + top: calc(50% - 10%); +} + +div.reef-section div.algae:nth-child(18) { + left: calc(50% + 18%); + top: calc(50% + 10%); +} + +div.reef-section div.flag { + position: absolute; + left: 50%; + top: 50%; + transform: translate(-50%, -50%); + font-size: 12vh; + z-index: 1; +} + +div.lower-section { + top: 80%; + left: 15%; + height: 20%; + width: 85%; +} + +div.lower-section div.subtract, +div.lower-section div.add, +div.lower-section div.coop { + top: 10%; + height: 80%; + width: 25%; + border-radius: 5vh; + border: 1px solid #333; +} + +div.lower-section div.subtract { + left: 7%; +} + +div.lower-section div.add { + left: 34%; +} + +div.lower-section div.coop { + left: 68%; +} + +div.lower-section div.coop.active { + background-color: #00b7ff; +} + +div.lower-section div.subtract div, +div.lower-section div.add div { + left: 50%; + top: 50%; + transform: translate(-50%, -50%); + font-size: 10vh; + filter: brightness(0); +} + +div.lower-section div.coop div { + display: none; + left: 50%; + top: 50%; + transform: translate(-50%, -50%); + font-size: 12vh; +} + +div.lower-section div.coop.active div { + display: initial; +} + +body.elims div.lower-section div.subtract { + left: 24%; +} + +body.elims div.lower-section div.add { + left: 51%; +} + +body.elims div.lower-section div.coop { + display: none; +} \ No newline at end of file diff --git a/reefcontrols-site-old/index.html b/reefcontrols-site-old/index.html new file mode 100644 index 0000000..95008a0 --- /dev/null +++ b/reefcontrols-site-old/index.html @@ -0,0 +1,67 @@ + + + + + + + + + + Reef Controls — FRC 9470 + + +
+
+
0
+
+
+
0
+
+
+
0
+
+
+
0
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ + +
+
+
➖
+
➕
+
🤝
+
+ + \ No newline at end of file diff --git a/reefcontrols-site-old/index.js b/reefcontrols-site-old/index.js new file mode 100644 index 0000000..4be0e63 --- /dev/null +++ b/reefcontrols-site-old/index.js @@ -0,0 +1,284 @@ +// Copyright (c) 2025 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +import { NT4_Client } from "./NT4.js"; + +// ***** NETWORKTABLES ***** + +// const toRobotPrefix = "/ReefControls/ToRobot/"; +// const toDashboardPrefix = "/ReefControls/ToDashboard/"; +// const selectedLevelTopicName = "SelectedLevel"; +// const l1TopicName = "Level1"; +// const l2TopicName = "Level2"; +// const l3TopicName = "Level3"; +// const l4TopicName = "Level4"; +// const algaeTopicName = "Algae"; +// const coopTopicName = "Coop"; +// const isElimsTopicName = "IsElims"; + +const ntClient = new NT4_Client( + window.location.hostname, + "Gamma2025", + () => { + // Topic announce + }, + () => { + // Topic unannounce + }, + (topic, _, value) => { + // New data + }, + () => { + // Connected + document.body.style.backgroundColor = "white"; + }, + () => { + // Disconnected + document.body.style.backgroundColor = "red"; + } +); + +// Start NT connection +window.addEventListener("load", () => { + ntClient.publishTopic("branch", "int"); + ntClient.publishTopic("level", "int"); + ntClient.connect(); +}); + +// ***** STATE CACHE ***** + +let selectedLevel = 0; // 0 = L2, 1 = L3, 2 = L4 +let l1State = 0; // Count +let l2State = 0; // Bitfield +let l3State = 0; // Bitfield +let l4State = 0; // Bitfield +let algaeState = 0; // Bitfield +let coopState = false; // Boolean +let isElims = false; // Boolean + +/** Update the full UI based on the state cache. */ +function updateUI() { + // Update counter highlight + Array.from(document.getElementsByClassName("counter-area")).forEach( + (element, index) => { + if (index > 0 && selectedLevel === index - 1) { + element.classList.add("active"); + } else { + element.classList.remove("active"); + } + } + ); + + // Update background color + switch (selectedLevel) { + case 0: + document.body.style.backgroundColor = "#ccffff"; + break; + case 1: + document.body.style.backgroundColor = "#b5e5ff"; + break; + case 2: + document.body.style.backgroundColor = "#dfbbfc"; + break; + } + + // Update level counts + let rpLevelCount = 0; + Array.from(document.getElementsByClassName("counter")).forEach( + (element, index) => { + if (index === 0) { + element.innerText = l1State; + if (l1State >= 5) rpLevelCount++; + } else { + let count = 0; + let levelState = [l2State, l3State, l4State][index - 1]; + for (let i = 0; i < 12; i++) { + if (((1 << i) & levelState) > 0) { + count++; + } + } + element.innerText = count === 12 ? "\u2705" : count; + if (count >= 5) rpLevelCount++; + } + } + ); + + // Update coral buttons + Array.from(document.getElementsByClassName("branch")).forEach( + (element, index) => { + let levelState = [l2State, l3State, l4State][selectedLevel]; + if (((1 << index) & levelState) > 0) { + element.classList.add("active"); + } else { + element.classList.remove("active"); + } + } + ); + + // Update algae buttons + Array.from(document.getElementsByClassName("algae")).forEach( + (element, index) => { + if (((1 << index) & algaeState) > 0) { + element.classList.add("active"); + } else { + element.classList.remove("active"); + } + } + ); + + // Update coop button + let coopDiv = document.getElementsByClassName("coop")[0]; + if (coopState) { + coopDiv.classList.add("active"); + } else { + coopDiv.classList.remove("active"); + } + + // Update RP flag + document.getElementsByClassName("flag")[0].hidden = + isElims || rpLevelCount < (coopState ? 3 : 4); + + // Update elims state + if (isElims) { + document.body.classList.add("elims"); + } else { + document.body.classList.remove("elims"); + } +} + +// ***** BUTTON BINDINGS ***** + +let isTouch = false; + +function bind(element, callback) { + let activate = (touchEvent) => { + if (touchEvent) { + isTouch = true; + } + if (isTouch == touchEvent) { + callback(); + } + }; + + element.addEventListener("touchstart", () => activate(true)); + element.addEventListener("mousedown", () => activate(false)); + element.addEventListener("contextmenu", (event) => { + event.preventDefault(); + activate(false); + }); +} + +let lastMouseEvent = 0; +window.addEventListener("mousemove", () => { + let now = new Date().getTime(); + if (now - lastMouseEvent < 50) { + isTouch = false; + } + lastMouseEvent = now; +}); + +window.addEventListener("load", () => { + // Buttons to change selected level + Array.from(document.getElementsByClassName("counter-area")).forEach( + (element, index) => { + if (index > 0) { + bind(element, () => { + selectedLevel = index; + }); + } + } + ); + + // Coral toggle buttons + Array.from(document.getElementsByClassName("branch")).forEach( + (element, index) => { + bind(element, () => { + ntClient.addSample( + "branch", + index + ); + ntClient.addSample( + "level", + selectedLevel + ); + }); + } + ); + + // // Algae toggle buttons + // Array.from(document.getElementsByClassName("algae")).forEach( + // (element, index) => { + // bind(element, () => { + // ntClient.addSample( + // toRobotPrefix + algaeTopicName, + // algaeState ^ (1 << index) + // ); + // }); + // } + // ); + + // // L1 count controls + // bind(document.getElementsByClassName("subtract")[0], () => { + // if (l1State > 0) { + // ntClient.addSample(toRobotPrefix + l1TopicName, l1State - 1); + // } + // }); + // bind(document.getElementsByClassName("add")[0], () => { + // ntClient.addSample(toRobotPrefix + l1TopicName, l1State + 1); + // }); + + // // Coop button + // bind(document.getElementsByClassName("coop")[0], () => { + // ntClient.addSample(toRobotPrefix + coopTopicName, !coopState); + // }); +}); + +// ***** REEF CANVAS ***** + +window.addEventListener("load", () => { + const canvas = document.getElementsByTagName("canvas")[0]; + const context = canvas.getContext("2d"); + + let render = () => { + const devicePixelRatio = window.devicePixelRatio; + const width = canvas.clientWidth; + const height = canvas.clientHeight; + canvas.width = width * devicePixelRatio; + canvas.height = height * devicePixelRatio; + context.scale(devicePixelRatio, devicePixelRatio); + context.clearRect(0, 0, width, height); + + const corners = [ + [width * 0.74, height * 0.9], + [width * 0.26, height * 0.9], + [width * 0.03, height * 0.5], + [width * 0.26, height * 0.1], + [width * 0.74, height * 0.1], + [width * 0.97, height * 0.5], + ]; + + context.beginPath(); + corners.forEach((corner) => { + context.moveTo(width * 0.5, height * 0.5); + context.lineTo(...corner); + }); + corners.forEach((corner, index) => { + if (index == 0) { + context.moveTo(...corner); + } else { + context.lineTo(...corner); + } + }); + context.closePath(); + + context.strokeStyle = "black"; + context.stroke(); + }; + + render(); + window.addEventListener("resize", render); +}); diff --git a/reefcontrols-site-old/msgpack.js b/reefcontrols-site-old/msgpack.js new file mode 100644 index 0000000..ecf20ee --- /dev/null +++ b/reefcontrols-site-old/msgpack.js @@ -0,0 +1,518 @@ +// Serializes a value to a MessagePack byte array. +// +// data: The value to serialize. This can be a scalar, array or object. +// options: An object that defined additional options. +// - multiple: Indicates whether multiple values in data are concatenated to multiple MessagePack arrays. +// - invalidTypeReplacement: The value that is used to replace values of unsupported types, or a function that returns such a value, given the original value as parameter. +export function serialize(data, options) { + if (options && options.multiple && !Array.isArray(data)) { + throw new Error("Invalid argument type: Expected an Array to serialize multiple values."); + } + const pow32 = 0x100000000; // 2^32 + let floatBuffer, floatView; + let array = new Uint8Array(128); + let length = 0; + + let th = ""; + if (options && options.typeHint) { + th = options.typeHint; + } + + if (options && options.multiple) { + for (let i = 0; i < data.length; i++) { + append(data[i], false, th); + } + } else { + append(data, false, th); + } + return array.subarray(0, length); + + function append(data, isReplacement, th) { + switch (typeof data) { + case "undefined": + appendNull(data); + break; + case "boolean": + appendBoolean(data); + break; + case "number": + appendNumber(data, th); + break; + case "string": + appendString(data); + break; + case "object": + if (data === null) appendNull(data); + else if (data instanceof Date) appendDate(data); + else if (Array.isArray(data)) appendArray(data); + else if (data instanceof Uint8Array || data instanceof Uint8ClampedArray) appendBinArray(data); + else if ( + data instanceof Int8Array || + data instanceof Int16Array || + data instanceof Uint16Array || + data instanceof Int32Array || + data instanceof Uint32Array || + data instanceof Float32Array || + data instanceof Float64Array + ) + appendArray(data); + else appendObject(data); + break; + default: + if (!isReplacement && options && options.invalidTypeReplacement) { + if (typeof options.invalidTypeReplacement === "function") + append(options.invalidTypeReplacement(data), true, th); + else append(options.invalidTypeReplacement, true, th); + } else { + throw new Error("Invalid argument type: The type '" + typeof data + "' cannot be serialized."); + } + } + } + + function appendNull(data) { + appendByte(0xc0); + } + + function appendBoolean(data) { + appendByte(data ? 0xc3 : 0xc2); + } + + function appendNumber(data, th) { + const isInteger = + th === "int" || (isFinite(data) && Math.floor(data) === data && th !== "double" && th !== "float"); + if (isInteger) { + // Integer + if (data >= 0 && data <= 0x7f) { + appendByte(data); + } else if (data < 0 && data >= -0x20) { + appendByte(data); + } else if (data > 0 && data <= 0xff) { + // uint8 + appendBytes([0xcc, data]); + } else if (data >= -0x80 && data <= 0x7f) { + // int8 + appendBytes([0xd0, data]); + } else if (data > 0 && data <= 0xffff) { + // uint16 + appendBytes([0xcd, data >>> 8, data]); + } else if (data >= -0x8000 && data <= 0x7fff) { + // int16 + appendBytes([0xd1, data >>> 8, data]); + } else if (data > 0 && data <= 0xffffffff) { + // uint32 + appendBytes([0xce, data >>> 24, data >>> 16, data >>> 8, data]); + } else if (data >= -0x80000000 && data <= 0x7fffffff) { + // int32 + appendBytes([0xd2, data >>> 24, data >>> 16, data >>> 8, data]); + } else if (data > 0 && data <= 0xffffffffffffffff) { + // uint64 + // Split 64-bit number into two 32-bit numbers because JavaScript only regards + // 32 bits for bitwise operations. + let hi = data / pow32; + let lo = data % pow32; + appendBytes([0xd3, hi >>> 24, hi >>> 16, hi >>> 8, hi, lo >>> 24, lo >>> 16, lo >>> 8, lo]); + } else if (data >= -0x8000000000000000 && data <= 0x7fffffffffffffff) { + // int64 + appendByte(0xd3); + appendInt64(data); + } else if (data < 0) { + // below int64 + appendBytes([0xd3, 0x80, 0, 0, 0, 0, 0, 0, 0]); + } else { + // above uint64 + appendBytes([0xcf, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff]); + } + } else { + // Float + if (!floatView) { + floatBuffer = new ArrayBuffer(8); + floatView = new DataView(floatBuffer); + } + floatView.setFloat64(0, data); + appendByte(0xcb); + appendBytes(new Uint8Array(floatBuffer)); + } + } + + function appendString(data) { + let bytes = encodeUtf8(data); + let length = bytes.length; + + if (length <= 0x1f) appendByte(0xa0 + length); + else if (length <= 0xff) appendBytes([0xd9, length]); + else if (length <= 0xffff) appendBytes([0xda, length >>> 8, length]); + else appendBytes([0xdb, length >>> 24, length >>> 16, length >>> 8, length]); + + appendBytes(bytes); + } + + function appendArray(data) { + let length = data.length; + + if (length <= 0xf) appendByte(0x90 + length); + else if (length <= 0xffff) appendBytes([0xdc, length >>> 8, length]); + else appendBytes([0xdd, length >>> 24, length >>> 16, length >>> 8, length]); + + for (let index = 0; index < length; index++) { + append(data[index]); + } + } + + function appendBinArray(data) { + let length = data.length; + + if (length <= 0xf) appendBytes([0xc4, length]); + else if (length <= 0xffff) appendBytes([0xc5, length >>> 8, length]); + else appendBytes([0xc6, length >>> 24, length >>> 16, length >>> 8, length]); + + appendBytes(data); + } + + function appendObject(data) { + let length = 0; + for (let key in data) { + if (data[key] !== undefined) { + length++; + } + } + + if (length <= 0xf) appendByte(0x80 + length); + else if (length <= 0xffff) appendBytes([0xde, length >>> 8, length]); + else appendBytes([0xdf, length >>> 24, length >>> 16, length >>> 8, length]); + + for (let key in data) { + let value = data[key]; + if (value !== undefined) { + append(key); + append(value); + } + } + } + + function appendDate(data) { + let sec = data.getTime() / 1000; + if (data.getMilliseconds() === 0 && sec >= 0 && sec < 0x100000000) { + // 32 bit seconds + appendBytes([0xd6, 0xff, sec >>> 24, sec >>> 16, sec >>> 8, sec]); + } else if (sec >= 0 && sec < 0x400000000) { + // 30 bit nanoseconds, 34 bit seconds + let ns = data.getMilliseconds() * 1000000; + appendBytes([ + 0xd7, + 0xff, + ns >>> 22, + ns >>> 14, + ns >>> 6, + ((ns << 2) >>> 0) | (sec / pow32), + sec >>> 24, + sec >>> 16, + sec >>> 8, + sec + ]); + } else { + // 32 bit nanoseconds, 64 bit seconds, negative values allowed + let ns = data.getMilliseconds() * 1000000; + appendBytes([0xc7, 12, 0xff, ns >>> 24, ns >>> 16, ns >>> 8, ns]); + appendInt64(sec); + } + } + + function appendByte(byte) { + if (array.length < length + 1) { + let newLength = array.length * 2; + while (newLength < length + 1) newLength *= 2; + let newArray = new Uint8Array(newLength); + newArray.set(array); + array = newArray; + } + array[length] = byte; + length++; + } + + function appendBytes(bytes) { + if (array.length < length + bytes.length) { + let newLength = array.length * 2; + while (newLength < length + bytes.length) newLength *= 2; + let newArray = new Uint8Array(newLength); + newArray.set(array); + array = newArray; + } + array.set(bytes, length); + length += bytes.length; + } + + function appendInt64(value) { + // Split 64-bit number into two 32-bit numbers because JavaScript only regards 32 bits for + // bitwise operations. + let hi, lo; + if (value >= 0) { + // Same as uint64 + hi = value / pow32; + lo = value % pow32; + } else { + // Split absolute value to high and low, then NOT and ADD(1) to restore negativity + value++; + hi = Math.abs(value) / pow32; + lo = Math.abs(value) % pow32; + hi = ~hi; + lo = ~lo; + } + appendBytes([hi >>> 24, hi >>> 16, hi >>> 8, hi, lo >>> 24, lo >>> 16, lo >>> 8, lo]); + } +} + +// Deserializes a MessagePack byte array to a value. +// +// array: The MessagePack byte array to deserialize. This must be an Array or Uint8Array containing bytes, not a string. +// options: An object that defined additional options. +// - multiple: Indicates whether multiple concatenated MessagePack arrays are returned as an array. +export function deserialize(array, options) { + const pow32 = 0x100000000; // 2^32 + let pos = 0; + if (array instanceof ArrayBuffer) { + array = new Uint8Array(array); + } + if (typeof array !== "object" || typeof array.length === "undefined") { + throw new Error("Invalid argument type: Expected a byte array (Array or Uint8Array) to deserialize."); + } + if (!array.length) { + throw new Error("Invalid argument: The byte array to deserialize is empty."); + } + if (!(array instanceof Uint8Array)) { + array = new Uint8Array(array); + } + let data; + if (options && options.multiple) { + // Read as many messages as are available + data = []; + while (pos < array.length) { + data.push(read()); + } + } else { + // Read only one message and ignore additional data + data = read(); + } + return data; + + function read() { + const byte = array[pos++]; + if (byte >= 0x00 && byte <= 0x7f) return byte; // positive fixint + if (byte >= 0x80 && byte <= 0x8f) return readMap(byte - 0x80); // fixmap + if (byte >= 0x90 && byte <= 0x9f) return readArray(byte - 0x90); // fixarray + if (byte >= 0xa0 && byte <= 0xbf) return readStr(byte - 0xa0); // fixstr + if (byte === 0xc0) return null; // nil + if (byte === 0xc1) throw new Error("Invalid byte code 0xc1 found."); // never used + if (byte === 0xc2) return false; // false + if (byte === 0xc3) return true; // true + if (byte === 0xc4) return readBin(-1, 1); // bin 8 + if (byte === 0xc5) return readBin(-1, 2); // bin 16 + if (byte === 0xc6) return readBin(-1, 4); // bin 32 + if (byte === 0xc7) return readExt(-1, 1); // ext 8 + if (byte === 0xc8) return readExt(-1, 2); // ext 16 + if (byte === 0xc9) return readExt(-1, 4); // ext 32 + if (byte === 0xca) return readFloat(4); // float 32 + if (byte === 0xcb) return readFloat(8); // float 64 + if (byte === 0xcc) return readUInt(1); // uint 8 + if (byte === 0xcd) return readUInt(2); // uint 16 + if (byte === 0xce) return readUInt(4); // uint 32 + if (byte === 0xcf) return readUInt(8); // uint 64 + if (byte === 0xd0) return readInt(1); // int 8 + if (byte === 0xd1) return readInt(2); // int 16 + if (byte === 0xd2) return readInt(4); // int 32 + if (byte === 0xd3) return readInt(8); // int 64 + if (byte === 0xd4) return readExt(1); // fixext 1 + if (byte === 0xd5) return readExt(2); // fixext 2 + if (byte === 0xd6) return readExt(4); // fixext 4 + if (byte === 0xd7) return readExt(8); // fixext 8 + if (byte === 0xd8) return readExt(16); // fixext 16 + if (byte === 0xd9) return readStr(-1, 1); // str 8 + if (byte === 0xda) return readStr(-1, 2); // str 16 + if (byte === 0xdb) return readStr(-1, 4); // str 32 + if (byte === 0xdc) return readArray(-1, 2); // array 16 + if (byte === 0xdd) return readArray(-1, 4); // array 32 + if (byte === 0xde) return readMap(-1, 2); // map 16 + if (byte === 0xdf) return readMap(-1, 4); // map 32 + if (byte >= 0xe0 && byte <= 0xff) return byte - 256; // negative fixint + console.debug("msgpack array:", array); + throw new Error( + "Invalid byte value '" + + byte + + "' at index " + + (pos - 1) + + " in the MessagePack binary data (length " + + array.length + + "): Expecting a range of 0 to 255. This is not a byte array." + ); + } + + function readInt(size) { + let value = 0; + let first = true; + while (size-- > 0) { + if (first) { + let byte = array[pos++]; + value += byte & 0x7f; + if (byte & 0x80) { + value -= 0x80; // Treat most-significant bit as -2^i instead of 2^i + } + first = false; + } else { + value *= 256; + value += array[pos++]; + } + } + return value; + } + + function readUInt(size) { + let value = 0; + while (size-- > 0) { + value *= 256; + value += array[pos++]; + } + return value; + } + + function readFloat(size) { + let view = new DataView(array.buffer, pos + array.byteOffset, size); + pos += size; + if (size === 4) return view.getFloat32(0, false); + if (size === 8) return view.getFloat64(0, false); + } + + function readBin(size, lengthSize) { + if (size < 0) size = readUInt(lengthSize); + let data = array.subarray(pos, pos + size); + pos += size; + return data; + } + + function readMap(size, lengthSize) { + if (size < 0) size = readUInt(lengthSize); + let data = {}; + while (size-- > 0) { + let key = read(); + data[key] = read(); + } + return data; + } + + function readArray(size, lengthSize) { + if (size < 0) size = readUInt(lengthSize); + let data = []; + while (size-- > 0) { + data.push(read()); + } + return data; + } + + function readStr(size, lengthSize) { + if (size < 0) size = readUInt(lengthSize); + let start = pos; + pos += size; + return decodeUtf8(array, start, size); + } + + function readExt(size, lengthSize) { + if (size < 0) size = readUInt(lengthSize); + let type = readUInt(1); + let data = readBin(size); + switch (type) { + case 255: + return readExtDate(data); + } + return { type: type, data: data }; + } + + function readExtDate(data) { + if (data.length === 4) { + let sec = ((data[0] << 24) >>> 0) + ((data[1] << 16) >>> 0) + ((data[2] << 8) >>> 0) + data[3]; + return new Date(sec * 1000); + } + if (data.length === 8) { + let ns = ((data[0] << 22) >>> 0) + ((data[1] << 14) >>> 0) + ((data[2] << 6) >>> 0) + (data[3] >>> 2); + let sec = + (data[3] & 0x3) * pow32 + ((data[4] << 24) >>> 0) + ((data[5] << 16) >>> 0) + ((data[6] << 8) >>> 0) + data[7]; + return new Date(sec * 1000 + ns / 1000000); + } + if (data.length === 12) { + let ns = ((data[0] << 24) >>> 0) + ((data[1] << 16) >>> 0) + ((data[2] << 8) >>> 0) + data[3]; + pos -= 8; + let sec = readInt(8); + return new Date(sec * 1000 + ns / 1000000); + } + throw new Error("Invalid data length for a date value."); + } +} + +// Encodes a string to UTF-8 bytes. +function encodeUtf8(str) { + // Prevent excessive array allocation and slicing for all 7-bit characters + let ascii = true, + length = str.length; + for (let x = 0; x < length; x++) { + if (str.charCodeAt(x) > 127) { + ascii = false; + break; + } + } + + // Based on: https://gist.github.com/pascaldekloe/62546103a1576803dade9269ccf76330 + let i = 0, + bytes = new Uint8Array(str.length * (ascii ? 1 : 4)); + for (let ci = 0; ci !== length; ci++) { + let c = str.charCodeAt(ci); + if (c < 128) { + bytes[i++] = c; + continue; + } + if (c < 2048) { + bytes[i++] = (c >> 6) | 192; + } else { + if (c > 0xd7ff && c < 0xdc00) { + if (++ci >= length) throw new Error("UTF-8 encode: incomplete surrogate pair"); + let c2 = str.charCodeAt(ci); + if (c2 < 0xdc00 || c2 > 0xdfff) + throw new Error( + "UTF-8 encode: second surrogate character 0x" + c2.toString(16) + " at index " + ci + " out of range" + ); + c = 0x10000 + ((c & 0x03ff) << 10) + (c2 & 0x03ff); + bytes[i++] = (c >> 18) | 240; + bytes[i++] = ((c >> 12) & 63) | 128; + } else bytes[i++] = (c >> 12) | 224; + bytes[i++] = ((c >> 6) & 63) | 128; + } + bytes[i++] = (c & 63) | 128; + } + return ascii ? bytes : bytes.subarray(0, i); +} + +// Decodes a string from UTF-8 bytes. +function decodeUtf8(bytes, start, length) { + // Based on: https://gist.github.com/pascaldekloe/62546103a1576803dade9269ccf76330 + let i = start, + str = ""; + length += start; + while (i < length) { + let c = bytes[i++]; + if (c > 127) { + if (c > 191 && c < 224) { + if (i >= length) throw new Error("UTF-8 decode: incomplete 2-byte sequence"); + c = ((c & 31) << 6) | (bytes[i++] & 63); + } else if (c > 223 && c < 240) { + if (i + 1 >= length) throw new Error("UTF-8 decode: incomplete 3-byte sequence"); + c = ((c & 15) << 12) | ((bytes[i++] & 63) << 6) | (bytes[i++] & 63); + } else if (c > 239 && c < 248) { + if (i + 2 >= length) throw new Error("UTF-8 decode: incomplete 4-byte sequence"); + c = ((c & 7) << 18) | ((bytes[i++] & 63) << 12) | ((bytes[i++] & 63) << 6) | (bytes[i++] & 63); + } else throw new Error("UTF-8 decode: unknown multibyte start 0x" + c.toString(16) + " at index " + (i - 1)); + } + if (c <= 0xffff) str += String.fromCharCode(c); + else if (c <= 0x10ffff) { + c -= 0x10000; + str += String.fromCharCode((c >> 10) | 0xd800); + str += String.fromCharCode((c & 0x3ff) | 0xdc00); + } else throw new Error("UTF-8 decode: code point 0x" + c.toString(16) + " exceeds UTF-16 reach"); + } + return str; +} diff --git a/reefcontrols-site/index.js b/reefcontrols-site/index.js index 7ede457..e4d18ad 100644 --- a/reefcontrols-site/index.js +++ b/reefcontrols-site/index.js @@ -9,20 +9,20 @@ import { NT4_Client } from "./NT4.js"; // ***** NETWORKTABLES ***** -const toRobotPrefix = "/ReefControls/ToRobot/"; -const toDashboardPrefix = "/ReefControls/ToDashboard/"; -const selectedLevelTopicName = "SelectedLevel"; -const l1TopicName = "Level1"; -const l2TopicName = "Level2"; -const l3TopicName = "Level3"; -const l4TopicName = "Level4"; -const algaeTopicName = "Algae"; -const coopTopicName = "Coop"; -const isElimsTopicName = "IsElims"; +// const toRobotPrefix = "/ReefControls/ToRobot/"; +const toDashboardPrefix = "/DriveState/"; +// const selectedLevelTopicName = "SelectedLevel"; +// const l1TopicName = "Level1"; +// const l2TopicName = "Level2"; +// const l3TopicName = "Level3"; +// const l4TopicName = "Level4"; +// const algaeTopicName = "Algae"; +// const coopTopicName = "Coop"; +// const isElimsTopicName = "IsElims"; const ntClient = new NT4_Client( window.location.hostname, - "ReefControls", + "Beta2025", () => { // Topic announce }, @@ -31,29 +31,10 @@ const ntClient = new NT4_Client( }, (topic, _, value) => { // New data - if (topic.name === toDashboardPrefix + selectedLevelTopicName) { - selectedLevel = value; - } else if (topic.name === toDashboardPrefix + l1TopicName) { - l1State = value; - } else if (topic.name === toDashboardPrefix + l2TopicName) { - l2State = value; - } else if (topic.name === toDashboardPrefix + l3TopicName) { - l3State = value; - } else if (topic.name === toDashboardPrefix + l4TopicName) { - l4State = value; - } else if (topic.name === toDashboardPrefix + algaeTopicName) { - algaeState = value; - } else if (topic.name === toDashboardPrefix + coopTopicName) { - coopState = value; - } else if (topic.name === toDashboardPrefix + isElimsTopicName) { - isElims = value; - } else { - return; - } - updateUI(); }, () => { // Connected + document.body.style.backgroundColor = "white"; }, () => { // Disconnected @@ -63,29 +44,9 @@ const ntClient = new NT4_Client( // Start NT connection window.addEventListener("load", () => { - ntClient.subscribe( - [ - toDashboardPrefix + selectedLevelTopicName, - toDashboardPrefix + l1TopicName, - toDashboardPrefix + l2TopicName, - toDashboardPrefix + l3TopicName, - toDashboardPrefix + l4TopicName, - toDashboardPrefix + algaeTopicName, - toDashboardPrefix + coopTopicName, - toDashboardPrefix + isElimsTopicName, - ], - false, - false, - 0.02 - ); - ntClient.publishTopic(toRobotPrefix + selectedLevelTopicName, "int"); - ntClient.publishTopic(toRobotPrefix + l1TopicName, "int"); - ntClient.publishTopic(toRobotPrefix + l2TopicName, "int"); - ntClient.publishTopic(toRobotPrefix + l3TopicName, "int"); - ntClient.publishTopic(toRobotPrefix + l4TopicName, "int"); - ntClient.publishTopic(toRobotPrefix + algaeTopicName, "int"); - ntClient.publishTopic(toRobotPrefix + coopTopicName, "boolean"); + ntClient.publishTopic(toDashboardPrefix+"branch", "int"); + ntClient.publishTopic(toDashboardPrefix+"level", "int"); ntClient.connect(); }); @@ -200,6 +161,7 @@ function bind(element, callback) { isTouch = true; } if (isTouch == touchEvent) { + console.log("EEEEEEEEE"); callback(); } }; @@ -227,7 +189,7 @@ window.addEventListener("load", () => { (element, index) => { if (index > 0) { bind(element, () => { - ntClient.addSample(toRobotPrefix + selectedLevelTopicName, index - 1); + selectedLevel = index; }); } } @@ -237,56 +199,47 @@ window.addEventListener("load", () => { Array.from(document.getElementsByClassName("branch")).forEach( (element, index) => { bind(element, () => { - switch (selectedLevel) { - case 0: - ntClient.addSample( - toRobotPrefix + l2TopicName, - l2State ^ (1 << index) - ); - break; - case 1: - ntClient.addSample( - toRobotPrefix + l3TopicName, - l3State ^ (1 << index) - ); - break; - case 2: - ntClient.addSample( - toRobotPrefix + l4TopicName, - l4State ^ (1 << index) - ); - break; - } - }); - } - ); - - // Algae toggle buttons - Array.from(document.getElementsByClassName("algae")).forEach( - (element, index) => { - bind(element, () => { + console.log("FFFFFFF"); + console.log(index); + console.log(selectedLevel); + ntClient.addSample( + toDashboardPrefix+"branch", + index + ); ntClient.addSample( - toRobotPrefix + algaeTopicName, - algaeState ^ (1 << index) + toDashboardPrefix+"level", + selectedLevel ); }); } ); - // L1 count controls - bind(document.getElementsByClassName("subtract")[0], () => { - if (l1State > 0) { - ntClient.addSample(toRobotPrefix + l1TopicName, l1State - 1); - } - }); - bind(document.getElementsByClassName("add")[0], () => { - ntClient.addSample(toRobotPrefix + l1TopicName, l1State + 1); - }); - - // Coop button - bind(document.getElementsByClassName("coop")[0], () => { - ntClient.addSample(toRobotPrefix + coopTopicName, !coopState); - }); + // // Algae toggle buttons + // Array.from(document.getElementsByClassName("algae")).forEach( + // (element, index) => { + // bind(element, () => { + // ntClient.addSample( + // toRobotPrefix + algaeTopicName, + // algaeState ^ (1 << index) + // ); + // }); + // } + // ); + + // // L1 count controls + // bind(document.getElementsByClassName("subtract")[0], () => { + // if (l1State > 0) { + // ntClient.addSample(toRobotPrefix + l1TopicName, l1State - 1); + // } + // }); + // bind(document.getElementsByClassName("add")[0], () => { + // ntClient.addSample(toRobotPrefix + l1TopicName, l1State + 1); + // }); + + // // Coop button + // bind(document.getElementsByClassName("coop")[0], () => { + // ntClient.addSample(toRobotPrefix + coopTopicName, !coopState); + // }); }); // ***** REEF CANVAS ***** diff --git a/simgui-ds.json b/simgui-ds.json index 9f298da..e4167aa 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Joysticks": { + "window": { + "visible": false + } + }, "Keyboard 0 Settings": { "window": { "visible": true @@ -13,7 +18,7 @@ { "axisConfig": [ { - "decKey": 82, + "decKey": 83, "incKey": 87 }, { @@ -23,22 +28,35 @@ { "decKey": 69, "decayRate": 0.0, + "incKey": 82, "keyRate": 0.009999999776482582 + }, + { + "incKey": 85 + }, + { + "decKey": 263, + "incKey": 262 } ], - "axisCount": 3, - "buttonCount": 4, + "axisCount": 5, + "buttonCount": 15, "buttonKeys": [ 89, - 85, - 73, - 79, -1, -1, -1, -1, -1, - -1 + -1, + -1, + -1, + -1, + -1, + -1, + -1, + -1, + 343 ], "povConfig": [ { @@ -57,7 +75,6 @@ { "axisConfig": [ { - "decKey": 74, "incKey": 76 }, { @@ -84,10 +101,7 @@ }, { "axisConfig": [ - { - "decKey": 263, - "incKey": 262 - }, + {}, { "decKey": 265, "incKey": 264 diff --git a/simgui.json b/simgui.json index ebef95f..371d8e5 100644 --- a/simgui.json +++ b/simgui.json @@ -26,7 +26,6 @@ "windows": { "/Pose": { "bottom": 1638, - "builtin": "2025 Reefscape", "height": 8.051901817321777, "left": 534, "right": 3466, @@ -38,7 +37,6 @@ }, "/SmartDashboard/VisionSystemSim-main/Sim Field": { "bottom": 1638, - "builtin": "2025 Reefscape", "height": 8.051901817321777, "left": 534, "right": 3466, @@ -62,8 +60,5 @@ "open": true }, "visible": true - }, - "NetworkTables View": { - "visible": false } } diff --git a/src/main/java/com/team9470/RobotContainer.java b/src/main/java/com/team9470/RobotContainer.java index c3332e6..05d3209 100644 --- a/src/main/java/com/team9470/RobotContainer.java +++ b/src/main/java/com/team9470/RobotContainer.java @@ -7,6 +7,8 @@ import com.team9470.commands.Autos; import com.team9470.subsystems.Superstructure; import com.team9470.subsystems.Swerve; +import com.team9470.subsystems.sim.ButtonBoardPub; +import com.team9470.subsystems.sim.ButtonBoardSub; import com.team9470.subsystems.vision.Vision; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; @@ -30,7 +32,7 @@ public class RobotContainer { private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); public final Swerve drivetrain = TunerConstants.createDrivetrain(); - private final Telemetry logger = new Telemetry(MaxSpeed); + private final Telemetry logger = new Telemetry(); // ---------------- MECHANISM2D -------------------- private final Mechanism2d mech = new Mechanism2d(5, 10); @@ -46,6 +48,9 @@ public class RobotContainer { private final Autos autos = new Autos(superstructure, drivetrain); private final AutoChooser autoChooser = new AutoChooser(); + private final ButtonBoardPub publisher = new ButtonBoardPub(logger); + // private final ButtonBoardSub subscriber = new ButtonBoardSub(logger); + CommandXboxController xbox = new CommandXboxController(0); Joystick buttonBoard = new Joystick(1); @@ -72,11 +77,11 @@ public RobotContainer() { public void periodic(){ drivetrain.periodic(); - + // subscriber.periodic(); + // autoScoring } private void configureBindings() { - drivetrain.registerTelemetry(logger::telemeterize); drivetrain.setDefaultCommand( drivetrain.applyRequest(() -> @@ -106,15 +111,15 @@ private void configureBindings() { final int id = i; Trigger trig = new Trigger(() -> (id < 2) ? buttonBoard.getX() == Math.pow(-1.0, id + 1) : buttonBoard.getY() == Math.pow(-1.0, id)); - trig.whileTrue(new InstantCommand(() -> autoScoring.setLevel(id + 1))); + trig.whileTrue(new InstantCommand(() -> autoScoring.overrideLevel(id + 1))); } // Reef position bindings (remaining unchanged) - for (int i = 0; i < 12; i++) { - JoystickButton button = new JoystickButton(buttonBoard, i+1); - final int id = i; - button.whileTrue(new InstantCommand(() -> autoScoring.setBranch(id))); - } + // for (int i = 0; i < 12; i++) { + // JoystickButton button = new JoystickButton(buttonBoard, i+1); + // final int id = i; + // button.whileTrue(new InstantCommand(() -> autoScoring.setBranch(id))); + // } xbox.rightTrigger() .whileTrue(autoScoring.autoScore(superstructure)) @@ -124,6 +129,6 @@ private void configureBindings() { xbox.y() .whileTrue(autoScoring.autoScoreNoDrive(superstructure).onlyIf(superstructure.getCoral()::hasCoral)); - xbox.rightStick().whileTrue(new InstantCommand(autoScoring::updateClosestReefPos)); + // xbox.rightStick().whileTrue(new InstantCommand(autoScoring::updateClosestReefPos)); } } \ No newline at end of file diff --git a/src/main/java/com/team9470/Telemetry.java b/src/main/java/com/team9470/Telemetry.java index 4dd9686..4118f29 100644 --- a/src/main/java/com/team9470/Telemetry.java +++ b/src/main/java/com/team9470/Telemetry.java @@ -1,124 +1,19 @@ package com.team9470; -import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.networktables.DoubleArrayPublisher; -import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.IntegerPublisher; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.networktables.StructArrayPublisher; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; public class Telemetry { - private final double MaxSpeed; - - /** - * Construct a telemetry object, with the specified max speed of the robot - * - * @param maxSpeed Maximum speed in meters per second - */ - public Telemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - SignalLogger.start(); - } - /* What to publish over networktables for telemetry */ private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); /* Robot swerve drive state */ private final NetworkTable driveStateTable = inst.getTable("DriveState"); - private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); - private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); - private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); - private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); - private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); - - /* Robot pose for field positioning */ - private final NetworkTable table = inst.getTable("Pose"); - private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); - private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); - - /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - }; - /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - }; - /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) - .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; - - private final double[] m_poseArray = new double[3]; - private final double[] m_moduleStatesArray = new double[8]; - private final double[] m_moduleTargetsArray = new double[8]; + /* For button board communications */ + // public final StringPublisher coralInfo = driveStateTable.getStringTopic("CoralInfo").publish(); + public final IntegerPublisher branch = driveStateTable.getIntegerTopic("branch").publish(); + public final IntegerPublisher level = driveStateTable.getIntegerTopic("level").publish(); /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the swerve drive state */ - drivePose.set(state.Pose); - driveSpeeds.set(state.Speeds); - driveModuleStates.set(state.ModuleStates); - driveModuleTargets.set(state.ModuleTargets); - driveModulePositions.set(state.ModulePositions); - driveTimestamp.set(state.Timestamp); - driveOdometryFrequency.set(1.0 / state.OdometryPeriod); - - /* Also write to log file */ - m_poseArray[0] = state.Pose.getX(); - m_poseArray[1] = state.Pose.getY(); - m_poseArray[2] = state.Pose.getRotation().getDegrees(); - for (int i = 0; i < 4; ++i) { - m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians(); - m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond; - m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians(); - m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; - } - - SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); - SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); - SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); - SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - - /* Telemeterize the pose to a Field2d */ - fieldTypePub.set("Field2d"); - fieldPub.set(m_poseArray); - - /* Telemeterize the module states to a Mechanism2d */ - for (int i = 0; i < 4; ++i) { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - - SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - } - } } diff --git a/src/main/java/com/team9470/commands/AutoScoring.java b/src/main/java/com/team9470/commands/AutoScoring.java index 3bfa6c6..dc01d50 100644 --- a/src/main/java/com/team9470/commands/AutoScoring.java +++ b/src/main/java/com/team9470/commands/AutoScoring.java @@ -15,21 +15,21 @@ import static edu.wpi.first.units.Units.Meters; public class AutoScoring { - - private CoralObjective coralObjective = AutoScoring.CoralObjective.NONE; - private final Swerve drivetrain; + private ScoringState scoringState = ScoringState.DEFAULT; + private static Swerve drivetrain; public AutoScoring(Swerve drivetrain){ - this.drivetrain = drivetrain; + AutoScoring.drivetrain = drivetrain; } public Command autoScore(Superstructure superstructure) { return new DeferredCommand(() -> { + CoralObjective coralObjective = scoringState.getOptimalObjective(); Command driveToScore = new DriveToPose(coralObjective::getScoringPose, drivetrain) .alongWith( new WaitUntilCommand(() -> closeEnough(coralObjective, Constants.DriverAssistConstants.RAISE_DISTANCE)) .andThen(superstructure.waitForIntake().asProxy()) - .andThen(superstructure.raise(coralObjective.level).asProxy()) + .andThen(superstructure.raise(scoringState.getLevel()).asProxy()) ); System.out.println("Scoring with coralobjective: " + coralObjective); return driveToScore.andThen(superstructure.getCoral().scoreCommand().asProxy()); @@ -59,7 +59,7 @@ public static Command autoScoreWithTimeout(Superstructure superstructure, CoralO return driveToScore.andThen(superstructure.getCoral().scoreCommand().withTimeout(timeout).asProxy()); } public Command autoScoreNoDrive(Superstructure superstructure) { - return new DeferredCommand(() -> superstructure.raise(coralObjective.level), Set.of(superstructure)).andThen(superstructure.score()); + return new DeferredCommand(() -> superstructure.raise(scoringState.getLevel()), Set.of(superstructure)).andThen(superstructure.score()); } private Pose2d computeDriveBackPose(Pose2d scoringPose) { @@ -69,7 +69,7 @@ private Pose2d computeDriveBackPose(Pose2d scoringPose) { } public Command driveBack(){ - Pose2d scoringPose = coralObjective.getScoringPose(); + Pose2d scoringPose = scoringState.getOptimalObjective().getScoringPose(); Pose2d driveBackPose = computeDriveBackPose(scoringPose); return new DriveToPose(() -> driveBackPose, drivetrain); @@ -77,13 +77,14 @@ public Command driveBack(){ } public Command autoAlgae(Superstructure superstructure){ - return new DriveToPose(() -> coralObjective.getScoringPose(), drivetrain) + CoralObjective coralObjective = scoringState.getOptimalObjective(); + return new DriveToPose(coralObjective::getScoringPose, drivetrain) .alongWith(new WaitUntilCommand(() -> closeEnough(coralObjective, Constants.DriverAssistConstants.RAISE_DISTANCE))) .andThen(new DeferredCommand(() -> superstructure.dealgify(coralObjective.getAlgaeLevel()), Set.of(superstructure))); } public Command autoAlgaeNoDrive(Superstructure superstructure){ - return new DeferredCommand(() -> superstructure.dealgify(coralObjective.getAlgaeLevel()), Set.of(superstructure)); + return new DeferredCommand(() -> superstructure.dealgify(scoringState.getOptimalObjective().getAlgaeLevel()), Set.of(superstructure)); } private static boolean closeEnough(CoralObjective objective, Distance distance){ @@ -92,36 +93,81 @@ private static boolean closeEnough(CoralObjective objective, Distance distance){ return currentPose.getTranslation().getDistance(scoringPose.getTranslation()) < distance.in(Meters); } - // Update the reef value used for automatic level selection. - public void setBranch(int branchId) { - coralObjective = coralObjective.updateBranchId(branchId); - System.out.println("Branch ID: " + branchId); + + // allows op to choose level + public void overrideLevel(int level) { + scoringState = scoringState.updateLevel(level); } - public void setLevel(int level) { - coralObjective = coralObjective.updateLevel(level); + // removes operator override of level + public void removeOverride(){ + scoringState = scoringState.updateLevel(0); } - public void updateClosestReefPos() { - Pose2d[] reefPoses = Constants.DriverAssistConstants.getReefPositions(); - // Get the current robot pose at initialization. - Pose2d currentPose = Swerve.getInstance().getPose(); + public void addCoral(int branchId, int level){ + scoringState = scoringState.updateCoral(branchId, level, true); + } + + public void removeCoral(int branchId, int level){ + scoringState = scoringState.updateCoral(branchId, level, false); + } + + public boolean[][] getCorals(){ + return scoringState.corals; + } + + // stores scored coral and current level + // if curLevel = 0, it will automatically set it + public record ScoringState(boolean[][] corals, int curLevel){ + public static final ScoringState DEFAULT = new ScoringState(new boolean[4][12], 0); + + public ScoringState updateCoral(int branchId, int level, boolean newValue){ + corals[level-1][branchId] = newValue; + return new ScoringState(corals, curLevel); + } - // Find the closest reef pose. - double shortestDistance = Double.MAX_VALUE; - int closestPoseId = -1; - for (int i = 0; i < 12; i++) { - Pose2d pose = reefPoses[i]; - double distance = currentPose.getTranslation().getDistance(pose.getTranslation()); - if (distance < shortestDistance) { - shortestDistance = distance; - closestPoseId = i; + public int getLevel(){ + if(curLevel == 0){ + for(int i = 3; i >= 0 ; i--){ + for(int j = 0; j < 12; j++){ + if(!corals[i][j]){ + return i+1; + } + } + } + return 1; } + return curLevel; } - setBranch(closestPoseId); - } + public ScoringState updateLevel(int newLevel){ + return new ScoringState(corals, newLevel); + } + public CoralObjective getOptimalObjective(){ + Pose2d[] reefPoses = Constants.DriverAssistConstants.getReefPositions(); + Pose2d currentPose = drivetrain.getPose(); + int level = getLevel(); + + // Find the closest reef pose. + double shortestDistance = Double.MAX_VALUE; + int closestPoseId = -1; + for (int i = 0; i < 12; i++) { + // if coral is already scored here, can't select! + if (corals[level-1][i]) { + continue; + } + Pose2d pose = reefPoses[i]; + double distance = currentPose.getTranslation().getDistance(pose.getTranslation()); + if (distance < shortestDistance) { + shortestDistance = distance; + closestPoseId = i; + } + } + return new CoralObjective(closestPoseId, level); + } + } + // Starting facing the driver station, moving counterclockwise public record CoralObjective(int branchId, int level){ public static final CoralObjective NONE = new CoralObjective(0, 0); @@ -139,13 +185,5 @@ public int getAlgaeLevel(){ // starting with (0, 1) = 3, then (2, 3) = 2, each reef alternates level between 2 and 3 return (branchId)/2 % 2 == 0 ? 3 : 2; } - - public CoralObjective updateBranchId(int branchId){ - return new CoralObjective(branchId, level); - } - - public CoralObjective updateLevel(int level){ - return new CoralObjective(branchId, level); - } } } diff --git a/src/main/java/com/team9470/subsystems/sim/ButtonBoardPub.java b/src/main/java/com/team9470/subsystems/sim/ButtonBoardPub.java new file mode 100644 index 0000000..ba009ae --- /dev/null +++ b/src/main/java/com/team9470/subsystems/sim/ButtonBoardPub.java @@ -0,0 +1,61 @@ +package com.team9470.subsystems.sim; + +import com.team9470.Telemetry; +import com.team9470.util.Util; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.google.gson.Gson; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +public class ButtonBoardPub extends SubsystemBase { + private static ButtonBoardPub mInstance; + // Joystick buttonBoard; + private final boolean[][] corals; + private final Telemetry logger; + public Gson gson = new Gson(); + + public static ButtonBoardPub getInstance(Telemetry logger) { + if (mInstance == null) { + mInstance = new ButtonBoardPub(logger); + } + + return mInstance; + } + + public ButtonBoardPub(Telemetry logger) { + this.logger = logger; + + // buttonBoard = new Joystick(1); + + // TODO: will actually be populated by website! + corals = new boolean[4][12]; + + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 12; j++) { + corals[i][j] = false; + } + } + + // branchId = 2 (NO SUBTRACT), level = 4 (- 1) + // corals[0][2] = true; + // corals[1][2] = true; + // corals[2][2] = true; + // corals[3][2] = true; + } + + @Override + public void periodic() { +// int opBranchId = 0; +// int level = 0; + +// corals[level][opBranchId] = true; +// // buildAutoChooser(String defaultAutoName, return chooser; } + +// /* converts a 2D boolean array to JSON to be sent across a string */ +// public static String Boolean2DArrayToJSON(boolean[][] data) { +// JSONArray jsonArray = new JSONArray(); +// +// for (boolean[] row : data) { +// jsonArray.put(new JSONArray(row)); +// } +// +// return jsonArray.toString(); +// } +// +// /* de-serializes JSONArray (String form) to a 2D boolean array */ +// public static boolean[][] BooleanJSONArrayStringTo2DArray(String jsonArrayString) { +// JSONArray jsonArray = new JSONArray(jsonArrayString); +// +// boolean[][] result = new boolean[12][3]; +// for (int i = 0; i < jsonArray.length(); i++) { +// JSONArray row = jsonArray.getJSONArray(i); +// result[i] = new boolean[row.length()]; +// for (int j = 0; j < row.length(); j++) { +// result[i][j] = row.getBoolean(j); +// } +// } +// +// return result; +// } }