Add y2023_bot3 folder
Signed-off-by: James (Peilun) Li <jamespeilunli@gmail.com>
Change-Id: I6fae2492d0497596a632efc79a6a258c0ca15f49
diff --git a/y2023_bot3/www/BUILD b/y2023_bot3/www/BUILD
new file mode 100644
index 0000000..50367ba
--- /dev/null
+++ b/y2023_bot3/www/BUILD
@@ -0,0 +1,22 @@
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
+
+filegroup(
+ name = "files",
+ srcs = glob([
+ "**/*.html",
+ "**/*.css",
+ "**/*.png",
+ ]),
+ visibility = ["//visibility:public"],
+)
+
+aos_downloader_dir(
+ name = "www_files",
+ srcs = [
+ ":files",
+ "//frc971/analysis:plot_index_bundle.min.js",
+ ],
+ dir = "www",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2023_bot3/www/constants.ts b/y2023_bot3/www/constants.ts
new file mode 100644
index 0000000..d6ecfaf
--- /dev/null
+++ b/y2023_bot3/www/constants.ts
@@ -0,0 +1,8 @@
+// Conversion constants to meters
+export const IN_TO_M = 0.0254;
+export const FT_TO_M = 0.3048;
+// Dimensions of the field in meters
+// Numbers are slightly hand-tuned to match the PNG that we are using.
+export const FIELD_WIDTH = 26 * FT_TO_M + 7.25 * IN_TO_M;
+export const FIELD_LENGTH = 54 * FT_TO_M + 5.25 * IN_TO_M;
+
diff --git a/y2023_bot3/www/field.html b/y2023_bot3/www/field.html
new file mode 100644
index 0000000..ee512d3
--- /dev/null
+++ b/y2023_bot3/www/field.html
@@ -0,0 +1,63 @@
+<html>
+ <head>
+ <script src="field_main_bundle.min.js" defer></script>
+ <link rel="stylesheet" href="styles.css">
+ </head>
+ <body>
+ <div id="field"> </div>
+ <div id="legend"> </div>
+ <div id="readouts">
+ <table>
+ <tr>
+ <th colspan="2">Robot State</th>
+ </tr>
+ <tr>
+ <td>X</td>
+ <td id="x"> NA </td>
+ </tr>
+ <tr>
+ <td>Y</td>
+ <td id="y"> NA </td>
+ </tr>
+ <tr>
+ <td>Theta</td>
+ <td id="theta"> NA </td>
+ </tr>
+ </table>
+
+ <table>
+ <tr>
+ <th colspan="2">Superstructure</th>
+ </tr>
+ </table>
+ <table>
+ <tr>
+ <th colspan="2">Game Piece</th>
+ </tr>
+ <tr>
+ <td>Game Piece Held</td>
+ <td id="game_piece"> NA </td>
+ </tr>
+ <tr>
+ <td>Game Piece Position (+ = left, 0 = empty)</td>
+ <td id="game_piece_position"> NA </td>
+ </tr>
+ </table>
+
+ <h3>Zeroing Faults:</h3>
+ <p id="zeroing_faults"> NA </p>
+ </div>
+ <div id="middle_readouts">
+ <div id="vision_readouts">
+ </div>
+ <div id="message_bridge_status">
+ <div>
+ <div>Node</div>
+ <div>Client</div>
+ <div>Server</div>
+ </div>
+ </div>
+ </div>
+ </body>
+</html>
+
diff --git a/y2023_bot3/www/field_handler.ts b/y2023_bot3/www/field_handler.ts
new file mode 100644
index 0000000..809bc5d
--- /dev/null
+++ b/y2023_bot3/www/field_handler.ts
@@ -0,0 +1,185 @@
+import {ByteBuffer} from 'flatbuffers'
+import {ClientStatistics} from '../../aos/network/message_bridge_client_generated'
+import {ServerStatistics, State as ConnectionState} from '../../aos/network/message_bridge_server_generated'
+import {Connection} from '../../aos/network/www/proxy'
+import {ZeroingError} from '../../frc971/control_loops/control_loops_generated'
+import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated'
+import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated'
+
+import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
+
+// (0,0) is field center, +X is toward red DS
+const FIELD_SIDE_Y = FIELD_WIDTH / 2;
+const FIELD_EDGE_X = FIELD_LENGTH / 2;
+
+const ROBOT_WIDTH = 25 * IN_TO_M;
+const ROBOT_LENGTH = 32 * IN_TO_M;
+
+export class FieldHandler {
+ private canvas = document.createElement('canvas');
+ private localizerOutput: LocalizerOutput|null = null;
+ private drivetrainStatus: DrivetrainStatus|null = null;
+
+ private handleDrivetrainStatus(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.drivetrainStatus = DrivetrainStatus.getRootAsStatus(fbBuffer);
+ }
+
+ private setCurrentNodeState(element: HTMLElement, state: ConnectionState):
+ void {
+ if (state === ConnectionState.CONNECTED) {
+ element.innerHTML = ConnectionState[state];
+ element.classList.remove('faulted');
+ element.classList.add('connected');
+ } else {
+ element.innerHTML = ConnectionState[state];
+ element.classList.remove('connected');
+ element.classList.add('faulted');
+ }
+ }
+
+ private handleServerStatistics(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ const serverStatistics =
+ ServerStatistics.getRootAsServerStatistics(fbBuffer);
+
+ for (let ii = 0; ii < serverStatistics.connectionsLength(); ++ii) {
+ const connection = serverStatistics.connections(ii);
+ const nodeName = connection.node().name();
+ if (!this.serverStatuses.has(nodeName)) {
+ this.populateNodeConnections(nodeName);
+ }
+ this.setCurrentNodeState(
+ this.serverStatuses.get(nodeName), connection.state());
+ }
+ }
+
+ private handleClientStatistics(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ const clientStatistics =
+ ClientStatistics.getRootAsClientStatistics(fbBuffer);
+
+ for (let ii = 0; ii < clientStatistics.connectionsLength(); ++ii) {
+ const connection = clientStatistics.connections(ii);
+ const nodeName = connection.node().name();
+ if (!this.clientStatuses.has(nodeName)) {
+ this.populateNodeConnections(nodeName);
+ }
+ this.setCurrentNodeState(
+ this.clientStatuses.get(nodeName), connection.state());
+ }
+ }
+
+ drawField(): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.scale(1.0, -1.0);
+ ctx.restore();
+ }
+
+ drawCamera(x: number, y: number, theta: number, color: string = 'blue'):
+ void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.translate(x, y);
+ ctx.rotate(theta);
+ ctx.strokeStyle = color;
+ ctx.beginPath();
+ ctx.moveTo(0.5, 0.5);
+ ctx.lineTo(0, 0);
+ ctx.lineTo(0.5, -0.5);
+ ctx.stroke();
+ ctx.beginPath();
+ ctx.arc(0, 0, 0.25, -Math.PI / 4, Math.PI / 4);
+ ctx.stroke();
+ ctx.restore();
+ }
+
+ drawRobot(
+ x: number, y: number, theta: number, color: string = 'blue',
+ dashed: boolean = false): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.translate(x, y);
+ ctx.rotate(theta);
+ ctx.strokeStyle = color;
+ ctx.lineWidth = ROBOT_WIDTH / 10.0;
+ if (dashed) {
+ ctx.setLineDash([0.05, 0.05]);
+ } else {
+ // Empty array = solid line.
+ ctx.setLineDash([]);
+ }
+ ctx.rect(-ROBOT_LENGTH / 2, -ROBOT_WIDTH / 2, ROBOT_LENGTH, ROBOT_WIDTH);
+ ctx.stroke();
+
+ // Draw line indicating which direction is forwards on the robot.
+ ctx.beginPath();
+ ctx.moveTo(0, 0);
+ ctx.lineTo(ROBOT_LENGTH / 2.0, 0);
+ ctx.stroke();
+
+ ctx.restore();
+ }
+
+ setZeroing(div: HTMLElement): void {
+ div.innerHTML = 'zeroing';
+ div.classList.remove('faulted');
+ div.classList.add('zeroing');
+ div.classList.remove('near');
+ }
+
+ setEstopped(div: HTMLElement): void {
+ div.innerHTML = 'estopped';
+ div.classList.add('faulted');
+ div.classList.remove('zeroing');
+ div.classList.remove('near');
+ }
+
+ setValue(div: HTMLElement, val: number): void {
+ div.innerHTML = val.toFixed(4);
+ div.classList.remove('faulted');
+ div.classList.remove('zeroing');
+ div.classList.remove('near');
+ }
+
+ draw(): void {
+ this.reset();
+ this.drawField();
+
+ // Draw the matches with debugging information from the localizer.
+ const now = Date.now() / 1000.0;
+
+ if (this.drivetrainStatus && this.drivetrainStatus.trajectoryLogging()) {
+ this.drawRobot(
+ this.drivetrainStatus.trajectoryLogging().x(),
+ this.drivetrainStatus.trajectoryLogging().y(),
+ this.drivetrainStatus.trajectoryLogging().theta(), '#000000FF',
+ false);
+ }
+
+ window.requestAnimationFrame(() => this.draw());
+ }
+
+ reset(): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.setTransform(1, 0, 0, 1, 0, 0);
+ const size = window.innerHeight * 0.9;
+ ctx.canvas.height = size;
+ const width = size / 2 + 20;
+ ctx.canvas.width = width;
+ ctx.clearRect(0, 0, size, width);
+
+ // Translate to center of display.
+ ctx.translate(width / 2, size / 2);
+ // Coordinate system is:
+ // x -> forward.
+ // y -> to the left.
+ ctx.rotate(-Math.PI / 2);
+ ctx.scale(1, -1);
+
+ const M_TO_PX = (size - 10) / FIELD_LENGTH;
+ ctx.scale(M_TO_PX, M_TO_PX);
+ ctx.lineWidth = 1 / M_TO_PX;
+ }
+}
diff --git a/y2023_bot3/www/field_main.ts b/y2023_bot3/www/field_main.ts
new file mode 100644
index 0000000..d71a45e
--- /dev/null
+++ b/y2023_bot3/www/field_main.ts
@@ -0,0 +1,12 @@
+import {Connection} from '../../aos/network/www/proxy';
+
+import {FieldHandler} from './field_handler';
+
+const conn = new Connection();
+
+conn.connect();
+
+const fieldHandler = new FieldHandler(conn);
+
+fieldHandler.draw();
+
diff --git a/y2023_bot3/www/index.html b/y2023_bot3/www/index.html
new file mode 100644
index 0000000..e4e185e
--- /dev/null
+++ b/y2023_bot3/www/index.html
@@ -0,0 +1,6 @@
+<html>
+ <body>
+ <a href="field.html">Field Visualization</a><br>
+ <a href="plotter.html">Plots</a>
+ </body>
+</html>
diff --git a/y2023_bot3/www/plotter.html b/y2023_bot3/www/plotter.html
new file mode 100644
index 0000000..629ceaa
--- /dev/null
+++ b/y2023_bot3/www/plotter.html
@@ -0,0 +1,7 @@
+<html>
+ <head>
+ <script src="plot_index_bundle.min.js" defer></script>
+ </head>
+ <body>
+ </body>
+</html>
diff --git a/y2023_bot3/www/styles.css b/y2023_bot3/www/styles.css
new file mode 100644
index 0000000..c2c44d2
--- /dev/null
+++ b/y2023_bot3/www/styles.css
@@ -0,0 +1,74 @@
+.channel {
+ display: flex;
+ border-bottom: 1px solid;
+ font-size: 24px;
+}
+#field {
+ display: inline-block
+}
+
+#readouts,
+#middle_readouts
+{
+ display: inline-block;
+ vertical-align: top;
+ float: right;
+}
+
+
+#legend {
+ display: inline-block;
+}
+
+table, th, td {
+ border: 1px solid black;
+ border-collapse: collapse;
+ padding: 5px;
+ margin: 10px;
+}
+
+th, td {
+ text-align: right;
+ width: 70px;
+}
+
+td:first-child {
+ width: 150px;
+}
+
+.connected, .near {
+ background-color: LightGreen;
+ border-radius: 10px;
+}
+
+.zeroing {
+ background-color: yellow;
+ border-radius: 10px;
+}
+
+.faulted {
+ background-color: red;
+ border-radius: 10px;
+}
+
+#vision_readouts > div {
+ display: table-row;
+ padding: 5px;
+}
+
+#vision_readouts > div > div {
+ display: table-cell;
+ padding: 5px;
+ text-align: right;
+}
+
+#message_bridge_status > div {
+ display: table-row;
+ padding: 5px;
+}
+
+#message_bridge_status > div > div {
+ display: table-cell;
+ padding: 5px;
+ text-align: right;
+}