Adding field plot for 2022
Adds some bare minimum status information, mostly focused on getting the
camera readings showing on the screen.
Note that this code is largely borrowed from the 2020 field viewing
code.
TODO: Add debug information for superstructure state machine/catapult.
Change-Id: I896a3def0d8d8b0ef2fb132086eaba1ba8ec0c61
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/BUILD b/y2022/BUILD
index 1a5f692..98723af 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -300,3 +300,15 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
)
+
+sh_binary(
+ name = "log_web_proxy",
+ srcs = ["log_web_proxy.sh"],
+ data = [
+ ":aos_config",
+ "//aos/network:log_web_proxy_main",
+ "//y2022/www:field_main_bundle.min.js",
+ "//y2022/www:files",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
diff --git a/y2022/localizer/BUILD b/y2022/localizer/BUILD
index 9e8c3d4..1a57caf 100644
--- a/y2022/localizer/BUILD
+++ b/y2022/localizer/BUILD
@@ -25,6 +25,13 @@
visibility = ["//visibility:public"],
)
+flatbuffer_ts_library(
+ name = "localizer_output_ts_fbs",
+ srcs = ["localizer_output.fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
flatbuffer_cc_library(
name = "localizer_status_fbs",
srcs = [
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index ea80e73..2b3c72c 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -978,6 +978,7 @@
output_builder.add_x(model_based_.xytheta()(0));
output_builder.add_y(model_based_.xytheta()(1));
output_builder.add_theta(model_based_.xytheta()(2));
+ output_builder.add_zeroed(zeroer_.Zeroed());
const Eigen::Quaterniond &orientation = model_based_.orientation();
Quaternion quaternion;
quaternion.mutate_x(orientation.x());
diff --git a/y2022/localizer/localizer_output.fbs b/y2022/localizer/localizer_output.fbs
index 03abf19..b07278b 100644
--- a/y2022/localizer/localizer_output.fbs
+++ b/y2022/localizer/localizer_output.fbs
@@ -21,6 +21,9 @@
theta:double (id: 3);
// Current estimate of the robot's 3-D rotation.
orientation:Quaternion (id: 4);
+ // Whether we have zeroed the IMU (may go false if we observe a fault with the
+ // IMU).
+ zeroed:bool (id: 5);
}
root_type LocalizerOutput;
diff --git a/y2022/localizer/localizer_test.cc b/y2022/localizer/localizer_test.cc
index 58df869..3c11dff 100644
--- a/y2022/localizer/localizer_test.cc
+++ b/y2022/localizer/localizer_test.cc
@@ -398,12 +398,20 @@
}
{
auto builder = turret_sender_.MakeBuilder();
+ auto turret_estimator_builder =
+ builder
+ .MakeBuilder<frc971::PotAndAbsoluteEncoderEstimatorState>();
+ turret_estimator_builder.add_position(turret_position_);
+ const flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ turret_estimator_offset = turret_estimator_builder.Finish();
auto turret_builder =
builder
.MakeBuilder<frc971::control_loops::
PotAndAbsoluteEncoderProfiledJointStatus>();
turret_builder.add_position(turret_position_);
turret_builder.add_velocity(turret_velocity_);
+ turret_builder.add_zeroed(true);
+ turret_builder.add_estimator_state(turret_estimator_offset);
const auto turret_offset = turret_builder.Finish();
auto status_builder =
builder
@@ -784,6 +792,53 @@
status_fetcher_->model_based()->statistics()->total_accepted());
}
+// Tests that image corrections are ignored when the turret moves too fast.
+TEST_F(EventLoopLocalizerTest, ImageCorrectionsTurretTooFast) {
+ output_voltages_ << 0.0, 0.0;
+ drivetrain_plant_.mutable_state()->x() = 2.0;
+ drivetrain_plant_.mutable_state()->y() = 2.0;
+ SendLocalizerControl(5.0, 3.0, 0.0);
+ turret_velocity_ = 10.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(output_fetcher_.Fetch());
+ ASSERT_NEAR(5.0, output_fetcher_->x(), 1e-5);
+ ASSERT_NEAR(3.0, output_fetcher_->y(), 1e-5);
+ ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-5);
+
+ send_targets_ = true;
+
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ CHECK(output_fetcher_.Fetch());
+ ASSERT_NEAR(5.0, output_fetcher_->x(), 1e-5);
+ ASSERT_NEAR(3.0, output_fetcher_->y(), 1e-5);
+ ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-5);
+ ASSERT_TRUE(status_fetcher_->model_based()->has_statistics());
+ ASSERT_LT(10,
+ status_fetcher_->model_based()->statistics()->total_candidates());
+ ASSERT_EQ(0, status_fetcher_->model_based()->statistics()->total_accepted());
+ ASSERT_EQ(status_fetcher_->model_based()->statistics()->total_candidates(),
+ status_fetcher_->model_based()
+ ->statistics()
+ ->rejection_reason_count()
+ ->Get(static_cast<int>(RejectionReason::TURRET_TOO_FAST)));
+ // We expect one more rejection to occur due to the time it takes all the
+ // information to propagate.
+ const int rejected_count =
+ status_fetcher_->model_based()->statistics()->total_candidates() + 1;
+ // Check that when we go back to being still we do successfully converge.
+ turret_velocity_ = 0.0;
+ turret_position_ = 1.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(4));
+ CHECK(status_fetcher_.Fetch());
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-1));
+ ASSERT_TRUE(status_fetcher_->model_based()->has_statistics());
+ ASSERT_EQ(status_fetcher_->model_based()->statistics()->total_candidates(),
+ rejected_count +
+ status_fetcher_->model_based()->statistics()->total_accepted());
+}
+
// Tests that image corrections when we are in accel mode works.
TEST_F(EventLoopLocalizerTest, ImageCorrectionsInAccel) {
output_voltages_ << 0.0, 0.0;
diff --git a/y2022/log_web_proxy.sh b/y2022/log_web_proxy.sh
new file mode 100755
index 0000000..cb945ab
--- /dev/null
+++ b/y2022/log_web_proxy.sh
@@ -0,0 +1 @@
+./aos/network/log_web_proxy_main --data_dir=y2022/www $@
diff --git a/y2022/www/2022.png b/y2022/www/2022.png
new file mode 100644
index 0000000..68087bd
--- /dev/null
+++ b/y2022/www/2022.png
Binary files differ
diff --git a/y2022/www/BUILD b/y2022/www/BUILD
index 55cbde2..9ee56e3 100644
--- a/y2022/www/BUILD
+++ b/y2022/www/BUILD
@@ -1,3 +1,5 @@
+load("@npm//@bazel/typescript:index.bzl", "ts_library")
+load("//tools/build_rules:js.bzl", "rollup_bundle")
load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
filegroup(
@@ -5,13 +7,44 @@
srcs = glob([
"**/*.html",
"**/*.css",
+ "**/*.png",
]),
visibility = ["//visibility:public"],
)
+ts_library(
+ name = "field_main",
+ srcs = [
+ "constants.ts",
+ "field_handler.ts",
+ "field_main.ts",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network:connect_ts_fbs",
+ "//aos/network:web_proxy_ts_fbs",
+ "//aos/network/www:proxy",
+ "//y2022/control_loops/superstructure:superstructure_status_ts_fbs",
+ "//y2022/localizer:localizer_output_ts_fbs",
+ "//y2022/localizer:localizer_visualization_ts_fbs",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+ ],
+)
+
+rollup_bundle(
+ name = "field_main_bundle",
+ entry_point = "field_main.ts",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2022:__subpackages__"],
+ deps = [
+ ":field_main",
+ ],
+)
+
aos_downloader_dir(
name = "www_files",
srcs = [
+ ":field_main_bundle",
":files",
"//frc971/analysis:plot_index_bundle.min.js",
],
diff --git a/y2022/www/constants.ts b/y2022/www/constants.ts
new file mode 100644
index 0000000..b94d7a7
--- /dev/null
+++ b/y2022/www/constants.ts
@@ -0,0 +1,7 @@
+// Conversion constants to meters
+export const IN_TO_M = 0.0254;
+export const FT_TO_M = 0.3048;
+// Dimensions of the field in meters
+export const FIELD_WIDTH = 26 * FT_TO_M + 11.25 * IN_TO_M;
+export const FIELD_LENGTH = 52 * FT_TO_M + 5.25 * IN_TO_M;
+
diff --git a/y2022/www/field.html b/y2022/www/field.html
new file mode 100644
index 0000000..8b5f0ff
--- /dev/null
+++ b/y2022/www/field.html
@@ -0,0 +1,74 @@
+<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">Aiming</th>
+ </tr>
+ <tr>
+ <td>Shot distance</td>
+ <td id="shot_distance"> NA </td>
+ </tr>
+ <tr>
+ <td>Turret</td>
+ <td id="turret"> NA </td>
+ </tr>
+ </table>
+
+ <table>
+ <tr>
+ <th colspan="2">Intakes</th>
+ </tr>
+ <tr>
+ <td>Front Intake</td>
+ <td id="front_intake"> NA </td>
+ </tr>
+ <tr>
+ <td>Back Intake</td>
+ <td id="back_intake"> NA </td>
+ </tr>
+ </table>
+
+ <table>
+ <tr>
+ <th colspan="2">Images</th>
+ </tr>
+ <tr>
+ <td> Images Accepted </td>
+ <td id="images_accepted"> NA </td>
+ </tr>
+ <tr>
+ <td> Images Rejected </td>
+ <td id="images_rejected"> NA </td>
+ </tr>
+ </table>
+ </div>
+ <div id="vision_readouts">
+ </div>
+ </body>
+</html>
+
diff --git a/y2022/www/field_handler.ts b/y2022/www/field_handler.ts
new file mode 100644
index 0000000..7603820
--- /dev/null
+++ b/y2022/www/field_handler.ts
@@ -0,0 +1,390 @@
+import * as web_proxy from 'org_frc971/aos/network/web_proxy_generated';
+import {Connection} from 'org_frc971/aos/network/www/proxy';
+import * as flatbuffers_builder from 'org_frc971/external/com_github_google_flatbuffers/ts/builder';
+import {ByteBuffer} from 'org_frc971/external/com_github_google_flatbuffers/ts/byte-buffer';
+import * as localizer from 'org_frc971/y2022/localizer/localizer_visualization_generated';
+import * as output from 'org_frc971/y2022/localizer/localizer_output_generated';
+import * as ss from 'org_frc971/y2022/control_loops/superstructure/superstructure_status_generated'
+
+import LocalizerVisualization = localizer.frc971.controls.LocalizerVisualization;
+import LocalizerOutput = output.frc971.controls.LocalizerOutput;
+import RejectionReason = localizer.frc971.controls.RejectionReason;
+import TargetEstimateDebug = localizer.frc971.controls.TargetEstimateDebug;
+import SuperstructureStatus = ss.y2022.control_loops.superstructure.Status;
+
+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 = 34 * IN_TO_M;
+const ROBOT_LENGTH = 36 * IN_TO_M;
+
+const PI_COLORS = ['#ff00ff', '#ffff00', '#00ffff', '#ffa500'];
+
+export class FieldHandler {
+ private canvas = document.createElement('canvas');
+ private localizerOutput: LocalizerOutput|null = null;
+ private superstructureStatus: SuperstructureStatus|null = null;
+
+ // Image information indexed by timestamp (seconds since the epoch), so that
+ // we can stop displaying images after a certain amount of time.
+ private localizerImageMatches = new Map<number, LocalizerVisualization>();
+ private outerTarget: HTMLElement =
+ (document.getElementById('outer_target') as HTMLElement);
+ private innerTarget: HTMLElement =
+ (document.getElementById('inner_target') as HTMLElement);
+ private x: HTMLElement = (document.getElementById('x') as HTMLElement);
+ private y: HTMLElement = (document.getElementById('y') as HTMLElement);
+ private theta: HTMLElement =
+ (document.getElementById('theta') as HTMLElement);
+ private shotDistance: HTMLElement =
+ (document.getElementById('shot_distance') as HTMLElement);
+ private turret: HTMLElement =
+ (document.getElementById('turret') as HTMLElement);
+ private frontIntake: HTMLElement =
+ (document.getElementById('front_intake') as HTMLElement);
+ private backIntake: HTMLElement =
+ (document.getElementById('back_intake') as HTMLElement);
+ private imagesAcceptedCounter: HTMLElement =
+ (document.getElementById('images_accepted') as HTMLElement);
+ private imagesRejectedCounter: HTMLElement =
+ (document.getElementById('images_rejected') as HTMLElement);
+ private rejectionReasonCells: HTMLElement[] = [];
+ private fieldImage: HTMLImageElement = new Image();
+
+ constructor(private readonly connection: Connection) {
+ (document.getElementById('field') as HTMLElement).appendChild(this.canvas);
+
+ this.fieldImage.src = "2022.png";
+
+ for (const value in RejectionReason) {
+ // Typescript generates an iterator that produces both numbers and
+ // strings... don't do anything on the string iterations.
+ if (isNaN(Number(value))) {
+ continue;
+ }
+ const row = document.createElement('div');
+ const nameCell = document.createElement('div');
+ nameCell.innerHTML = RejectionReason[value];
+ row.appendChild(nameCell);
+ const valueCell = document.createElement('div');
+ valueCell.innerHTML = 'NA';
+ this.rejectionReasonCells.push(valueCell);
+ row.appendChild(valueCell);
+ document.getElementById('vision_readouts').appendChild(row);
+ }
+
+ for (let ii = 0; ii < PI_COLORS.length; ++ii) {
+ const legendEntry = document.createElement('div');
+ legendEntry.style.color = PI_COLORS[ii];
+ legendEntry.innerHTML = 'PI' + (ii + 1).toString()
+ document.getElementById('legend').appendChild(legendEntry);
+ }
+
+ this.connection.addConfigHandler(() => {
+ this.connection.addHandler(
+ '/localizer', LocalizerVisualization.getFullyQualifiedName(),
+ (data) => {
+ this.handleLocalizerDebug(data);
+ });
+ this.connection.addHandler(
+ '/localizer', LocalizerOutput.getFullyQualifiedName(), (data) => {
+ this.handleLocalizerOutput(data);
+ });
+ this.connection.addHandler(
+ '/superstructure', SuperstructureStatus.getFullyQualifiedName(),
+ (data) => {
+ this.handleSuperstructureStatus(data);
+ });
+ });
+ }
+
+ private handleLocalizerDebug(data: Uint8Array): void {
+ const now = Date.now() / 1000.0;
+
+ const fbBuffer = new ByteBuffer(data);
+ this.localizerImageMatches.set(
+ now,
+ LocalizerVisualization.getRootAsLocalizerVisualization(
+ fbBuffer as unknown as flatbuffers.ByteBuffer));
+
+ const debug = this.localizerImageMatches.get(now);
+
+ if (debug.statistics()) {
+ this.imagesAcceptedCounter.innerHTML =
+ debug.statistics().totalAccepted().toString();
+ this.imagesRejectedCounter.innerHTML =
+ (debug.statistics().totalCandidates() -
+ debug.statistics().totalAccepted())
+ .toString();
+ if (debug.statistics().rejectionReasonCountLength() ==
+ this.rejectionReasonCells.length) {
+ for (let ii = 0; ii < debug.statistics().rejectionReasonCountLength();
+ ++ii) {
+ this.rejectionReasonCells[ii].innerHTML =
+ debug.statistics().rejectionReasonCount(ii).toString();
+ }
+ } else {
+ console.error('Unexpected number of rejection reasons in counter.');
+ }
+ this.imagesRejectedCounter.innerHTML =
+ (debug.statistics().totalCandidates() -
+ debug.statistics().totalAccepted())
+ .toString();
+ }
+ }
+
+ private handleLocalizerOutput(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.localizerOutput = LocalizerOutput.getRootAsLocalizerOutput(
+ fbBuffer as unknown as flatbuffers.ByteBuffer);
+ }
+
+ private handleSuperstructureStatus(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.superstructureStatus = SuperstructureStatus.getRootAsStatus(
+ fbBuffer as unknown as flatbuffers.ByteBuffer);
+ }
+
+ drawField(): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.drawImage(
+ this.fieldImage, 0, 0, this.fieldImage.width, this.fieldImage.height,
+ -FIELD_EDGE_X, -FIELD_SIDE_Y, FIELD_LENGTH, FIELD_WIDTH);
+ }
+
+ drawCamera(
+ x: number, y: number, theta: number, color: string = 'blue',
+ extendLines: boolean = true): 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);
+ if (extendLines) {
+ ctx.lineTo(100.0, 0);
+ 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, turret: number|null,
+ color: string = 'blue', dashed: boolean = false,
+ extendLines: boolean = true): 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);
+ if (extendLines) {
+ ctx.lineTo(1000.0, 0);
+ } else {
+ ctx.lineTo(ROBOT_LENGTH / 2.0, 0);
+ }
+ ctx.stroke();
+
+ if (turret !== null) {
+ ctx.save();
+ ctx.rotate(turret);
+ const turretRadius = ROBOT_WIDTH / 3.0;
+ ctx.strokeStyle = 'red';
+ // Draw circle for turret.
+ ctx.beginPath();
+ ctx.arc(0, 0, turretRadius, 0, 2.0 * Math.PI);
+ ctx.stroke();
+ // Draw line in circle to show forwards.
+ ctx.beginPath();
+ ctx.moveTo(0, 0);
+ if (extendLines) {
+ ctx.lineTo(1000.0, 0);
+ } else {
+ ctx.lineTo(turretRadius, 0);
+ }
+ ctx.stroke();
+ ctx.restore();
+ }
+ 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');
+ }
+
+ setTargetValue(
+ div: HTMLElement, target: number, val: number, tolerance: number): void {
+ div.innerHTML = val.toFixed(4);
+ div.classList.remove('faulted');
+ div.classList.remove('zeroing');
+ if (Math.abs(target - val) < tolerance) {
+ div.classList.add('near');
+ } else {
+ 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;
+ for (const [time, value] of this.localizerImageMatches) {
+ const age = now - time;
+ const kRemovalAge = 2.0;
+ if (age > kRemovalAge) {
+ this.localizerImageMatches.delete(time);
+ continue;
+ }
+ const ageAlpha = (kRemovalAge - age) / kRemovalAge
+ for (let i = 0; i < value.targetsLength(); i++) {
+ const imageDebug = value.targets(i);
+ const x = imageDebug.impliedRobotX();
+ const y = imageDebug.impliedRobotY();
+ const theta = imageDebug.impliedRobotTheta();
+ const cameraX = imageDebug.cameraX();
+ const cameraY = imageDebug.cameraY();
+ const cameraTheta = imageDebug.cameraTheta();
+ const accepted = imageDebug.accepted();
+ // Make camera readings fade over time.
+ const alpha = Math.round(255 * ageAlpha).toString(16).padStart(2, '0');
+ const dashed = false;
+ const acceptedRgb = accepted ? '#00FF00' : '#FF0000';
+ const acceptedRgba = acceptedRgb + alpha;
+ const cameraRgb = PI_COLORS[imageDebug.camera()];
+ const cameraRgba = cameraRgb + alpha;
+ this.drawRobot(x, y, theta, null, acceptedRgba, dashed, false);
+ this.drawCamera(cameraX, cameraY, cameraTheta, cameraRgba, false);
+ }
+ }
+
+ if (this.localizerOutput) {
+ if (!this.localizerOutput.zeroed()) {
+ this.setZeroing(this.x);
+ this.setZeroing(this.y);
+ this.setZeroing(this.theta);
+ } else {
+ this.setValue(this.x, this.localizerOutput.x());
+ this.setValue(this.y, this.localizerOutput.y());
+ this.setValue(this.theta, this.localizerOutput.theta());
+ }
+
+ if (this.superstructureStatus) {
+ this.shotDistance.innerHTML = this.superstructureStatus.aimer() ?
+ this.superstructureStatus.aimer().shotDistance().toFixed(2) :
+ 'NA';
+
+ if (!this.superstructureStatus.turret() ||
+ !this.superstructureStatus.turret().zeroed()) {
+ this.setZeroing(this.turret);
+ } else if (this.superstructureStatus.turret().estopped()) {
+ this.setEstopped(this.turret);
+ } else {
+ this.setTargetValue(
+ this.turret,
+ this.superstructureStatus.turret().unprofiledGoalPosition(),
+ this.superstructureStatus.turret().estimatorState().position(),
+ 1e-3);
+ }
+
+ if (!this.superstructureStatus.intakeBack() ||
+ !this.superstructureStatus.intakeBack().zeroed()) {
+ this.setZeroing(this.backIntake);
+ } else if (this.superstructureStatus.intakeBack().estopped()) {
+ this.setEstopped(this.backIntake);
+ } else {
+ this.setValue(
+ this.backIntake,
+ this.superstructureStatus.intakeBack()
+ .estimatorState()
+ .position());
+ }
+
+ if (!this.superstructureStatus.intakeFront() ||
+ !this.superstructureStatus.intakeFront().zeroed()) {
+ this.setZeroing(this.frontIntake);
+ } else if (this.superstructureStatus.intakeFront().estopped()) {
+ this.setEstopped(this.frontIntake);
+ } else {
+ this.setValue(
+ this.frontIntake,
+ this.superstructureStatus.intakeFront()
+ .estimatorState()
+ .position());
+ }
+ }
+
+
+ this.drawRobot(
+ this.localizerOutput.x(), this.localizerOutput.y(),
+ this.localizerOutput.theta(),
+ this.superstructureStatus ?
+ this.superstructureStatus.turret().position() :
+ null);
+ }
+
+ 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/y2022/www/field_main.ts b/y2022/www/field_main.ts
new file mode 100644
index 0000000..7e2e392
--- /dev/null
+++ b/y2022/www/field_main.ts
@@ -0,0 +1,12 @@
+import {Connection} from 'org_frc971/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/y2022/www/index.html b/y2022/www/index.html
index 70442f9..e4e185e 100644
--- a/y2022/www/index.html
+++ b/y2022/www/index.html
@@ -1,5 +1,6 @@
<html>
<body>
+ <a href="field.html">Field Visualization</a><br>
<a href="plotter.html">Plots</a>
</body>
</html>
