Merge "Adding check for valid charuco diamond markers"
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/scouting/deploy/BUILD b/scouting/deploy/BUILD
index eb8b537..c8f68c4 100644
--- a/scouting/deploy/BUILD
+++ b/scouting/deploy/BUILD
@@ -23,8 +23,8 @@
# So we work around it by manually adding some symlinks that let us pretend
# that we're at the root of the runfiles tree.
symlinks = {
- "opt/frc971/scouting_server/org_frc971": ".",
- "opt/frc971/scouting_server/bazel_tools": "external/bazel_tools",
+ "org_frc971": ".",
+ "bazel_tools": "external/bazel_tools",
},
)
diff --git a/scouting/deploy/deploy.py b/scouting/deploy/deploy.py
index f947398..61c0481 100644
--- a/scouting/deploy/deploy.py
+++ b/scouting/deploy/deploy.py
@@ -47,6 +47,8 @@
"-c 'create schema public;'",
# List all tables as a sanity check.
"-c '\dt'",
+ # Make sure we make the visualizations accessible.
+ "-c 'GRANT ALL ON SCHEMA public TO tableau;'",
"postgres\"",
]),
shell=True,
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/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index 521fc09..063e972 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -162,6 +162,7 @@
"//y2023/constants:constants_fbs",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
"//y2023/control_loops/superstructure:superstructure_status_fbs",
+ "//y2023/vision:game_pieces_fbs",
],
)
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
index aacfbbf..16a0090 100644
--- a/y2023/control_loops/drivetrain/target_selector.cc
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -3,6 +3,7 @@
#include "aos/containers/sized_array.h"
#include "frc971/shooter_interpolation/interpolation.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2023/vision/game_pieces_generated.h"
namespace y2023::control_loops::drivetrain {
namespace {
@@ -15,7 +16,8 @@
: joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
hint_fetcher_(event_loop->MakeFetcher<TargetSelectorHint>("/drivetrain")),
- superstructure_status_fetcher_(event_loop->MakeFetcher<superstructure::Status>("/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<superstructure::Status>("/superstructure")),
status_sender_(
event_loop->MakeSender<TargetSelectorStatus>("/drivetrain")),
constants_fetcher_(event_loop) {
@@ -33,12 +35,14 @@
LateralOffsetForTimeOfFlight(msg.cone_position());
});
- event_loop->AddPhasedLoop([this](int){
- auto builder = status_sender_.MakeBuilder();
- auto status_builder = builder.MakeBuilder<TargetSelectorStatus>();
- status_builder.add_game_piece_position(game_piece_position_);
- builder.CheckOk(builder.Send(status_builder.Finish()));
- }, std::chrono::milliseconds(100));
+ event_loop->AddPhasedLoop(
+ [this](int) {
+ auto builder = status_sender_.MakeBuilder();
+ auto status_builder = builder.MakeBuilder<TargetSelectorStatus>();
+ status_builder.add_game_piece_position(game_piece_position_);
+ builder.CheckOk(builder.Send(status_builder.Finish()));
+ },
+ std::chrono::milliseconds(100));
}
void TargetSelector::UpdateAlliance() {
@@ -180,10 +184,13 @@
superstructure_status_fetcher_.Fetch();
if (superstructure_status_fetcher_.get() != nullptr) {
switch (superstructure_status_fetcher_->game_piece()) {
- case superstructure::GamePiece::NONE:
- case superstructure::GamePiece::CUBE:
+ case vision::Class::NONE:
+ case vision::Class::CUBE:
return 0.0;
- case superstructure::GamePiece::CONE:
+ case vision::Class::CONE_UP:
+ // execute logic below.
+ break;
+ case vision::Class::CONE_DOWN:
// execute logic below.
break;
}
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index f18a0b6..5faada9 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -13,7 +13,7 @@
gi.require_version('Gtk', '3.0')
from gi.repository import Gdk, Gtk
import cairo
-from y2023.control_loops.python.graph_tools import to_theta, to_xy, alpha_blend, shift_angles
+from y2023.control_loops.python.graph_tools import to_theta, to_xy, alpha_blend, shift_angles, get_xy
from y2023.control_loops.python.graph_tools import l1, l2, joint_center
from y2023.control_loops.python.graph_tools import DRIVER_CAM_POINTS
from y2023.control_loops.python import graph_paths
@@ -192,10 +192,45 @@
DRIVER_CAM_HEIGHT = DRIVER_CAM_POINTS[-1][1] - DRIVER_CAM_POINTS[0][1]
+class SegmentSelector(basic_window.BaseWindow):
+
+ def __init__(self, segments):
+ super(SegmentSelector, self).__init__()
+
+ self.window = Gtk.Window()
+ self.window.set_title("Segment Selector")
+
+ self.segments = segments
+
+ self.segment_store = Gtk.ListStore(int, str)
+
+ for i, segment in enumerate(segments):
+ self.segment_store.append([i, segment.name])
+
+ self.segment_box = Gtk.ComboBox.new_with_model_and_entry(
+ self.segment_store)
+ self.segment_box.connect("changed", self.on_combo_changed)
+ self.segment_box.set_entry_text_column(1)
+
+ self.current_path_index = None
+
+ self.window.add(self.segment_box)
+ self.window.show_all()
+
+ def on_combo_changed(self, combo):
+ iter = combo.get_active_iter()
+
+ if iter is not None:
+ model = combo.get_model()
+ id, name = model[iter][:2]
+ print("Selected: ID=%d, name=%s" % (id, name))
+ self.current_path_index = id
+
+
# Create a GTK+ widget on which we will draw using Cairo
class ArmUi(basic_window.BaseWindow):
- def __init__(self):
+ def __init__(self, segments):
super(ArmUi, self).__init__()
self.window = Gtk.Window()
@@ -221,7 +256,7 @@
self.circular_index_select = 1
# Extra stuff for drawing lines.
- self.segments = []
+ self.segments = segments
self.prev_segment_pt = None
self.now_segment_pt = None
self.spline_edit = 0
@@ -247,6 +282,11 @@
[DRIVER_CAM_X, DRIVER_CAM_Y],
DRIVER_CAM_WIDTH, DRIVER_CAM_HEIGHT)
+ self.segment_selector = SegmentSelector(self.segments)
+ self.segment_selector.show()
+
+ self.show_indicators = True
+
def _do_button_press_internal(self, event):
o_x = event.x
o_y = event.y
@@ -295,6 +335,8 @@
# Handle the expose-event by drawing
def handle_draw(self, cr):
# use "with px(cr): blah;" to transform to pixel coordinates.
+ if self.segment_selector.current_path_index is not None:
+ self.index = self.segment_selector.current_path_index
# Fill the background color of the window with grey
set_color(cr, palette["GREY"])
@@ -378,6 +420,7 @@
random.shuffle(color)
set_color(cr, Color(color[0], color[1], color[2]))
self.segments[i].DrawTo(cr, self.theta_version)
+
with px(cr):
cr.stroke()
@@ -385,6 +428,26 @@
color = [0, 0, 0]
set_color(cr, Color(color[0], color[1], color[2]))
self.segments[self.index].DrawTo(cr, self.theta_version)
+
+ with px(cr):
+ cr.stroke()
+ control1 = get_xy(self.segments[self.index].control1)
+ control2 = get_xy(self.segments[self.index].control2)
+
+ if self.theta_version:
+ control1 = shift_angles(self.segments[self.index].control1)
+ control2 = shift_angles(self.segments[self.index].control2)
+
+ if self.show_indicators:
+ set_color(cr, Color(1.0, 0.0, 1.0))
+ cr.move_to(control1[0] + 0.02, control1[1])
+ cr.arc(control1[0], control1[1], 0.02, 0, 2.0 * np.pi)
+ with px(cr):
+ cr.stroke()
+ set_color(cr, Color(1.0, 0.7, 0.0))
+ cr.move_to(control2[0] + 0.02, control2[1])
+ cr.arc(control2[0], control2[1], 0.02, 0, 2.0 * np.pi)
+
with px(cr):
cr.stroke()
@@ -498,6 +561,9 @@
print("Switched to segment:", self.segments[self.index].name)
self.segments[self.index].Print(graph_paths.points)
+ elif keyval == Gdk.KEY_i:
+ self.show_indicators = not self.show_indicators
+
elif keyval == Gdk.KEY_n:
self.index += 1
self.index = self.index % len(self.segments)
@@ -563,8 +629,7 @@
self.redraw()
-arm_ui = ArmUi()
-arm_ui.segments = graph_paths.segments
+arm_ui = ArmUi(graph_paths.segments)
print('Starting with segment: ', arm_ui.segments[arm_ui.index].name)
arm_ui.segments[arm_ui.index].Print(graph_paths.points)
basic_window.RunApp()
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index f7571a6..12a8967 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -1,3 +1,5 @@
+import sys
+
import numpy as np
from y2023.control_loops.python.graph_tools import *
@@ -419,3 +421,26 @@
back_points = []
unnamed_segments = []
segments = named_segments + unnamed_segments
+
+# This checks that all points are unique
+
+seen_segments = []
+
+for segment in segments:
+ # check for equality of the start and end values
+
+ if (segment.start.tolist(), segment.end.tolist()) in seen_segments:
+ print("Repeated value")
+ segment.Print(points)
+ sys.exit(1)
+ else:
+ seen_segments.append((segment.start.tolist(), segment.end.tolist()))
+
+seen_points = []
+
+for point in points:
+ if point in seen_points:
+ print(f"Repeated value {point}")
+ sys.exit(1)
+ else:
+ seen_points.append(point)
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index a4cb337..2700bbc 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -33,6 +33,7 @@
deps = [
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
+ "//y2023/vision:game_pieces_fbs",
],
)
@@ -44,6 +45,7 @@
deps = [
"//frc971/control_loops:control_loops_ts_fbs",
"//frc971/control_loops:profiled_subsystem_ts_fbs",
+ "//y2023/vision:game_pieces_ts_fbs",
],
)
@@ -76,6 +78,7 @@
"//aos/time",
"//frc971/control_loops:control_loop",
"//y2023:constants",
+ "//y2023/vision:game_pieces_fbs",
],
)
@@ -145,6 +148,7 @@
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2023/control_loops/superstructure/roll:roll_plants",
+ "//y2023/vision:game_pieces_fbs",
],
)
diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index 44b2aac..39e549b 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -22,6 +22,7 @@
"//y2023/control_loops/superstructure/arm:arm_constants",
"//y2023/control_loops/superstructure/arm:trajectory",
"//y2023/control_loops/superstructure/roll:roll_plants",
+ "//y2023/vision:game_pieces_fbs",
],
)
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/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
index 287f0e7..4d5d43e 100644
--- a/y2023/control_loops/superstructure/end_effector.cc
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -3,6 +3,7 @@
#include "aos/events/event_loop.h"
#include "aos/time/time.h"
#include "frc971/control_loops/control_loop.h"
+#include "y2023/vision/game_pieces_generated.h"
namespace y2023 {
namespace control_loops {
@@ -12,7 +13,7 @@
EndEffector::EndEffector()
: state_(EndEffectorState::IDLE),
- game_piece_(GamePiece::NONE),
+ game_piece_(vision::Class::NONE),
timer_(aos::monotonic_clock::min_time),
beambreak_(false) {}
@@ -25,16 +26,22 @@
constexpr double kMinCurrent = 40.0;
constexpr double kMaxConePosition = 0.92;
- bool beambreak_status = (beambreak || (falcon_current > kMinCurrent &&
- cone_position < kMaxConePosition));
-
// Let them switch game pieces
- if (roller_goal == RollerGoal::INTAKE_CONE) {
- game_piece_ = GamePiece::CONE;
+ if (roller_goal == RollerGoal::INTAKE_CONE_UP) {
+ game_piece_ = vision::Class::CONE_UP;
+ } else if (roller_goal == RollerGoal::INTAKE_CONE_DOWN) {
+ game_piece_ = vision::Class::CONE_DOWN;
} else if (roller_goal == RollerGoal::INTAKE_CUBE) {
- game_piece_ = GamePiece::CUBE;
+ game_piece_ = vision::Class::CUBE;
}
+ bool beambreak_status =
+ (((game_piece_ == vision::Class::CUBE ||
+ game_piece_ == vision::Class::CONE_UP) &&
+ beambreak) ||
+ ((game_piece_ == vision::Class::CONE_DOWN &&
+ falcon_current > kMinCurrent && cone_position < kMaxConePosition)));
+
// Go into spitting if we were told to, no matter where we are
if (roller_goal == RollerGoal::SPIT && state_ != EndEffectorState::SPITTING) {
state_ = EndEffectorState::SPITTING;
@@ -47,7 +54,8 @@
switch (state_) {
case EndEffectorState::IDLE:
// If idle and intake requested, intake
- if (roller_goal == RollerGoal::INTAKE_CONE ||
+ if (roller_goal == RollerGoal::INTAKE_CONE_UP ||
+ roller_goal == RollerGoal::INTAKE_CONE_DOWN ||
roller_goal == RollerGoal::INTAKE_CUBE ||
roller_goal == RollerGoal::INTAKE_LAST) {
state_ = EndEffectorState::INTAKING;
@@ -56,7 +64,8 @@
break;
case EndEffectorState::INTAKING:
// If intaking and beam break is not triggered, keep intaking
- if (roller_goal == RollerGoal::INTAKE_CONE ||
+ if (roller_goal == RollerGoal::INTAKE_CONE_UP ||
+ roller_goal == RollerGoal::INTAKE_CONE_DOWN ||
roller_goal == RollerGoal::INTAKE_CUBE ||
roller_goal == RollerGoal::INTAKE_LAST) {
timer_ = timestamp;
@@ -72,7 +81,7 @@
break;
}
- if (game_piece_ == GamePiece::CUBE) {
+ if (game_piece_ == vision::Class::CUBE) {
*roller_voltage = kRollerCubeSuckVoltage();
} else {
*roller_voltage = kRollerConeSuckVoltage();
@@ -88,7 +97,7 @@
break;
case EndEffectorState::SPITTING:
// If spit requested, spit
- if (game_piece_ == GamePiece::CUBE) {
+ if (game_piece_ == vision::Class::CUBE) {
*roller_voltage = kRollerCubeSpitVoltage();
} else {
*roller_voltage = kRollerConeSpitVoltage();
@@ -100,7 +109,7 @@
} else if (timestamp > timer_ + constants::Values::kExtraSpittingTime()) {
// Finished spitting
state_ = EndEffectorState::IDLE;
- game_piece_ = GamePiece::NONE;
+ game_piece_ = vision::Class::NONE;
}
break;
diff --git a/y2023/control_loops/superstructure/end_effector.h b/y2023/control_loops/superstructure/end_effector.h
index 14245c8..5ae96da 100644
--- a/y2023/control_loops/superstructure/end_effector.h
+++ b/y2023/control_loops/superstructure/end_effector.h
@@ -7,6 +7,7 @@
#include "y2023/constants.h"
#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2023/vision/game_pieces_generated.h"
namespace y2023 {
namespace control_loops {
@@ -26,12 +27,12 @@
double cone_position, bool beambreak,
double *intake_roller_voltage);
EndEffectorState state() const { return state_; }
- GamePiece game_piece() const { return game_piece_; }
+ vision::Class game_piece() const { return game_piece_; }
void Reset();
private:
EndEffectorState state_;
- GamePiece game_piece_;
+ vision::Class game_piece_;
aos::monotonic_clock::time_point timer_;
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
index ee99f1c..670351a 100644
--- a/y2023/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -4,10 +4,11 @@
enum RollerGoal: ubyte {
IDLE = 0,
- INTAKE_CONE = 1,
+ INTAKE_CONE_UP = 1,
INTAKE_CUBE = 2,
INTAKE_LAST = 3,
SPIT = 4,
+ INTAKE_CONE_DOWN = 5,
}
table Goal {
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index ee440ed..e2524c1 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -562,10 +562,10 @@
class SuperstructureBeambreakTest
: public SuperstructureTest,
- public ::testing::WithParamInterface<GamePiece> {
+ public ::testing::WithParamInterface<vision::Class> {
public:
- void SetBeambreak(GamePiece game_piece, bool status) {
- if (game_piece == GamePiece::CONE) {
+ void SetBeambreak(vision::Class game_piece, bool status) {
+ if (game_piece == vision::Class::CONE_UP) {
// TODO(milind): handle cone
} else {
superstructure_plant_.set_end_effector_cube_beam_break(status);
@@ -577,12 +577,20 @@
SetEnabled(true);
WaitUntilZeroed();
- double spit_voltage =
- (GetParam() == GamePiece::CUBE ? EndEffector::kRollerCubeSpitVoltage()
- : EndEffector::kRollerConeSpitVoltage());
- double suck_voltage =
- (GetParam() == GamePiece::CUBE ? EndEffector::kRollerCubeSuckVoltage()
- : EndEffector::kRollerConeSuckVoltage());
+ double spit_voltage = (GetParam() == vision::Class::CUBE
+ ? EndEffector::kRollerCubeSpitVoltage()
+ : EndEffector::kRollerConeSpitVoltage());
+ double suck_voltage = (GetParam() == vision::Class::CUBE
+ ? EndEffector::kRollerCubeSuckVoltage()
+ : EndEffector::kRollerConeSuckVoltage());
+
+ RollerGoal roller_goal = RollerGoal::INTAKE_CUBE;
+
+ if (GetParam() == vision::Class::CONE_DOWN) {
+ roller_goal = RollerGoal::INTAKE_CONE_DOWN;
+ } else if (GetParam() == vision::Class::CONE_UP) {
+ roller_goal = RollerGoal::INTAKE_CONE_UP;
+ }
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -591,9 +599,7 @@
goal_builder.add_arm_goal_position(arm::NeutralIndex());
goal_builder.add_trajectory_override(false);
- goal_builder.add_roller_goal(GetParam() == GamePiece::CONE
- ? RollerGoal::INTAKE_CONE
- : RollerGoal::INTAKE_CUBE);
+ goal_builder.add_roller_goal(roller_goal);
builder.CheckOk(builder.Send(goal_builder.Finish()));
}
@@ -668,9 +674,7 @@
goal_builder.add_arm_goal_position(arm::NeutralIndex());
goal_builder.add_trajectory_override(false);
- goal_builder.add_roller_goal(GetParam() == GamePiece::CONE
- ? RollerGoal::INTAKE_CONE
- : RollerGoal::INTAKE_CUBE);
+ goal_builder.add_roller_goal(roller_goal);
builder.CheckOk(builder.Send(goal_builder.Finish()));
}
@@ -757,7 +761,7 @@
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
EndEffectorState::IDLE);
- EXPECT_EQ(superstructure_status_fetcher_->game_piece(), GamePiece::NONE);
+ EXPECT_EQ(superstructure_status_fetcher_->game_piece(), vision::Class::NONE);
}
// Tests that we don't freak out without a goal.
@@ -834,7 +838,7 @@
// TODO(milind): add cone
INSTANTIATE_TEST_SUITE_P(EndEffectorGoal, SuperstructureBeambreakTest,
- ::testing::Values(GamePiece::CUBE));
+ ::testing::Values(vision::Class::CUBE));
} // namespace testing
} // namespace superstructure
diff --git a/y2023/control_loops/superstructure/superstructure_status.fbs b/y2023/control_loops/superstructure/superstructure_status.fbs
index 80a0d3d..5381b0a 100644
--- a/y2023/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023/control_loops/superstructure/superstructure_status.fbs
@@ -1,4 +1,5 @@
include "frc971/control_loops/control_loops.fbs";
+include "y2023/vision/game_pieces.fbs";
include "frc971/control_loops/profiled_subsystem.fbs";
namespace y2023.control_loops.superstructure;
@@ -76,12 +77,6 @@
SPITTING = 3,
}
-enum GamePiece : ubyte {
- NONE = 0,
- CONE = 1,
- CUBE = 2,
-}
-
table Status {
// All subsystems know their location.
zeroed:bool (id: 0);
@@ -94,7 +89,7 @@
wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
end_effector_state:EndEffectorState (id: 4);
- game_piece:GamePiece (id: 5);
+ game_piece:vision.Class (id: 5);
}
root_type Status;
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 43962dd..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;
@@ -334,10 +335,10 @@
std::optional<double> score_wrist_goal = std::nullopt;
if (data.IsPressed(kGroundPickupConeUp) || data.IsPressed(kHPConePickup)) {
- roller_goal = RollerGoal::INTAKE_CONE;
+ roller_goal = RollerGoal::INTAKE_CONE_UP;
current_game_piece_ = GamePiece::CONE_UP;
} else if (data.IsPressed(kGroundPickupConeDownBase)) {
- roller_goal = RollerGoal::INTAKE_CONE;
+ roller_goal = RollerGoal::INTAKE_CONE_DOWN;
current_game_piece_ = GamePiece::CONE_DOWN;
} else if (data.IsPressed(kGroundPickupCube)) {
roller_goal = RollerGoal::INTAKE_CUBE;
@@ -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/localizer/localizer.cc b/y2023/localizer/localizer.cc
index 3a6de93..ffc1df3 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -166,7 +166,7 @@
}
event_loop_->AddPhasedLoop([this](int) { SendOutput(); },
- std::chrono::milliseconds(5));
+ std::chrono::milliseconds(20));
event_loop_->MakeWatcher(
"/drivetrain",
diff --git a/y2023/localizer/localizer_test.cc b/y2023/localizer/localizer_test.cc
index 947771f..57b9dd0 100644
--- a/y2023/localizer/localizer_test.cc
+++ b/y2023/localizer/localizer_test.cc
@@ -332,7 +332,9 @@
// correctly.
TEST_F(LocalizerTest, NominalSpinInPlace) {
output_voltages_ << -1.0, 1.0;
- event_loop_factory_.RunFor(std::chrono::seconds(2));
+ // Go 1 ms over 2 sec to make sure we actually see relatively recent messages
+ // on each channel.
+ event_loop_factory_.RunFor(std::chrono::milliseconds(2001));
CHECK(output_fetcher_.Fetch());
CHECK(status_fetcher_.Fetch());
// The two can be different because they may've been sent at different
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index e90b825..68ba833 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -1,4 +1,5 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
cc_binary(
name = "camera_reader",
@@ -239,3 +240,10 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
)
+
+flatbuffer_ts_library(
+ name = "game_pieces_ts_fbs",
+ srcs = ["game_pieces.fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2023/vision/game_pieces.fbs b/y2023/vision/game_pieces.fbs
index 773cc93..89f7505 100644
--- a/y2023/vision/game_pieces.fbs
+++ b/y2023/vision/game_pieces.fbs
@@ -2,9 +2,10 @@
// Object class.
enum Class : byte {
- CONE_DOWN,
- CONE_UP,
- CUBE
+ NONE = 0,
+ CONE_UP = 1,
+ CUBE = 2,
+ CONE_DOWN = 3,
}
// Bounding box dimensions and position.
@@ -27,4 +28,4 @@
best_piece:uint (id: 1); // Index of the "best piece".
}
-root_type GamePieces;
\ No newline at end of file
+root_type GamePieces;
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(),
diff --git a/y2023/y2023_imu.json b/y2023/y2023_imu.json
index 07c3b08..96134f8 100644
--- a/y2023/y2023_imu.json
+++ b/y2023/y2023_imu.json
@@ -312,7 +312,7 @@
"name": "/localizer",
"type": "frc971.controls.LocalizerOutput",
"source_node": "imu",
- "frequency": 210,
+ "frequency": 52,
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
"roborio"
@@ -334,7 +334,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "imu",
"logger": "NOT_LOGGED",
- "frequency": 210,
+ "frequency": 52,
"num_senders": 2,
"max_size": 200
},
diff --git a/y2023/y2023_logger.json b/y2023/y2023_logger.json
index 861372a..f4ac45e 100644
--- a/y2023/y2023_logger.json
+++ b/y2023/y2023_logger.json
@@ -21,26 +21,6 @@
]
},
{
- "name": "/drivetrain",
- "type": "frc971.control_loops.drivetrain.Position",
- "source_node": "roborio",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 2,
- "time_to_live": 500000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "roborio"
- ]
- }
- ]
- },
- {
"name": "/logger/camera",
"type": "y2023.vision.GamePieces",
"source_node": "logger",
@@ -73,44 +53,6 @@
"max_size": 200
},
{
- "name": "/roborio/aos/remote_timestamps/logger/drivetrain/frc971-control_loops-drivetrain-Position",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "roborio",
- "logger": "NOT_LOGGED",
- "frequency": 400,
- "num_senders": 2,
- "max_size": 200
- },
- {
- "name": "/drivetrain",
- "type": "frc971.control_loops.drivetrain.Output",
- "source_node": "roborio",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 2,
- "time_to_live": 500000000,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "roborio"
- ]
- }
- ]
- },
- {
- "name": "/roborio/aos/remote_timestamps/logger/drivetrain/frc971-control_loops-drivetrain-Output",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "roborio",
- "logger": "NOT_LOGGED",
- "frequency": 400,
- "num_senders": 2,
- "max_size": 400
- },
- {
"name": "/pi1/aos",
"type": "aos.message_bridge.Timestamp",
"source_node": "pi1",
