Merge "Add inidicator switch and colors to ArmUI"
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index c547833..e308954 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -188,7 +188,7 @@
data = glob([
"field_images/*.png",
"field_images/*.svg",
- ]),
+ ]) + ["//third_party/y2023/field:pictures"],
legacy_create_init = False,
target_compatible_with = ["@platforms//cpu:x86_64"],
visibility = ["//visibility:public"],
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
index b2d9d57..3a61b5e 100644
--- a/frc971/control_loops/python/constants.py
+++ b/frc971/control_loops/python/constants.py
@@ -15,6 +15,9 @@
ROBOT_SIDE_TO_HATCH_PANEL = 0.1
HATCH_PANEL_WIDTH = 0.4826
+# field_id is either just a file prefix for a .png in field_images/ or is a
+# full path preceded by // specifying a location relative to the root of the
+# repository.
FieldType = namedtuple(
'Field', ['name', 'tags', 'year', 'width', 'length', 'robot', 'field_id'])
RobotType = namedtuple("Robot", ['width', 'length'])
@@ -33,6 +36,7 @@
Robot2020 = RobotType(width=0.8128, length=0.8636) # 32 in x 34 in
Robot2021 = Robot2020
Robot2022 = RobotType(width=0.8763, length=0.96647)
+Robot2023 = RobotType(width=0.8763, length=0.96647)
FIELDS = {
"2019 Field":
@@ -115,9 +119,17 @@
length=8.2296,
robot=Robot2022,
field_id="2022"),
+ "2023 Field":
+ FieldType("2023 Field",
+ tags=[],
+ year=2023,
+ width=16.59255,
+ length=8.10895,
+ robot=Robot2023,
+ field_id="//third_party/y2023/field/2023.png"),
}
-FIELD = FIELDS["2022 Field"]
+FIELD = FIELDS["2023 Field"]
def get_json_folder(field):
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 2b55e94..86777e5 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -81,9 +81,13 @@
def set_field(self, field):
self.field = field
try:
- self.field_png = cairo.ImageSurface.create_from_png(
- "frc971/control_loops/python/field_images/" +
- self.field.field_id + ".png")
+ if self.field.field_id.startswith('//'):
+ self.field_png = cairo.ImageSurface.create_from_png(
+ self.field.field_id[2:])
+ else:
+ self.field_png = cairo.ImageSurface.create_from_png(
+ "frc971/control_loops/python/field_images/" +
+ self.field.field_id + ".png")
except cairo.Error:
self.field_png = None
diff --git a/frc971/control_loops/python/spline_graph.py b/frc971/control_loops/python/spline_graph.py
index fbe43bf..ce5efe1 100755
--- a/frc971/control_loops/python/spline_graph.py
+++ b/frc971/control_loops/python/spline_graph.py
@@ -107,7 +107,7 @@
self.file_name_box = Gtk.Entry()
self.file_name_box.set_size_request(50, 40)
- self.file_name_box.set_text(FIELD.field_id + ".json")
+ self.file_name_box.set_text("test.json")
self.file_name_box.set_editable(True)
self.long_input = Gtk.SpinButton()
diff --git a/y2023/www/2023.png b/third_party/y2023/field/2023.png
similarity index 100%
rename from y2023/www/2023.png
rename to third_party/y2023/field/2023.png
Binary files differ
diff --git a/third_party/y2023/field/BUILD b/third_party/y2023/field/BUILD
index e7ba4e2..de1d382 100644
--- a/third_party/y2023/field/BUILD
+++ b/third_party/y2023/field/BUILD
@@ -1,11 +1,14 @@
-# Pictures from FIRST modified by Tea Fazio.
-# https://firstfrc.blob.core.windows.net/frc2023/Manual/2023FRCGameManual.pdf
-# Copyright 2023 FIRST
-
filegroup(
name = "pictures",
srcs = [
- "field.jpg",
- ],
+ # Picture from the FIRST inspires field drawings.
+ # https://www.firstinspires.org/robotics/frc/playing-field
+ # Copyright 2023 FIRST
+ "2023.png",
+ # Picture from FIRST modified by Tea Fazio.
+ # https://firstfrc.blob.core.windows.net/frc2023/Manual/2023FRCGameManual.pdf
+ # Copyright 2023 FIRST
+ "field.jpg",
+ ],
visibility = ["//visibility:public"],
)
diff --git a/y2023/control_loops/superstructure/arm/arm.cc b/y2023/control_loops/superstructure/arm/arm.cc
index 6cd8d0d..fd3028c 100644
--- a/y2023/control_loops/superstructure/arm/arm.cc
+++ b/y2023/control_loops/superstructure/arm/arm.cc
@@ -292,7 +292,6 @@
follower_.Update(X_hat, disable, constants::Values::kArmDt(), vmax_,
max_operating_voltage);
- AOS_LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
arm_ekf_.Predict(follower_.U().head<2>(), constants::Values::kArmDt());
roll_joint_loop_.UpdateObserver(follower_.U().tail<1>(),
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 68fe6af..4c4b1e3 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -43,7 +43,7 @@
namespace joysticks {
// TODO(milind): add correct locations
-const ButtonLocation kScore(4, 4);
+const ButtonLocation kDriverSpit(2, 1);
const ButtonLocation kSpit(4, 13);
const ButtonLocation kHighConeScoreLeft(4, 14);
@@ -68,6 +68,7 @@
const ButtonLocation kBack(4, 12);
const ButtonLocation kWrist(4, 10);
+const ButtonLocation kStayIn(3, 4);
namespace superstructure = y2023::control_loops::superstructure;
namespace arm = superstructure::arm;
@@ -386,9 +387,12 @@
// And, pull the bits out of it.
if (current_setpoint_ != nullptr) {
- wrist_goal = current_setpoint_->wrist_goal;
- arm_goal_position_ = current_setpoint_->index;
- score_wrist_goal = current_setpoint_->score_wrist_goal;
+ if (!data.IsPressed(kStayIn)) {
+ wrist_goal = current_setpoint_->wrist_goal;
+ arm_goal_position_ = current_setpoint_->index;
+ score_wrist_goal = current_setpoint_->score_wrist_goal;
+ }
+
placing_row = current_setpoint_->row_hint;
}
@@ -396,7 +400,7 @@
if (data.IsPressed(kSuck)) {
roller_goal = RollerGoal::INTAKE_LAST;
- } else if (data.IsPressed(kSpit)) {
+ } else if (data.IsPressed(kSpit) || data.IsPressed(kDriverSpit)) {
if (score_wrist_goal.has_value()) {
wrist_goal = score_wrist_goal.value();
diff --git a/y2023/www/BUILD b/y2023/www/BUILD
index f8b706c..09cd4d8 100644
--- a/y2023/www/BUILD
+++ b/y2023/www/BUILD
@@ -7,10 +7,17 @@
"**/*.html",
"**/*.css",
"**/*.png",
- ]),
+ ]) + ["2023.png"],
visibility = ["//visibility:public"],
)
+genrule(
+ name = "2023_field_png",
+ srcs = ["//third_party/y2023/field:pictures"],
+ outs = ["2023.png"],
+ cmd = "cp third_party/y2023/field/2023.png $@",
+)
+
ts_project(
name = "field_main",
srcs = [
@@ -25,6 +32,7 @@
"//aos/network/www:proxy",
"//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
"//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
+ "//y2023/control_loops/superstructure:superstructure_status_ts_fbs",
"//y2023/localizer:status_ts_fbs",
"//y2023/localizer:visualization_ts_fbs",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
diff --git a/y2023/www/constants.ts b/y2023/www/constants.ts
index b94d7a7..d6ecfaf 100644
--- a/y2023/www/constants.ts
+++ b/y2023/www/constants.ts
@@ -2,6 +2,7 @@
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;
+// 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/www/field.html b/y2023/www/field.html
index 72d8f54..52e0d11 100644
--- a/y2023/www/field.html
+++ b/y2023/www/field.html
@@ -34,6 +34,63 @@
<td id="images_accepted"> NA </td>
</tr>
</table>
+
+ <table>
+ <tr>
+ <th colspan="2">Superstructure</th>
+ </tr>
+ <tr>
+ <td>End Effector State</td>
+ <td id="end_effector_state"> NA </td>
+ </tr>
+ <tr>
+ <td>Wrist</td>
+ <td id="wrist"> NA </td>
+ </tr>
+ </table>
+ <table>
+ <tr>
+ <th colspan="2">Game Piece</th>
+ </tr>
+ <tr>
+ <td>Game Piece Held</td>
+ <td id="game_piece"> NA </td>
+ </tr>
+ </table>
+
+ <table>
+ <tr>
+ <th colspan="2">Arm</th>
+ </tr>
+ <tr>
+ <td>State</td>
+ <td id="arm_state"> NA </td>
+ </tr>
+ <tr>
+ <td>X</td>
+ <td id="arm_x"> NA </td>
+ </tr>
+ <tr>
+ <td>Y</td>
+ <td id="arm_y"> NA </td>
+ </tr>
+ <tr>
+ <td>Circular Index</td>
+ <td id="arm_circular_index"> NA </td>
+ </tr>
+ <tr>
+ <td>Roll</td>
+ <td id="arm_roll"> NA </td>
+ </tr>
+ <tr>
+ <td>Proximal</td>
+ <td id="arm_proximal"> NA </td>
+ </tr>
+ <tr>
+ <td>Distal</td>
+ <td id="arm_distal"> NA </td>
+ </tr>
+ </table>
</div>
<div id="vision_readouts">
</div>
diff --git a/y2023/www/field_handler.ts b/y2023/www/field_handler.ts
index 9e2d9ed..db881af 100644
--- a/y2023/www/field_handler.ts
+++ b/y2023/www/field_handler.ts
@@ -3,6 +3,8 @@
import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated';
import {RejectionReason} from '../localizer/status_generated';
import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated';
+import {Status as SuperstructureStatus, EndEffectorState, ArmState, ArmStatus} from '../control_loops/superstructure/superstructure_status_generated'
+import {Class} from '../vision/game_pieces_generated'
import {Visualization, TargetEstimateDebug} from '../localizer/visualization_generated';
import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
@@ -21,6 +23,7 @@
private canvas = document.createElement('canvas');
private localizerOutput: LocalizerOutput|null = null;
private drivetrainStatus: DrivetrainStatus|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.
@@ -33,6 +36,26 @@
(document.getElementById('images_accepted') as HTMLElement);
private rejectionReasonCells: HTMLElement[] = [];
private fieldImage: HTMLImageElement = new Image();
+ private endEffectorState: HTMLElement =
+ (document.getElementById('end_effector_state') as HTMLElement);
+ private wrist: HTMLElement =
+ (document.getElementById('wrist') as HTMLElement);
+ private armState: HTMLElement =
+ (document.getElementById('arm_state') as HTMLElement);
+ private gamePiece: HTMLElement =
+ (document.getElementById('game_piece') as HTMLElement);
+ private armX: HTMLElement =
+ (document.getElementById('arm_x') as HTMLElement);
+ private armY: HTMLElement =
+ (document.getElementById('arm_y') as HTMLElement);
+ private circularIndex: HTMLElement =
+ (document.getElementById('arm_circular_index') as HTMLElement);
+ private roll: HTMLElement =
+ (document.getElementById('arm_roll') as HTMLElement);
+ private proximal: HTMLElement =
+ (document.getElementById('arm_proximal') as HTMLElement);
+ private distal: HTMLElement =
+ (document.getElementById('arm_distal') as HTMLElement);
constructor(private readonly connection: Connection) {
(document.getElementById('field') as HTMLElement).appendChild(this.canvas);
@@ -81,6 +104,11 @@
'/localizer', "frc971.controls.LocalizerOutput", (data) => {
this.handleLocalizerOutput(data);
});
+ this.connection.addHandler(
+ '/superstructure', "y2023.control_loops.superstructure.Status",
+ (data) => {
+ this.handleSuperstructureStatus(data)
+ });
});
}
@@ -117,6 +145,11 @@
this.drivetrainStatus = DrivetrainStatus.getRootAsStatus(fbBuffer);
}
+ private handleSuperstructureStatus(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.superstructureStatus = SuperstructureStatus.getRootAsStatus(fbBuffer);
+ }
+
drawField(): void {
const ctx = this.canvas.getContext('2d');
ctx.save();
@@ -179,6 +212,25 @@
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');
@@ -193,6 +245,39 @@
// Draw the matches with debugging information from the localizer.
const now = Date.now() / 1000.0;
+ if (this.superstructureStatus) {
+ this.endEffectorState.innerHTML =
+ EndEffectorState[this.superstructureStatus.endEffectorState()];
+ if (!this.superstructureStatus.wrist() ||
+ !this.superstructureStatus.wrist().zeroed()) {
+ this.setZeroing(this.wrist);
+ } else if (this.superstructureStatus.wrist().estopped()) {
+ this.setEstopped(this.wrist);
+ } else {
+ this.setTargetValue(
+ this.wrist,
+ this.superstructureStatus.wrist().unprofiledGoalPosition(),
+ this.superstructureStatus.wrist().estimatorState().position(),
+ 1e-3);
+ }
+ this.armState.innerHTML =
+ ArmState[this.superstructureStatus.arm().state()];
+ this.gamePiece.innerHTML =
+ Class[this.superstructureStatus.gamePiece()];
+ this.armX.innerHTML =
+ this.superstructureStatus.arm().armX().toFixed(2);
+ this.armY.innerHTML =
+ this.superstructureStatus.arm().armY().toFixed(2);
+ this.circularIndex.innerHTML =
+ this.superstructureStatus.arm().armCircularIndex().toFixed(0);
+ this.roll.innerHTML =
+ this.superstructureStatus.arm().rollJointEstimatorState().position().toFixed(2);
+ this.proximal.innerHTML =
+ this.superstructureStatus.arm().proximalEstimatorState().position().toFixed(2);
+ this.distal.innerHTML =
+ this.superstructureStatus.arm().distalEstimatorState().position().toFixed(2);
+ }
+
if (this.drivetrainStatus && this.drivetrainStatus.trajectoryLogging()) {
this.drawRobot(
this.drivetrainStatus.trajectoryLogging().x(),
