Add superstructure status to y2022 field_handler
Could definitely be prettied up a bit (e.g., making the true/false
fields also have background colors, or something, or maybe even
a little diagram showing where in the robot we think the ball is).
Change-Id: I248d46c59d8b67f044aa06355d89f18640987024
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 5dd5ddf..0d7b4ca 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -356,7 +356,7 @@
unlink(FLAGS_output_folder.c_str());
logger_event_loop_ = MakeEventLoop("logger", roborio_);
logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
- logger_->StartLoggingLocalNamerOnRun(FLAGS_output_folder);
+ logger_->StartLoggingOnRun(FLAGS_output_folder);
}
}
diff --git a/y2022/www/field.html b/y2022/www/field.html
index 8b5f0ff..e2a1cf6 100644
--- a/y2022/www/field.html
+++ b/y2022/www/field.html
@@ -41,6 +41,58 @@
<table>
<tr>
+ <th colspan="2">Catapult</th>
+ </tr>
+ <tr>
+ <td>Fire</td>
+ <td id="fire"> NA </td>
+ </tr>
+ <tr>
+ <td>Solve Time</td>
+ <td id="mpc_solve_time"> NA </td>
+ </tr>
+ <tr>
+ <td>MPC Active</td>
+ <td id="mpc_active"> NA </td>
+ </tr>
+ <tr>
+ <td>Shot Count</td>
+ <td id="shot_count"> NA </td>
+ </tr>
+ <tr>
+ <td>Position</td>
+ <td id="catapult"> NA </td>
+ </tr>
+ </table>
+
+ <table>
+ <tr>
+ <th colspan="2">Superstructure</th>
+ </tr>
+ <tr>
+ <td>State</td>
+ <td id="superstructure_state"> NA </td>
+ </tr>
+ <tr>
+ <td>Intake State</td>
+ <td id="intake_state"> NA </td>
+ </tr>
+ <tr>
+ <td>Reseating</td>
+ <td id="reseating_in_catapult"> NA </td>
+ </tr>
+ <tr>
+ <td>Flippers Open</td>
+ <td id="flippers_open"> NA </td>
+ </tr>
+ <tr>
+ <td>Climber</td>
+ <td id="climber"> NA </td>
+ </tr>
+ </table>
+
+ <table>
+ <tr>
<th colspan="2">Intakes</th>
</tr>
<tr>
diff --git a/y2022/www/field_handler.ts b/y2022/www/field_handler.ts
index 7603820..584b65e 100644
--- a/y2022/www/field_handler.ts
+++ b/y2022/www/field_handler.ts
@@ -11,6 +11,8 @@
import RejectionReason = localizer.frc971.controls.RejectionReason;
import TargetEstimateDebug = localizer.frc971.controls.TargetEstimateDebug;
import SuperstructureStatus = ss.y2022.control_loops.superstructure.Status;
+import SuperstructureState = ss.y2022.control_loops.superstructure.SuperstructureState;
+import IntakeState = ss.y2022.control_loops.superstructure.IntakeState;
import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
@@ -43,6 +45,26 @@
(document.getElementById('shot_distance') as HTMLElement);
private turret: HTMLElement =
(document.getElementById('turret') as HTMLElement);
+ private fire: HTMLElement =
+ (document.getElementById('fire') as HTMLElement);
+ private mpcSolveTime: HTMLElement =
+ (document.getElementById('mpc_solve_time') as HTMLElement);
+ private mpcActive: HTMLElement =
+ (document.getElementById('mpc_active') as HTMLElement);
+ private shotCount: HTMLElement =
+ (document.getElementById('shot_count') as HTMLElement);
+ private catapult: HTMLElement =
+ (document.getElementById('catapult') as HTMLElement);
+ private superstructureState: HTMLElement =
+ (document.getElementById('superstructure_state') as HTMLElement);
+ private intakeState: HTMLElement =
+ (document.getElementById('intake_state') as HTMLElement);
+ private reseatingInCatapult: HTMLElement =
+ (document.getElementById('reseating_in_catapult') as HTMLElement);
+ private flippersOpen: HTMLElement =
+ (document.getElementById('flippers_open') as HTMLElement);
+ private climber: HTMLElement =
+ (document.getElementById('climber') as HTMLElement);
private frontIntake: HTMLElement =
(document.getElementById('front_intake') as HTMLElement);
private backIntake: HTMLElement =
@@ -297,6 +319,98 @@
this.drawCamera(cameraX, cameraY, cameraTheta, cameraRgba, false);
}
}
+ if (this.superstructureStatus) {
+ this.shotDistance.innerHTML = this.superstructureStatus.aimer() ?
+ this.superstructureStatus.aimer().shotDistance().toFixed(2) :
+ 'NA';
+
+ this.fire.innerHTML = this.superstructureStatus.fire() ? 'true' : 'false';
+
+ this.mpcActive.innerHTML =
+ this.superstructureStatus.mpcActive() ? 'true' : 'false';
+
+ this.setValue(this.mpcSolveTime, this.superstructureStatus.solveTime());
+
+ this.shotCount.innerHTML =
+ this.superstructureStatus.shotCount().toFixed(0);
+
+ this.superstructureState.innerHTML =
+ SuperstructureState[this.superstructureStatus.state()];
+
+ this.intakeState.innerHTML =
+ IntakeState[this.superstructureStatus.intakeState()];
+
+ this.reseatingInCatapult.innerHTML =
+ this.superstructureStatus.reseatingInCatapult() ? 'true' : 'false';
+
+ this.flippersOpen.innerHTML =
+ this.superstructureStatus.flippersOpen() ? 'true' : 'false';
+
+ if (!this.superstructureStatus.catapult() ||
+ !this.superstructureStatus.catapult().zeroed()) {
+ this.setZeroing(this.catapult);
+ } else if (this.superstructureStatus.catapult().estopped()) {
+ this.setEstopped(this.catapult);
+ } else {
+ this.setTargetValue(
+ this.catapult,
+ this.superstructureStatus.catapult().unprofiledGoalPosition(),
+ this.superstructureStatus.catapult().estimatorState().position(),
+ 1e-3);
+ }
+
+ if (!this.superstructureStatus.climber() ||
+ !this.superstructureStatus.climber().zeroed()) {
+ this.setZeroing(this.climber);
+ } else if (this.superstructureStatus.climber().estopped()) {
+ this.setEstopped(this.climber);
+ } else {
+ this.setTargetValue(
+ this.climber,
+ this.superstructureStatus.climber().unprofiledGoalPosition(),
+ this.superstructureStatus.climber().estimatorState().position(),
+ 1e-3);
+ }
+
+
+
+ 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());
+ }
+ }
if (this.localizerOutput) {
if (!this.localizerOutput.zeroed()) {
@@ -309,52 +423,6 @@
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(),