Merge "All the setpoints and tip scoring paths"
diff --git a/frc971/control_loops/python/constants.py b/frc971/control_loops/python/constants.py
index 3a61b5e..7bc45db 100644
--- a/frc971/control_loops/python/constants.py
+++ b/frc971/control_loops/python/constants.py
@@ -36,7 +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)
+Robot2023 = RobotType(width=0.6061, length=0.77581)
FIELDS = {
"2019 Field":
@@ -137,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/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/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 287c5b0..e47128a 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -285,6 +285,10 @@
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
@@ -410,23 +414,26 @@
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)
+ 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()
+ 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)
@@ -434,10 +441,15 @@
control1 = shift_angles(self.segments[self.index].control1)
control2 = shift_angles(self.segments[self.index].control2)
- cr.move_to(control1[0] + 0.02, control1[1])
- cr.arc(control1[0], control1[1], 0.02, 0, 2.0 * np.pi)
- cr.move_to(control2[0] + 0.02, control2[1])
- cr.arc(control2[0], control2[1], 0.02, 0, 2.0 * np.pi)
+ 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()
@@ -552,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:
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index 2700bbc..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",
@@ -152,6 +156,35 @@
],
)
+cc_library(
+ name = "led_indicator_lib",
+ srcs = ["led_indicator.cc"],
+ hdrs = ["led_indicator.h"],
+ data = [
+ "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/queues:gyro_fbs",
+ "//third_party:phoenix",
+ "//third_party:wpilib",
+ "//y2023/vision:game_pieces_fbs",
+ ],
+)
+
cc_binary(
name = "superstructure_replay",
srcs = ["superstructure_replay.cc"],
diff --git a/y2023/control_loops/superstructure/led_indicator.cc b/y2023/control_loops/superstructure/led_indicator.cc
new file mode 100644
index 0000000..68e7f14
--- /dev/null
+++ b/y2023/control_loops/superstructure/led_indicator.cc
@@ -0,0 +1,183 @@
+#include "y2023/control_loops/superstructure/led_indicator.h"
+
+namespace led = ctre::phoenix::led;
+
+namespace y2023::control_loops::superstructure {
+
+LedIndicator::LedIndicator(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ drivetrain_output_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_position_fetcher_(
+ event_loop_->MakeFetcher<Position>("/superstructure")),
+ server_statistics_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
+ "/roborio/aos")),
+ client_statistics_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ClientStatistics>(
+ "/roborio/aos")),
+ localizer_output_fetcher_(
+ event_loop_->MakeFetcher<frc971::controls::LocalizerOutput>(
+ "/localizer")),
+ gyro_reading_fetcher_(
+ event_loop_->MakeFetcher<frc971::sensors::GyroReading>(
+ "/drivetrain")),
+ drivetrain_status_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")) {
+ led::CANdleConfiguration config;
+ config.statusLedOffWhenActive = true;
+ config.disableWhenLOS = false;
+ config.brightnessScalar = 1.0;
+ candle_.ConfigAllSettings(config, 0);
+
+ event_loop_->AddPhasedLoop([&](int) { DecideColor(); },
+ std::chrono::milliseconds(20));
+}
+
+// This method will be called once per scheduler run
+void LedIndicator::DisplayLed(uint8_t r, uint8_t g, uint8_t b) {
+ candle_.SetLEDs(static_cast<int>(r), static_cast<int>(g),
+ static_cast<int>(b));
+}
+
+namespace {
+bool DisconnectedPiServer(
+ const aos::message_bridge::ServerStatistics &server_stats) {
+ for (const auto *pi_server_status : *server_stats.connections()) {
+ if (pi_server_status->state() == aos::message_bridge::State::DISCONNECTED &&
+ pi_server_status->node()->name()->string_view() != "logger") {
+ return true;
+ }
+ }
+ return false;
+}
+
+bool DisconnectedPiClient(
+ const aos::message_bridge::ClientStatistics &client_stats) {
+ for (const auto *pi_client_status : *client_stats.connections()) {
+ if (pi_client_status->state() == aos::message_bridge::State::DISCONNECTED &&
+ pi_client_status->node()->name()->string_view() != "logger") {
+ return true;
+ }
+ }
+ return false;
+}
+} // namespace
+
+void LedIndicator::DecideColor() {
+ superstructure_status_fetcher_.Fetch();
+ superstructure_position_fetcher_.Fetch();
+ server_statistics_fetcher_.Fetch();
+ drivetrain_output_fetcher_.Fetch();
+ client_statistics_fetcher_.Fetch();
+ gyro_reading_fetcher_.Fetch();
+ localizer_output_fetcher_.Fetch();
+
+ if (localizer_output_fetcher_.get()) {
+ if (localizer_output_fetcher_->image_accepted_count() !=
+ last_accepted_count_) {
+ last_accepted_count_ = localizer_output_fetcher_->image_accepted_count();
+ last_accepted_time_ = event_loop_->monotonic_now();
+ }
+ }
+
+ // Estopped
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_status_fetcher_->estopped()) {
+ DisplayLed(255, 0, 0);
+ return;
+ }
+
+ // Not zeroed
+ if (superstructure_status_fetcher_.get() &&
+ !superstructure_status_fetcher_->zeroed()) {
+ DisplayLed(255, 0, 255);
+ return;
+ }
+
+ // If the imu gyro readings are not being sent/updated recently
+ if (!gyro_reading_fetcher_.get() ||
+ gyro_reading_fetcher_.context().monotonic_event_time <
+ event_loop_->monotonic_now() -
+ frc971::controls::kLoopFrequency * 10) {
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(255, 255, 255);
+ }
+ return;
+ }
+
+ // Pi disconnected
+ if ((server_statistics_fetcher_.get() &&
+ DisconnectedPiServer(*server_statistics_fetcher_)) ||
+ (client_statistics_fetcher_.get() &&
+ DisconnectedPiClient(*client_statistics_fetcher_))) {
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(0, 255, 0);
+ }
+
+ return;
+ }
+
+ if (superstructure_status_fetcher_.get()) {
+ // Check if end effector is intaking.
+ if (superstructure_status_fetcher_->end_effector_state() ==
+ EndEffectorState::INTAKING) {
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 165, 0);
+ } else {
+ DisplayLed(0, 0, 0);
+ }
+
+ return;
+ }
+ // Check if end effector is spitting.
+ if (superstructure_status_fetcher_->end_effector_state() ==
+ EndEffectorState::SPITTING) {
+ if (flash_counter_.Flash()) {
+ DisplayLed(0, 255, 0);
+ } else {
+ DisplayLed(0, 0, 0);
+ }
+
+ return;
+ }
+
+ // Check the if there is a cone in the end effector.
+ if (superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_UP ||
+ superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_DOWN) {
+ DisplayLed(255, 255, 0);
+ return;
+ }
+ // Check if the cube beam break is triggered.
+ if (superstructure_status_fetcher_->game_piece() == vision::Class::CUBE) {
+ DisplayLed(138, 43, 226);
+ return;
+ }
+
+ // Check if there is a target that is in sight
+ if (drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
+ DisplayLed(255, 165, 0);
+ return;
+ }
+
+ if (event_loop_->monotonic_now() <
+ last_accepted_time_ + std::chrono::milliseconds(500)) {
+ DisplayLed(0, 0, 255);
+ return;
+ }
+
+ return;
+ }
+}
+
+} // namespace y2023::control_loops::superstructure
diff --git a/y2023/control_loops/superstructure/led_indicator.h b/y2023/control_loops/superstructure/led_indicator.h
new file mode 100644
index 0000000..d88650d
--- /dev/null
+++ b/y2023/control_loops/superstructure/led_indicator.h
@@ -0,0 +1,97 @@
+#ifndef Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+#define Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/network/message_bridge_client_generated.h"
+#include "aos/network/message_bridge_server_generated.h"
+#include "ctre/phoenix/led/CANdle.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/queues/gyro_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2023/vision/game_pieces_generated.h"
+
+namespace y2023::control_loops::superstructure {
+
+class FlashCounter {
+ public:
+ FlashCounter(size_t flash_iterations) : flash_iterations_(flash_iterations) {}
+
+ bool Flash() {
+ if (counter_ % flash_iterations_ == 0) {
+ flash_ = !flash_;
+ }
+ counter_++;
+ return flash_;
+ }
+
+ private:
+ size_t flash_iterations_;
+ size_t counter_ = 0;
+ bool flash_ = false;
+};
+
+class LedIndicator {
+ public:
+ LedIndicator(aos::EventLoop *event_loop);
+
+ // Colors in order of priority:
+ //
+ // Red: estopped
+ // Pink: not zeroed
+ // Flash red/white: imu disconnected
+ // Flash red/green: pi disconnected
+ //
+ // Statemachine:
+ // END EFFECTOR INTAKING:
+ // Flash orange/off
+ // END EFFECTOR SPITTING:
+ // Flash green/off
+ // CONE LOADED:
+ // Yellow
+ // CUBE LOADED:
+ // Purple
+ // HAS A TARGET
+ // Gold
+ // VISION:
+ // Blue
+
+ void DecideColor();
+
+ private:
+ static constexpr size_t kFlashIterations = 5;
+
+ void DisplayLed(uint8_t r, uint8_t g, uint8_t b);
+
+ ctre::phoenix::led::CANdle candle_{0, ""};
+
+ aos::EventLoop *event_loop_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Output>
+ drivetrain_output_fetcher_;
+ aos::Fetcher<Status> superstructure_status_fetcher_;
+ aos::Fetcher<Position> superstructure_position_fetcher_;
+ aos::Fetcher<aos::message_bridge::ServerStatistics>
+ server_statistics_fetcher_;
+ aos::Fetcher<aos::message_bridge::ClientStatistics>
+ client_statistics_fetcher_;
+ aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
+ aos::Fetcher<frc971::sensors::GyroReading> gyro_reading_fetcher_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+
+ size_t last_accepted_count_ = 0;
+ aos::monotonic_clock::time_point last_accepted_time_ =
+ aos::monotonic_clock::min_time;
+
+ FlashCounter flash_counter_{kFlashIterations};
+};
+
+} // namespace y2023::control_loops::superstructure
+
+#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
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/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 09cd4d8..63089b5 100644
--- a/y2023/www/BUILD
+++ b/y2023/www/BUILD
@@ -30,6 +30,7 @@
"//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",
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 52e0d11..a63c6f5 100644
--- a/y2023/www/field.html
+++ b/y2023/www/field.html
@@ -91,6 +91,8 @@
<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 db881af..65b9a20 100644
--- a/y2023/www/field_handler.ts
+++ b/y2023/www/field_handler.ts
@@ -5,6 +5,7 @@
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';
@@ -56,6 +57,8 @@
(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);
@@ -276,6 +279,23 @@
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()) {
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",