Merge "Add led_indicator to 2023"
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..7bc45db 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.6061, length=0.77581)
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):
@@ -125,6 +137,8 @@
return "y2020/actors/splines"
elif field.year == 2022:
return "y2022/actors/splines"
+ elif field.year == 2023:
+ return "y2023/autonomous/splines"
else:
return "frc971/control_loops/python/spline_jsons"
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/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index ac708f6..89ebe19 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -24,6 +24,8 @@
DEFINE_uint32(
min_charucos, 10,
"The mininum number of aruco targets in charuco board required to match.");
+DEFINE_uint32(min_id, 12, "Minimum valid charuco id");
+DEFINE_uint32(max_id, 15, "Minimum valid charuco id");
DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
DEFINE_bool(
draw_axes, false,
@@ -426,24 +428,47 @@
square_length_ / marker_length_,
diamond_corners, diamond_ids);
- // Check to see if we found any diamond targets
- if (diamond_ids.size() > 0) {
- cv::aruco::drawDetectedDiamonds(rgb_image, diamond_corners,
- diamond_ids);
+ // Check that we have exactly one charuco diamond. For calibration, we
+ // can constrain things so that this is the case
+ if (diamond_ids.size() == 1) {
+ // TODO<Jim>: Could probably make this check more general than requiring
+ // range of ids
+ bool all_valid_ids = true;
+ for (uint i = 0; i < 4; i++) {
+ uint id = diamond_ids[0][i];
+ if ((id < FLAGS_min_id) || (id > FLAGS_max_id)) {
+ all_valid_ids = false;
+ LOG(INFO) << "Got invalid charuco id: " << id;
+ }
+ }
+ if (all_valid_ids) {
+ cv::aruco::drawDetectedDiamonds(rgb_image, diamond_corners,
+ diamond_ids);
- // estimate pose for diamonds doesn't return valid, so marking true
- valid = true;
- std::vector<cv::Vec3d> rvecs, tvecs;
- cv::aruco::estimatePoseSingleMarkers(
- diamond_corners, square_length_, calibration_.CameraIntrinsics(),
- calibration_.CameraDistCoeffs(), rvecs, tvecs);
- DrawTargetPoses(rgb_image, rvecs, tvecs);
+ // estimate pose for diamonds doesn't return valid, so marking true
+ valid = true;
+ std::vector<cv::Vec3d> rvecs, tvecs;
+ cv::aruco::estimatePoseSingleMarkers(
+ diamond_corners, square_length_, calibration_.CameraIntrinsics(),
+ calibration_.CameraDistCoeffs(), rvecs, tvecs);
+ DrawTargetPoses(rgb_image, rvecs, tvecs);
- PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
- result_ids = diamond_ids;
- result_corners = diamond_corners;
+ PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
+ result_ids = diamond_ids;
+ result_corners = diamond_corners;
+ } else {
+ LOG(INFO) << "Not all charuco ids were valid, so skipping";
+ }
} else {
- VLOG(2) << "Found aruco markers, but no charuco diamond targets";
+ if (diamond_ids.size() == 0) {
+ // OK to not see any markers sometimes
+ VLOG(2)
+ << "Found aruco markers, but no valid charuco diamond targets";
+ } else {
+ // But should never detect multiple
+ LOG(FATAL) << "Found multiple charuco diamond markers. Should only "
+ "be one";
+ }
}
} else {
LOG(FATAL) << "Unknown target type: "
diff --git a/scouting/deploy/scouting.service b/scouting/deploy/scouting.service
index 94582cd..2c55676 100644
--- a/scouting/deploy/scouting.service
+++ b/scouting/deploy/scouting.service
@@ -9,7 +9,7 @@
WorkingDirectory=/opt/frc971/scouting_server
Environment=RUNFILES_DIR=/opt/frc971/scouting_server
# Add "julia" to the PATH.
-Environment=PATH=/opt/frc971/scouting/julia_runtime/bin:/usr/local/bin:/usr/bin:/bin
+Environment=PATH=/opt/frc971/julia_runtime/bin:/usr/local/bin:/usr/bin:/bin
# Use the Julia cache set up by the frc971-scouting-julia package.
Environment=JULIA_DEPOT_PATH=/var/frc971/scouting/julia_depot/
Environment=JULIA_PROJECT=/opt/frc971/julia_manifest
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/BUILD b/y2023/BUILD
index a6fec71..d897c06 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -81,6 +81,7 @@
"//aos/network:web_proxy_main",
"//aos/starter:irq_affinity",
"//y2023/vision:camera_reader",
+ "//y2023/vision:image_logger",
"//aos/events/logging:logger_main",
"//y2023/vision:game_pieces_detector",
],
diff --git a/y2023/autonomous/splines/README.md b/y2023/autonomous/splines/README.md
new file mode 100644
index 0000000..c655416
--- /dev/null
+++ b/y2023/autonomous/splines/README.md
@@ -0,0 +1,3 @@
+# Spline Descriptions
+This folder contains reference material for what each spline does
+
diff --git a/y2023/autonomous/splines/spline_1.json b/y2023/autonomous/splines/spline_1.json
new file mode 100644
index 0000000..e6e24ed
--- /dev/null
+++ b/y2023/autonomous/splines/spline_1.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-6.450466090790975, -6.021160733648118, -5.591855376505261, -3.3652785421474576, -2.7749836760760287, -1.7732711760760287], "spline_y": [0.9493418961252269, 0.9493418961252269, 0.9314541729109411, 0.5975544198946889, 0.5975544198946889, 0.5796666966804032], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline_2.json b/y2023/autonomous/splines/spline_2.json
new file mode 100644
index 0000000..032a081
--- /dev/null
+++ b/y2023/autonomous/splines/spline_2.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-1.7732711760760287, -2.7749836760760287, -3.3652785421474576, -5.591855376505261, -6.021160733648118, -6.450466090790975], "spline_y": [0.5796666966804032, 0.5975544198946889, 0.5975544198946889, 0.40105062588141127, 0.41893834909569705, 0.41893834909569705], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline_3.json b/y2023/autonomous/splines/spline_3.json
new file mode 100644
index 0000000..4ca06a8
--- /dev/null
+++ b/y2023/autonomous/splines/spline_3.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-6.450466090790975, -6.021160733648118, -5.591855376505261, -3.605574338541678, -3.0269522872367363, -1.6929070022836754], "spline_y": [0.41893834909569705, 0.41893834909569705, 0.40105062588141127, 0.5475210271634618, 0.515375357646521, -0.3364848845524211], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline_4.json b/y2023/autonomous/splines/spline_4.json
new file mode 100644
index 0000000..a56d24e
--- /dev/null
+++ b/y2023/autonomous/splines/spline_4.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-1.6929070022836754, -3.0269522872367363, -3.605574338541678, -5.591855376505261, -6.021160733648118, -6.450466090790975], "spline_y": [-0.3364848845524211, 0.515375357646521, 0.5475210271634618, 0.40105062588141127, 0.41893834909569705, 0.41893834909569705], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/spline_5.json b/y2023/autonomous/splines/spline_5.json
new file mode 100644
index 0000000..4eee822
--- /dev/null
+++ b/y2023/autonomous/splines/spline_5.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [-6.450466090790975, -6.448323209188465, -6.468936183333308, -5.63485982210851, -5.224861021501398, -4.383040925048516], "spline_y": [0.41893834909569705, -0.2089748700255587, -1.0435424455861884, -0.6390449134590346, -0.8779649804883709, -0.8766708249234052], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/test_spline.json b/y2023/autonomous/splines/test_spline.json
index 7672596..733d516 100644
--- a/y2023/autonomous/splines/test_spline.json
+++ b/y2023/autonomous/splines/test_spline.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [6.22420997455908, 6.1347950111487386, 6.080329974810555, 6.023577036950107, 5.9617203084135255, 5.81469341092744], "spline_y": [-2.63127733767268, -2.63127733767268, -2.656484781970896, -2.656484781970896, -2.6668098529078925, -2.6448802602350456], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 2}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 4}]}
+{"spline_count": 1, "spline_x": [0, 0.4, 0.4, 0.6, 0.6, 1.0], "spline_y": [0, 0, 0.05, 0.1, 0.15, 0.15], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 1}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 2}]}
diff --git a/y2023/constants/simulated_constants_sender.cc b/y2023/constants/simulated_constants_sender.cc
index f18c3c1..8bfc17d 100644
--- a/y2023/constants/simulated_constants_sender.cc
+++ b/y2023/constants/simulated_constants_sender.cc
@@ -5,7 +5,7 @@
#include "frc971/constants/constants_sender_lib.h"
namespace y2023 {
-void SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
+bool SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
std::string constants_path) {
for (const aos::Node *node : factory->nodes()) {
std::unique_ptr<aos::EventLoop> event_loop =
@@ -13,5 +13,6 @@
frc971::constants::ConstantSender<Constants, ConstantsList> sender(
event_loop.get(), constants_path, team, "/constants");
}
+ return true;
}
} // namespace y2023
diff --git a/y2023/constants/simulated_constants_sender.h b/y2023/constants/simulated_constants_sender.h
index 44a868c..096ee20 100644
--- a/y2023/constants/simulated_constants_sender.h
+++ b/y2023/constants/simulated_constants_sender.h
@@ -5,7 +5,9 @@
#include "aos/testing/path.h"
namespace y2023 {
-void SendSimulationConstants(
+// Returns true, to allow this to be easily called in the initializer list of a
+// constructor.
+bool SendSimulationConstants(
aos::SimulatedEventLoopFactory *factory, int team,
std::string constants_path =
aos::testing::ArtifactPath("y2023/constants/test_constants.json"));
diff --git a/y2023/constants/test_data/test_team.json b/y2023/constants/test_data/test_team.json
index f09b23e..a1e77af 100644
--- a/y2023/constants/test_data/test_team.json
+++ b/y2023/constants/test_data/test_team.json
@@ -14,5 +14,19 @@
}
],
"target_map": {% include 'y2023/constants/test_data/target_map.json' %},
- "scoring_map": {% include 'y2023/constants/test_data/scoring_map.json' %}
+ "scoring_map": {% include 'y2023/constants/test_data/scoring_map.json' %},
+ "robot": {
+ "tof": {
+ "interpolation_table": [
+ {
+ "tof_reading": 0.1,
+ "lateral_position": 0.2
+ },
+ {
+ "tof_reading": 0.90,
+ "lateral_position": -0.2
+ }
+ ]
+ }
+ }
}
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
index 16a0090..1b70ca1 100644
--- a/y2023/control_loops/drivetrain/target_selector.cc
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -24,16 +24,6 @@
CHECK(constants_fetcher_.constants().has_scoring_map());
CHECK(constants_fetcher_.constants().scoring_map()->has_red());
CHECK(constants_fetcher_.constants().scoring_map()->has_blue());
- event_loop->MakeWatcher(
- "/superstructure",
- [this](const y2023::control_loops::superstructure::Position &msg) {
- // Technically this means that even if we have a cube we are relying on
- // getting a Position message before updating the game_piece_position_
- // to zero. But if we aren't getting position messages, then things are
- // very broken.
- game_piece_position_ =
- LateralOffsetForTimeOfFlight(msg.cone_position());
- });
event_loop->AddPhasedLoop(
[this](int) {
@@ -172,42 +162,15 @@
} else {
drive_direction_ = Side::DONT_CARE;
}
+ // Only update the game piece position when we reassign the target.
+ superstructure_status_fetcher_.Fetch();
+ if (superstructure_status_fetcher_.get() != nullptr) {
+ game_piece_position_ =
+ superstructure_status_fetcher_->game_piece_position();
+ }
}
CHECK(target_pose_.has_value());
return true;
}
-// TODO: Maybe this already handles field side correctly? Unsure if the line
-// follower ends up having positive as being robot frame relative or robot
-// direction relative...
-double TargetSelector::LateralOffsetForTimeOfFlight(double reading) {
- superstructure_status_fetcher_.Fetch();
- if (superstructure_status_fetcher_.get() != nullptr) {
- switch (superstructure_status_fetcher_->game_piece()) {
- case vision::Class::NONE:
- case vision::Class::CUBE:
- return 0.0;
- case vision::Class::CONE_UP:
- // execute logic below.
- break;
- case vision::Class::CONE_DOWN:
- // execute logic below.
- break;
- }
- } else {
- return 0.0;
- }
- const TimeOfFlight *calibration =
- CHECK_NOTNULL(constants_fetcher_.constants().robot()->tof());
- // TODO(james): Use a generic interpolation table class.
- auto table = CHECK_NOTNULL(calibration->interpolation_table());
- CHECK_EQ(2u, table->size());
- double x1 = table->Get(0)->tof_reading();
- double x2 = table->Get(1)->tof_reading();
- double y1 = table->Get(0)->lateral_position();
- double y2 = table->Get(1)->lateral_position();
- return frc971::shooter_interpolation::Blend((reading - x1) / (x2 - x1), y1,
- y2);
-}
-
} // namespace y2023::control_loops::drivetrain
diff --git a/y2023/control_loops/drivetrain/target_selector.h b/y2023/control_loops/drivetrain/target_selector.h
index bed56ce..5e7f015 100644
--- a/y2023/control_loops/drivetrain/target_selector.h
+++ b/y2023/control_loops/drivetrain/target_selector.h
@@ -47,8 +47,6 @@
private:
void UpdateAlliance();
- // Returns the Y coordinate of a game piece given the time-of-flight reading.
- double LateralOffsetForTimeOfFlight(double reading);
std::optional<Pose> target_pose_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<TargetSelectorHint> hint_fetcher_;
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index f18a0b6..e47128a 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,13 @@
[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
+ # Lets you only view selected path
+ self.view_current = False
+
def _do_button_press_internal(self, event):
o_x = event.x
o_y = event.y
@@ -295,6 +337,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"])
@@ -370,21 +414,43 @@
self.outline.draw_theta(cr)
set_color(cr, Color(0.0, 0.5, 1.0))
- for i in range(len(self.segments)):
- color = None
- if i == self.index:
- continue
- color = [0, random.random(), 1]
- 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()
+ if not self.view_current:
+ for i in range(len(self.segments)):
+ color = None
+ if i == self.index:
+ continue
+ color = [0, random.random(), 1]
+ 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()
# Draw current spline in black
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,12 +564,18 @@
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)
print("Switched to segment:", self.segments[self.index].name)
self.segments[self.index].Print(graph_paths.points)
+ elif keyval == Gdk.KEY_l:
+ self.view_current = not self.view_current
+
elif keyval == Gdk.KEY_t:
# Toggle between theta and xy renderings
if self.theta_version:
@@ -563,8 +635,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/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index 6df0830..e5bc830 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -101,9 +101,13 @@
":superstructure_status_fbs",
"//aos:flatbuffer_merge",
"//aos/events:event_loop",
+ "//frc971/constants:constants_sender_lib",
"//frc971/control_loops:control_loop",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/shooter_interpolation:interpolation",
"//y2023:constants",
+ "//y2023/constants:constants_fbs",
+ "//y2023/constants:simulated_constants_sender",
"//y2023/control_loops/drivetrain:drivetrain_can_position_fbs",
"//y2023/control_loops/superstructure/arm",
"//y2023/control_loops/superstructure/arm:arm_trajectories_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/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 9b9b119..0f19a1e 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -3,6 +3,7 @@
#include "aos/events/event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "aos/network/team_number.h"
+#include "frc971/shooter_interpolation/interpolation.h"
#include "frc971/zeroing/wrap.h"
#include "y2023/control_loops/superstructure/arm/arm_trajectories_generated.h"
@@ -27,6 +28,7 @@
: frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
values_(values),
+ constants_fetcher_(event_loop),
drivetrain_status_fetcher_(
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
@@ -100,6 +102,11 @@
status_builder.add_end_effector_state(end_effector_.state());
// TODO(milind): integrate this with ML game piece detection somehow
status_builder.add_game_piece(end_effector_.game_piece());
+ const std::optional<double> game_piece_position =
+ LateralOffsetForTimeOfFlight(position->cone_position());
+ if (game_piece_position.has_value()) {
+ status_builder.add_game_piece_position(game_piece_position.value());
+ }
(void)status->Send(status_builder.Finish());
}
@@ -110,6 +117,36 @@
: 0.0);
}
+std::optional<double> Superstructure::LateralOffsetForTimeOfFlight(
+ double reading) {
+ switch (end_effector_.game_piece()) {
+ case vision::Class::NONE:
+ return std::nullopt;
+ case vision::Class::CUBE:
+ // Cubes are definitionally centered.
+ return 0.0;
+ case vision::Class::CONE_UP:
+ case vision::Class::CONE_DOWN:
+ // execute logic below.
+ break;
+ }
+ constexpr double kInvalidReading = 0.93;
+ if (reading > kInvalidReading) {
+ return std::nullopt;
+ }
+ const TimeOfFlight *calibration = CHECK_NOTNULL(
+ CHECK_NOTNULL(constants_fetcher_.constants().robot())->tof());
+ // TODO(james): Use a generic interpolation table class.
+ auto table = CHECK_NOTNULL(calibration->interpolation_table());
+ CHECK_EQ(2u, table->size());
+ double x1 = table->Get(0)->tof_reading();
+ double x2 = table->Get(1)->tof_reading();
+ double y1 = table->Get(0)->lateral_position();
+ double y2 = table->Get(1)->lateral_position();
+ return frc971::shooter_interpolation::Blend((reading - x1) / (x2 - x1), y1,
+ y2);
+}
+
} // namespace superstructure
} // namespace control_loops
} // namespace y2023
diff --git a/y2023/control_loops/superstructure/superstructure.h b/y2023/control_loops/superstructure/superstructure.h
index 0c8b2c0..1100b86 100644
--- a/y2023/control_loops/superstructure/superstructure.h
+++ b/y2023/control_loops/superstructure/superstructure.h
@@ -3,9 +3,11 @@
#include "aos/events/event_loop.h"
#include "aos/json_to_flatbuffer.h"
+#include "frc971/constants/constants_sender_lib.h"
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2023/constants.h"
+#include "y2023/constants/constants_generated.h"
#include "y2023/control_loops/drivetrain/drivetrain_can_position_generated.h"
#include "y2023/control_loops/superstructure/arm/arm.h"
#include "y2023/control_loops/superstructure/arm/arm_trajectories_generated.h"
@@ -63,7 +65,11 @@
aos::Sender<Status>::Builder *status) override;
private:
+ // Returns the Y coordinate of a game piece given the time-of-flight reading.
+ std::optional<double> LateralOffsetForTimeOfFlight(double reading);
+
std::shared_ptr<const constants::Values> values_;
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
aos::Fetcher<frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index e2524c1..c1c20a3 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -8,6 +8,7 @@
#include "frc971/control_loops/subsystem_simulator.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
+#include "y2023/constants/simulated_constants_sender.h"
#include "y2023/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
#include "y2023/control_loops/superstructure/superstructure.h"
@@ -240,8 +241,7 @@
position_builder.add_wrist(wrist_offset);
position_builder.add_end_effector_cube_beam_break(
end_effector_cube_beam_break_);
- // TODO(milind): put into our state
- position_builder.add_cone_position(0.95);
+ position_builder.add_cone_position(cone_position_);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
@@ -250,6 +250,10 @@
end_effector_cube_beam_break_ = triggered;
}
+ void set_cone_position(double cone_position) {
+ cone_position_ = cone_position;
+ }
+
private:
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
@@ -265,6 +269,7 @@
::aos::Fetcher<Output> superstructure_output_fetcher_;
bool first_ = true;
+ double cone_position_ = 0.95;
};
class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
@@ -274,6 +279,8 @@
aos::configuration::ReadConfig("y2023/aos_config.json"),
std::chrono::microseconds(5050)),
values_(std::make_shared<constants::Values>(constants::MakeValues())),
+ simulated_constants_dummy_(SendSimulationConstants(
+ event_loop_factory(), 7971, "y2023/constants/test_constants.json")),
roborio_(aos::configuration::GetNode(configuration(), "roborio")),
logger_pi_(aos::configuration::GetNode(configuration(), "logger")),
arm_trajectories_(superstructure::Superstructure::GetArmTrajectories(
@@ -410,6 +417,7 @@
}
std::shared_ptr<const constants::Values> values_;
+ const bool simulated_constants_dummy_;
const aos::Node *const roborio_;
const aos::Node *const logger_pi_;
@@ -560,6 +568,42 @@
CheckIfZeroed();
}
+// Tests that the cone position ocnversion works.
+TEST_F(SuperstructureTest, ConePositionConversion) {
+ // Get ourselves into CONE mode.
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_arm_goal_position(arm::NeutralIndex());
+ goal_builder.add_trajectory_override(false);
+ goal_builder.add_roller_goal(RollerGoal::INTAKE_CONE_UP);
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+ superstructure_plant_.set_cone_position(1.0);
+ RunFor(chrono::seconds(1));
+ // Game piece position should not be populated when invalid.
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(vision::Class::CONE_UP, superstructure_status_fetcher_->game_piece());
+ EXPECT_FALSE(superstructure_status_fetcher_->has_game_piece_position());
+
+ // And then send a valid cone position.
+ superstructure_plant_.set_cone_position(0.5);
+ RunFor(chrono::seconds(1));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(vision::Class::CONE_UP, superstructure_status_fetcher_->game_piece());
+ EXPECT_TRUE(superstructure_status_fetcher_->has_game_piece_position());
+ EXPECT_FLOAT_EQ(0.0, superstructure_status_fetcher_->game_piece_position());
+
+ superstructure_plant_.set_cone_position(0.1);
+ RunFor(chrono::seconds(1));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(vision::Class::CONE_UP, superstructure_status_fetcher_->game_piece());
+ EXPECT_TRUE(superstructure_status_fetcher_->has_game_piece_position());
+ EXPECT_FLOAT_EQ(0.2, superstructure_status_fetcher_->game_piece_position());
+}
+
class SuperstructureBeambreakTest
: public SuperstructureTest,
public ::testing::WithParamInterface<vision::Class> {
diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
index 3c7a33b..83ca2b6 100644
--- a/y2023/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -47,7 +47,13 @@
// Positive position would be upwards
wrist:frc971.AbsolutePosition (id: 1);
- // If this is true, the cone beam break is triggered.
+ // Estimated position of a cone in the gripper from the time-of-flight
+ // sensors.
+ // If greater than 0.9, indicates that we cannot see a cone.
+ // Will be larger when the cone is farther forwards on the robot when
+ // the wrist and arm positions are all at zero (this will typically mean
+ // that it is larger when the cone is to the robot's current right when
+ // trying to core).
cone_position:double (id: 2);
// If this is true, the cube beam break is triggered.
diff --git a/y2023/control_loops/superstructure/superstructure_status.fbs b/y2023/control_loops/superstructure/superstructure_status.fbs
index 5381b0a..8d74bfe 100644
--- a/y2023/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023/control_loops/superstructure/superstructure_status.fbs
@@ -89,7 +89,15 @@
wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
end_effector_state:EndEffectorState (id: 4);
+
game_piece:vision.Class (id: 5);
+
+ // Indicates the current lateral position of the game piece, in meters.
+ // This number will be zero when the game piece is centered, and positive if
+ // the arm + wrist are all at zero and the game piece is towards the back
+ // of the robot. This will typically mean that positive is to the robot's
+ // left
+ game_piece_position:double (id: 6);
}
root_type Status;
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/vision/BUILD b/y2023/vision/BUILD
index 68ba833..1cdbe36 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -247,3 +247,22 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
)
+
+cc_binary(
+ name = "image_logger",
+ srcs = [
+ "image_logger.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/events/logging:log_writer",
+ "//aos/logging:log_namer",
+ "//frc971/input:joystick_state_fbs",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/y2023/vision/image_logger.cc b/y2023/vision/image_logger.cc
new file mode 100644
index 0000000..b87cec0
--- /dev/null
+++ b/y2023/vision/image_logger.cc
@@ -0,0 +1,95 @@
+#include <sys/resource.h>
+#include <sys/time.h>
+
+#include "aos/configuration.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/logging/log_namer.h"
+#include "frc971/input/joystick_state_generated.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+DEFINE_string(config, "aos_config.json", "Config file to use.");
+
+DEFINE_double(rotate_every, 0.0,
+ "If set, rotate the logger after this many seconds");
+DECLARE_int32(flush_size);
+DEFINE_double(disabled_time, 5.0,
+ "Continue logging if disabled for this amount of time or less");
+
+std::unique_ptr<aos::logger::MultiNodeLogNamer> MakeLogNamer(
+ aos::EventLoop *event_loop) {
+ return std::make_unique<aos::logger::MultiNodeLogNamer>(
+ absl::StrCat(aos::logging::GetLogName("fbs_log"), "/"), event_loop);
+}
+
+int main(int argc, char *argv[]) {
+ gflags::SetUsageMessage(
+ "This program provides a simple logger binary that logs all SHMEM data "
+ "directly to a file specified at the command line when the robot is "
+ "enabled and for a bit of time after.");
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ bool logging = false;
+ bool enabled = false;
+ aos::monotonic_clock::time_point last_disable_time =
+ event_loop.monotonic_now();
+ aos::monotonic_clock::time_point last_rotation_time =
+ event_loop.monotonic_now();
+ aos::logger::Logger logger(&event_loop);
+
+ if (FLAGS_rotate_every != 0.0) {
+ logger.set_on_logged_period([&] {
+ const auto now = event_loop.monotonic_now();
+ if (logging && now > last_rotation_time + std::chrono::duration<double>(
+ FLAGS_rotate_every)) {
+ logger.Rotate();
+ last_rotation_time = now;
+ }
+ });
+ }
+
+ event_loop.OnRun([]() {
+ errno = 0;
+ setpriority(PRIO_PROCESS, 0, -20);
+ PCHECK(errno == 0) << ": Renicing to -20 failed.";
+ });
+
+ event_loop.MakeWatcher(
+ "/roborio/aos", [&](const aos::JoystickState &joystick_state) {
+ const auto timestamp = event_loop.context().monotonic_event_time;
+ // Store the last time we got disabled
+ if (enabled && !joystick_state.enabled()) {
+ last_disable_time = timestamp;
+ }
+ enabled = joystick_state.enabled();
+
+ if (!logging && enabled) {
+ // Start logging if we just got enabled
+ LOG(INFO) << "Starting logging";
+ logger.StartLogging(MakeLogNamer(&event_loop));
+ logging = true;
+ last_rotation_time = event_loop.monotonic_now();
+ } else if (logging && !enabled &&
+ (timestamp - last_disable_time) >
+ std::chrono::duration<double>(FLAGS_disabled_time)) {
+ // Stop logging if we've been disabled for a non-negligible amount of
+ // time
+ LOG(INFO) << "Stopping logging";
+ logger.StopLogging(event_loop.monotonic_now());
+ logging = false;
+ }
+ });
+
+ event_loop.Run();
+
+ LOG(INFO) << "Shutting down";
+
+ return 0;
+}
diff --git a/y2023/www/BUILD b/y2023/www/BUILD
index f8b706c..63089b5 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 = [
@@ -23,8 +30,10 @@
"//aos/network:connect_ts_fbs",
"//aos/network:web_proxy_ts_fbs",
"//aos/network/www:proxy",
+ "//frc971/control_loops:control_loops_ts_fbs",
"//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..a63c6f5 100644
--- a/y2023/www/field.html
+++ b/y2023/www/field.html
@@ -34,6 +34,65 @@
<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>
+ <h3>Zeroing Faults:</h3>
+ <p id="zeroing_faults"> NA </p>
</div>
<div id="vision_readouts">
</div>
diff --git a/y2023/www/field_handler.ts b/y2023/www/field_handler.ts
index 9e2d9ed..65b9a20 100644
--- a/y2023/www/field_handler.ts
+++ b/y2023/www/field_handler.ts
@@ -3,6 +3,9 @@
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 {ZeroingError} from '../../frc971/control_loops/control_loops_generated';
import {Visualization, TargetEstimateDebug} from '../localizer/visualization_generated';
import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
@@ -21,6 +24,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 +37,28 @@
(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);
+ private zeroingFaults: HTMLElement =
+ (document.getElementById('zeroing_faults') as HTMLElement);_
constructor(private readonly connection: Connection) {
(document.getElementById('field') as HTMLElement).appendChild(this.canvas);
@@ -81,6 +107,11 @@
'/localizer', "frc971.controls.LocalizerOutput", (data) => {
this.handleLocalizerOutput(data);
});
+ this.connection.addHandler(
+ '/superstructure', "y2023.control_loops.superstructure.Status",
+ (data) => {
+ this.handleSuperstructureStatus(data)
+ });
});
}
@@ -117,6 +148,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 +215,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 +248,56 @@
// 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);
+ let zeroingErrors: string = "Roll Joint Errors:"+'<br/>';
+ for (let i = 0; i < this.superstructureStatus.arm().rollJointEstimatorState().errors.length; i++) {
+ zeroingErrors += ZeroingError[this.superstructureStatus.arm().rollJointEstimatorState().errors(i)]+'<br/>';
+ }
+ zeroingErrors += '<br/>'+"Proximal Joint Errors:"+'<br/>';
+ for (let i = 0; i < this.superstructureStatus.arm().proximalEstimatorState().errors.length; i++) {
+ zeroingErrors += ZeroingError[this.superstructureStatus.arm().proximalEstimatorState().errors(i)]+'<br/>';
+ }
+ zeroingErrors += '<br/>'+"Distal Joint Errors:"+'<br/>';
+ for (let i = 0; i < this.superstructureStatus.arm().distalEstimatorState().errors.length; i++) {
+ zeroingErrors += ZeroingError[this.superstructureStatus.arm().distalEstimatorState().errors(i)]+'<br/>';
+ }
+ zeroingErrors += '<br/>'+"Wrist Errors:"+'<br/>';
+ for (let i = 0; i < this.superstructureStatus.wrist().estimatorState().errors.length; i++) {
+ zeroingErrors += ZeroingError[this.superstructureStatus.wrist().estimatorState().errors(i)]+'<br/>';
+ }
+ this.zeroingFaults.innerHTML = zeroingErrors;
+ }
+
if (this.drivetrainStatus && this.drivetrainStatus.trajectoryLogging()) {
this.drawRobot(
this.drivetrainStatus.trajectoryLogging().x(),
diff --git a/y2023/y2023_logger.json b/y2023/y2023_logger.json
index f4ac45e..55d201d 100644
--- a/y2023/y2023_logger.json
+++ b/y2023/y2023_logger.json
@@ -446,8 +446,7 @@
},
{
"name": "image_logger",
- "executable_name": "logger_main",
- "autostart": false,
+ "executable_name": "image_logger",
"user": "pi",
"args": [
"--logging_folder",
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index baf7031..113e48f 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -392,8 +392,7 @@
},
{
"name": "image_logger",
- "executable_name": "logger_main",
- "autostart": false,
+ "executable_name": "image_logger",
"args": [
"--logging_folder",
"",
@@ -402,6 +401,7 @@
"--direct",
"--flush_size=4194304"
],
+ "user": "pi",
"nodes": [
"pi{{ NUM }}"
]
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index 03985c0..f3697ac 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -28,6 +28,42 @@
"timestamp_logger_nodes": [
"roborio"
]
+ },
+ {
+ "name": "pi1",
+ "priority": 5,
+ "time_to_live": 50000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "pi2",
+ "priority": 5,
+ "time_to_live": 50000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "pi3",
+ "priority": 5,
+ "time_to_live": 50000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "pi4",
+ "priority": 5,
+ "time_to_live": 50000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ]
}
]
},
@@ -50,6 +86,42 @@
"max_size": 200
},
{
+ "name": "/roborio/aos/remote_timestamps/pi1/roborio/aos/aos-JoystickState",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 300,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/pi2/roborio/aos/aos-JoystickState",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 300,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/pi3/roborio/aos/aos-JoystickState",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 300,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/pi4/roborio/aos/aos-JoystickState",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 300,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
"name": "/roborio/aos",
"type": "aos.RobotState",
"source_node": "roborio",
