Merge "Add duty cycle to CANPosition logging"
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index 58c0fc3..02a9fdd 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -237,8 +237,7 @@
}
iovec_.clear();
- const size_t original_iovec_size = std::min<size_t>(queue.size(), IOV_MAX);
- size_t iovec_size = original_iovec_size;
+ size_t iovec_size = std::min<size_t>(queue.size(), IOV_MAX);
iovec_.resize(iovec_size);
size_t counted_size = 0;
@@ -248,7 +247,7 @@
// The file is aligned if it is a multiple of kSector in length. The data is
// aligned if it's memory is kSector aligned, and the length is a multiple of
// kSector in length.
- bool aligned = (file_written_bytes_ % kSector) == 0;
+ bool aligned = (total_write_bytes_ % kSector) == 0;
size_t write_index = 0;
for (size_t i = 0; i < iovec_size; ++i) {
iovec_[write_index].iov_base = const_cast<uint8_t *>(queue[i].data());
@@ -296,13 +295,11 @@
++write_index;
}
- if (counted_size > 0) {
- // Either write the aligned data if it is all aligned, or write the rest
- // unaligned if we wrote aligned up above.
- WriteV(iovec_.data(), iovec_.size(), aligned, counted_size);
+ // Either write the aligned data if it is all aligned, or write the rest
+ // unaligned if we wrote aligned up above.
+ WriteV(iovec_.data(), iovec_.size(), aligned, counted_size);
- encoder_->Clear(original_iovec_size);
- }
+ encoder_->Clear(iovec_size);
}
size_t DetachedBufferWriter::WriteV(struct iovec *iovec_data, size_t iovec_size,
@@ -320,21 +317,21 @@
if (written > 0) {
// Flush asynchronously and force the data out of the cache.
- sync_file_range(fd_, file_written_bytes_, written, SYNC_FILE_RANGE_WRITE);
- if (file_written_bytes_ != 0) {
+ sync_file_range(fd_, total_write_bytes_, written, SYNC_FILE_RANGE_WRITE);
+ if (last_synced_bytes_ != 0) {
// Per Linus' recommendation online on how to do fast file IO, do a
// blocking flush of the previous write chunk, and then tell the kernel to
// drop the pages from the cache. This makes sure we can't get too far
// ahead.
sync_file_range(fd_, last_synced_bytes_,
- file_written_bytes_ - last_synced_bytes_,
+ total_write_bytes_ - last_synced_bytes_,
SYNC_FILE_RANGE_WAIT_BEFORE | SYNC_FILE_RANGE_WRITE |
SYNC_FILE_RANGE_WAIT_AFTER);
posix_fadvise(fd_, last_synced_bytes_,
- file_written_bytes_ - last_synced_bytes_,
+ total_write_bytes_ - last_synced_bytes_,
POSIX_FADV_DONTNEED);
- last_synced_bytes_ = file_written_bytes_;
+ last_synced_bytes_ = total_write_bytes_;
}
}
@@ -374,7 +371,6 @@
++total_write_count_;
total_write_messages_ += iovec_size;
total_write_bytes_ += written;
- file_written_bytes_ += written;
}
void DetachedBufferWriter::FlushAtThreshold(
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index 0c0ef7f..50f6b40 100644
--- a/aos/events/logging/logfile_utils.h
+++ b/aos/events/logging/logfile_utils.h
@@ -212,8 +212,6 @@
bool supports_odirect_ = true;
int flags_ = 0;
- size_t file_written_bytes_ = 0;
-
aos::monotonic_clock::time_point last_flush_time_ =
aos::monotonic_clock::min_time;
};
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 1ebf1ba..60528a9 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -59,6 +59,7 @@
"//y2022/localizer:localizer_plotter",
"//y2022/vision:vision_plotter",
"//y2023/localizer:corrections_plotter",
+ "//y2023/localizer:localizer_plotter",
],
)
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 1b62e42..57d5041 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -52,6 +52,8 @@
'../../y2022/control_loops/superstructure/climber_plotter'
import {plotLocalizer as plot2022Localizer} from
'../../y2022/localizer/localizer_plotter'
+import {plotLocalizer as plot2023Localizer} from
+ '../../y2023/localizer/localizer_plotter'
import {plotVision as plot2022Vision} from
'../../y2022/vision/vision_plotter'
import {plotVision as plot2023Corrections} from
@@ -115,6 +117,7 @@
['Down Estimator', new PlotState(plotDiv, plotDownEstimator)],
['Robot State', new PlotState(plotDiv, plotRobotState)],
['2023 Vision', new PlotState(plotDiv, plot2023Corrections)],
+ ['2023 Localizer', new PlotState(plotDiv, plot2023Localizer)],
['2020 Finisher', new PlotState(plotDiv, plot2020Finisher)],
['2020 Accelerator', new PlotState(plotDiv, plot2020Accelerator)],
['2020 Hood', new PlotState(plotDiv, plot2020Hood)],
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index ff027d0..d9e239b 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -30,6 +30,14 @@
: true;
}
+aos::Alliance LocalizationUtils::Alliance() {
+ joystick_state_fetcher_.Fetch();
+ return (joystick_state_fetcher_.get() != nullptr)
+ ? joystick_state_fetcher_->alliance()
+ : aos::Alliance::kInvalid;
+
+}
+
std::optional<aos::monotonic_clock::duration> LocalizationUtils::ClockOffset(
std::string_view node) {
std::optional<aos::monotonic_clock::duration> monotonic_offset;
diff --git a/frc971/control_loops/drivetrain/localization/utils.h b/frc971/control_loops/drivetrain/localization/utils.h
index 26242f9..cfd443a 100644
--- a/frc971/control_loops/drivetrain/localization/utils.h
+++ b/frc971/control_loops/drivetrain/localization/utils.h
@@ -29,6 +29,7 @@
// Returns true if either there is no JoystickState message available or if
// we are currently in autonomous mode.
bool MaybeInAutonomous();
+ aos::Alliance Alliance();
// Returns the offset between our node and the specified node (or nullopt if
// no offset is available). The sign of this will be such that the time on
diff --git a/y2023/constants/common.json b/y2023/constants/common.json
index c559810..d63c518 100644
--- a/y2023/constants/common.json
+++ b/y2023/constants/common.json
@@ -1,2 +1,6 @@
"target_map": {% include 'y2023/vision/maps/target_map.json' %},
- "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
+ "scoring_map": {% include 'y2023/constants/scoring_map.json' %},
+ "ignore_targets": {
+ "red": [4],
+ "blue": [5]
+ }
diff --git a/y2023/constants/constants.fbs b/y2023/constants/constants.fbs
index bd038b8..61c3365 100644
--- a/y2023/constants/constants.fbs
+++ b/y2023/constants/constants.fbs
@@ -29,11 +29,19 @@
tof:TimeOfFlight (id: 0);
}
+// Set of april tag targets, by april tag ID, to ignore when on a
+// given alliance.
+table IgnoreTargets {
+ red:[uint64] (id: 0);
+ blue:[uint64] (id: 1);
+}
+
table Constants {
cameras:[CameraConfiguration] (id: 0);
target_map:frc971.vision.TargetMap (id: 1);
scoring_map:localizer.ScoringMap (id: 2);
robot:RobotConstants (id: 3);
+ ignore_targets:IgnoreTargets (id: 4);
}
root_type Constants;
diff --git a/y2023/constants/test_data/test_team.json b/y2023/constants/test_data/test_team.json
index a1e77af..0a226f0 100644
--- a/y2023/constants/test_data/test_team.json
+++ b/y2023/constants/test_data/test_team.json
@@ -28,5 +28,9 @@
}
]
}
+ },
+ "ignore_targets": {
+ "red": [4],
+ "blue": [5]
}
}
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 77902e7..19e5a34 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -266,7 +266,7 @@
control1=np.array([3.6130298244820453, -0.2781204657180023]),
control2=np.array([3.804763224169111, -0.5179424890517237]),
end=points['ScoreBackMidConeUp'],
- control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
+ control_alpha_rolls=[(.95, np.pi / 2.0)],
))
points['ScoreBackLowConeUp'] = to_theta_with_circular_index_and_roll(
@@ -279,7 +279,7 @@
control1=np.array([3.260123029490386, -0.5296803702636037]),
control2=np.array([3.1249665389044283, -0.7810758529482493]),
end=points['ScoreBackLowConeUp'],
- control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
+ control_alpha_rolls=[(.95, np.pi / 2.0)],
))
named_segments.append(
@@ -335,8 +335,8 @@
ThetaSplineSegment(
name="NeutralToHPPickupBackConeUp",
start=points['Neutral'],
- control1=np.array([2.1007555300246317, -1.0652453944299012]),
- control2=np.array([0.7579462988809116, -1.6518672114346526]),
+ control1=np.array([2.7014360412658567, -0.32490272351246796]),
+ control2=np.array([0.8282769211604871, -1.8026615176254461]),
end=points['HPPickupBackConeUp'],
control_alpha_rolls=[(.9, np.pi / 2.0)],
alpha_unitizer=np.matrix(
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index e5bc830..afca18b 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -166,6 +166,7 @@
],
target_compatible_with = ["//tools/platforms/hardware:roborio"],
deps = [
+ ":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
":superstructure_status_fbs",
diff --git a/y2023/control_loops/superstructure/led_indicator.cc b/y2023/control_loops/superstructure/led_indicator.cc
index afd0201..5cee266 100644
--- a/y2023/control_loops/superstructure/led_indicator.cc
+++ b/y2023/control_loops/superstructure/led_indicator.cc
@@ -1,6 +1,7 @@
#include "y2023/control_loops/superstructure/led_indicator.h"
namespace led = ctre::phoenix::led;
+namespace chrono = std::chrono;
namespace y2023::control_loops::superstructure {
@@ -13,6 +14,8 @@
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_position_fetcher_(
event_loop_->MakeFetcher<Position>("/superstructure")),
+ superstructure_goal_fetcher_(
+ event_loop_->MakeFetcher<Goal>("/superstructure")),
server_statistics_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/roborio/aos")),
@@ -35,7 +38,9 @@
candle_.ConfigAllSettings(config, 0);
event_loop_->AddPhasedLoop([this](int) { DecideColor(); },
- std::chrono::milliseconds(20));
+ chrono::milliseconds(20));
+ event_loop_->OnRun(
+ [this]() { startup_time_ = event_loop_->monotonic_now(); });
}
// This method will be called once per scheduler run
@@ -75,6 +80,7 @@
drivetrain_output_fetcher_.Fetch();
drivetrain_status_fetcher_.Fetch();
client_statistics_fetcher_.Fetch();
+ superstructure_goal_fetcher_.Fetch();
gyro_reading_fetcher_.Fetch();
localizer_output_fetcher_.Fetch();
@@ -96,15 +102,18 @@
// Not zeroed
if (superstructure_status_fetcher_.get() &&
!superstructure_status_fetcher_->zeroed()) {
- DisplayLed(255, 0, 255);
+ DisplayLed(255, 255, 0);
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 the imu gyro readings are not being sent/updated recently. Only do this
+ // after we've been on for a bit.
+ if (event_loop_->context().monotonic_event_time >
+ startup_time_ + chrono::seconds(5) &&
+ (!gyro_reading_fetcher_.get() ||
+ gyro_reading_fetcher_.context().monotonic_event_time +
+ frc971::controls::kLoopFrequency * 10 <
+ event_loop_->monotonic_now())) {
if (flash_counter_.Flash()) {
DisplayLed(255, 0, 0);
} else {
@@ -127,54 +136,44 @@
return;
}
- if (superstructure_status_fetcher_.get()) {
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_goal_fetcher_.get()) {
+ const bool cone = (superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_UP ||
+ superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_DOWN);
+ const bool intaking = (superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_CONE_UP ||
+ superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_CUBE ||
+ superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_LAST ||
+ superstructure_goal_fetcher_->roller_goal() ==
+ RollerGoal::INTAKE_CONE_DOWN);
// Check if end effector is intaking.
if (superstructure_status_fetcher_->end_effector_state() ==
- EndEffectorState::INTAKING) {
+ EndEffectorState::LOADED &&
+ intaking) {
if (flash_counter_.Flash()) {
- DisplayLed(255, 165, 0);
- } else {
- DisplayLed(0, 0, 0);
+ if (cone) {
+ DisplayLed(255, 165, 0);
+ } else {
+ DisplayLed(138, 43, 226);
+ }
+ return;
}
-
- 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_.get() != nullptr &&
- 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);
+ last_accepted_time_ + chrono::milliseconds(100)) {
+ if (drivetrain_status_fetcher_.get() != nullptr &&
+ drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
+ DisplayLed(0, 255, 0);
+ return;
+ } else {
+ DisplayLed(0, 0, 255);
+ }
return;
}
}
diff --git a/y2023/control_loops/superstructure/led_indicator.h b/y2023/control_loops/superstructure/led_indicator.h
index 256de30..dfdf4b1 100644
--- a/y2023/control_loops/superstructure/led_indicator.h
+++ b/y2023/control_loops/superstructure/led_indicator.h
@@ -12,6 +12,7 @@
#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_goal_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"
@@ -76,6 +77,7 @@
drivetrain_output_fetcher_;
aos::Fetcher<Status> superstructure_status_fetcher_;
aos::Fetcher<Position> superstructure_position_fetcher_;
+ aos::Fetcher<Goal> superstructure_goal_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics>
server_statistics_fetcher_;
aos::Fetcher<aos::message_bridge::ClientStatistics>
@@ -89,6 +91,9 @@
aos::monotonic_clock::time_point last_accepted_time_ =
aos::monotonic_clock::min_time;
+ aos::monotonic_clock::time_point startup_time_ =
+ aos::monotonic_clock::min_time;
+
FlashCounter flash_counter_{kFlashIterations};
};
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
index 7ac7000..2ee2d2b 100644
--- a/y2023/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -28,5 +28,4 @@
}
-
root_type Goal;
diff --git a/y2023/localizer/BUILD b/y2023/localizer/BUILD
index 7795462..43c4481 100644
--- a/y2023/localizer/BUILD
+++ b/y2023/localizer/BUILD
@@ -1,6 +1,19 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
load("//tools/build_rules:js.bzl", "ts_project")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
+ts_project(
+ name = "localizer_plotter",
+ srcs = ["localizer_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ "//frc971/wpilib:imu_plot_utils",
+ ],
+)
flatbuffer_cc_library(
name = "status_fbs",
diff --git a/y2023/localizer/corrections_plotter.ts b/y2023/localizer/corrections_plotter.ts
index a114f3f..8a27f55 100644
--- a/y2023/localizer/corrections_plotter.ts
+++ b/y2023/localizer/corrections_plotter.ts
@@ -52,19 +52,25 @@
const localizerOutput = aosPlotter.addMessageSource(
'/localizer', 'frc971.controls.LocalizerOutput');
+ const statsPlot = aosPlotter.addPlot(element);
+ statsPlot.plot.getAxisLabels().setTitle('Statistics');
+ statsPlot.plot.getAxisLabels().setXLabel(TIME);
+ statsPlot.plot.getAxisLabels().setYLabel('[bool, enum]');
+
+ statsPlot
+ .addMessageLine(localizerStatus, ['statistics[]', 'total_accepted'])
+ .setDrawLine(false)
+ .setColor(BLUE);
+ statsPlot
+ .addMessageLine(localizerStatus, ['statistics[]', 'total_candidates'])
+ .setDrawLine(false)
+ .setColor(RED);
+
const rejectionPlot = aosPlotter.addPlot(element);
rejectionPlot.plot.getAxisLabels().setTitle('Rejection Reasons');
rejectionPlot.plot.getAxisLabels().setXLabel(TIME);
rejectionPlot.plot.getAxisLabels().setYLabel('[bool, enum]');
- rejectionPlot
- .addMessageLine(localizerStatus, ['statistics[]', 'total_accepted'])
- .setDrawLine(false)
- .setColor(BLUE);
- rejectionPlot
- .addMessageLine(localizerStatus, ['statistics[]', 'total_candidates'])
- .setDrawLine(false)
- .setColor(RED);
for (let ii = 0; ii < targets.length; ++ii) {
rejectionPlot.addMessageLine(targets[ii], ['rejection_reason'])
.setDrawLine(false)
@@ -126,6 +132,21 @@
.setLabel('pi' + (ii + 1));
}
+ const thetaPlot = aosPlotter.addPlot(element);
+ thetaPlot.plot.getAxisLabels().setTitle('Yaw');
+ thetaPlot.plot.getAxisLabels().setXLabel(TIME);
+ thetaPlot.plot.getAxisLabels().setYLabel('[m]');
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ thetaPlot.addMessageLine(targets[ii], ['implied_robot_theta'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + (ii + 1));
+ }
+ thetaPlot.addMessageLine(localizerOutput, ['theta'])
+ .setDrawLine(false)
+ .setColor(BLUE);
+
const aprilTagPlot = aosPlotter.addPlot(element);
aprilTagPlot.plot.getAxisLabels().setTitle('April Tag IDs');
aprilTagPlot.plot.getAxisLabels().setXLabel(TIME);
@@ -137,4 +158,16 @@
.setColor(PI_COLORS[ii])
.setLabel('pi' + (ii + 1));
}
+
+ const imageAgePlot = aosPlotter.addPlot(element);
+ imageAgePlot.plot.getAxisLabels().setTitle('Image Age');
+ imageAgePlot.plot.getAxisLabels().setXLabel(TIME);
+ imageAgePlot.plot.getAxisLabels().setYLabel('[sec]');
+
+ for (let ii = 0; ii < targets.length; ++ii) {
+ imageAgePlot.addMessageLine(targets[ii], ['image_age_sec'])
+ .setDrawLine(false)
+ .setColor(PI_COLORS[ii])
+ .setLabel('pi' + (ii + 1));
+ }
}
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index efffca3..c663954 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -248,6 +248,29 @@
return builder->Finish();
}
+bool Localizer::UseAprilTag(uint64_t target_id) {
+ if (target_poses_.count(target_id) == 0) {
+ return false;
+ }
+
+ const flatbuffers::Vector<uint64_t> *ignore_tags = nullptr;
+
+ switch (utils_.Alliance()) {
+ case aos::Alliance::kRed:
+ ignore_tags =
+ CHECK_NOTNULL(constants_fetcher_.constants().ignore_targets()->red());
+ break;
+ case aos::Alliance::kBlue:
+ ignore_tags = CHECK_NOTNULL(
+ constants_fetcher_.constants().ignore_targets()->blue());
+ break;
+ case aos::Alliance::kInvalid:
+ return true;
+ }
+ return std::find(ignore_tags->begin(), ignore_tags->end(), target_id) ==
+ ignore_tags->end();
+}
+
flatbuffers::Offset<TargetEstimateDebug> Localizer::HandleTarget(
int camera_index, const aos::monotonic_clock::time_point capture_time,
const frc971::vision::TargetPoseFbs &target,
@@ -267,7 +290,7 @@
const uint64_t target_id = target.id();
builder.add_april_tag(target_id);
VLOG(2) << aos::FlatbufferToJson(&target);
- if (target_poses_.count(target_id) == 0) {
+ if (!UseAprilTag(target_id)) {
VLOG(1) << "Rejecting target due to invalid ID " << target_id;
return RejectImage(camera_index, RejectionReason::NO_SUCH_TARGET, &builder);
}
diff --git a/y2023/localizer/localizer.h b/y2023/localizer/localizer.h
index accb0c1..4b0715b 100644
--- a/y2023/localizer/localizer.h
+++ b/y2023/localizer/localizer.h
@@ -91,6 +91,8 @@
static flatbuffers::Offset<CumulativeStatistics> StatisticsForCamera(
const CameraState &camera, flatbuffers::FlatBufferBuilder *fbb);
+ bool UseAprilTag(uint64_t target_id);
+
aos::EventLoop *const event_loop_;
const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
diff --git a/y2023/localizer/localizer_plotter.ts b/y2023/localizer/localizer_plotter.ts
new file mode 100644
index 0000000..51524fb
--- /dev/null
+++ b/y2023/localizer/localizer_plotter.ts
@@ -0,0 +1,217 @@
+// Provides a plot for debugging drivetrain-related issues.
+import {AosPlotter} from '../../aos/network/www/aos_plotter';
+import {ImuMessageHandler} from '../../frc971/wpilib/imu_plot_utils';
+import * as proxy from '../../aos/network/www/proxy';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from '../../aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
+
+export function plotLocalizer(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+
+ const position = aosPlotter.addMessageSource("/drivetrain",
+ "frc971.control_loops.drivetrain.Position");
+ const status = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status');
+ const output = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Output');
+ const localizer = aosPlotter.addMessageSource(
+ '/localizer', 'y2023.localizer.Status');
+ const imu = aosPlotter.addRawMessageSource(
+ '/localizer', 'frc971.IMUValuesBatch',
+ new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
+
+ // Drivetrain Status estimated relative position
+ const positionPlot = aosPlotter.addPlot(element);
+ positionPlot.plot.getAxisLabels().setTitle("Estimated Relative Position " +
+ "of the Drivetrain");
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
+ positionPlot.plot.getAxisLabels().setYLabel("Relative Position (m)");
+ const leftPosition =
+ positionPlot.addMessageLine(status, ["estimated_left_position"]);
+ leftPosition.setColor(RED);
+ const rightPosition =
+ positionPlot.addMessageLine(status, ["estimated_right_position"]);
+ rightPosition.setColor(GREEN);
+ positionPlot
+ .addMessageLine(localizer, ['imu', 'left_encoder'])
+ .setColor(BROWN)
+ .setPointSize(0.0);
+ positionPlot
+ .addMessageLine(localizer, ['imu', 'right_encoder'])
+ .setColor(CYAN)
+ .setPointSize(0.0);
+ positionPlot.addMessageLine(position, ['left_encoder'])
+ .setColor(BROWN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(imu, ['left_encoder'])
+ .setColor(BROWN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(position, ['right_encoder'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(imu, ['right_encoder'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+
+
+ // Drivetrain Velocities
+ const velocityPlot = aosPlotter.addPlot(element);
+ velocityPlot.plot.getAxisLabels().setTitle('Velocity Plots');
+ velocityPlot.plot.getAxisLabels().setXLabel(TIME);
+ velocityPlot.plot.getAxisLabels().setYLabel('Wheel Velocity (m/s)');
+
+ const leftVelocity =
+ velocityPlot.addMessageLine(status, ['estimated_left_velocity']);
+ leftVelocity.setColor(RED);
+ const rightVelocity =
+ velocityPlot.addMessageLine(status, ['estimated_right_velocity']);
+ rightVelocity.setColor(GREEN);
+
+ const leftSpeed = velocityPlot.addMessageLine(position, ["left_speed"]);
+ leftSpeed.setColor(BLUE);
+ const rightSpeed = velocityPlot.addMessageLine(position, ["right_speed"]);
+ rightSpeed.setColor(BROWN);
+
+ const ekfLeftVelocity = velocityPlot.addMessageLine(
+ localizer, ['state', 'left_velocity']);
+ ekfLeftVelocity.setColor(RED);
+ ekfLeftVelocity.setPointSize(0.0);
+ const ekfRightVelocity = velocityPlot.addMessageLine(
+ localizer, ['state', 'right_velocity']);
+ ekfRightVelocity.setColor(GREEN);
+ ekfRightVelocity.setPointSize(0.0);
+
+ // Lateral velocity
+ const lateralPlot = aosPlotter.addPlot(element);
+ lateralPlot.plot.getAxisLabels().setTitle('Lateral Velocity');
+ lateralPlot.plot.getAxisLabels().setXLabel(TIME);
+ lateralPlot.plot.getAxisLabels().setYLabel('Velocity (m/s)');
+
+ lateralPlot.addMessageLine(localizer, ['state', 'lateral_velocity']).setColor(CYAN);
+
+ // Drivetrain Voltage
+ const voltagePlot = aosPlotter.addPlot(element);
+ voltagePlot.plot.getAxisLabels().setTitle('Voltage Plots');
+ voltagePlot.plot.getAxisLabels().setXLabel(TIME);
+ voltagePlot.plot.getAxisLabels().setYLabel('Voltage (V)')
+
+ voltagePlot.addMessageLine(localizer, ['state', 'left_voltage_error'])
+ .setColor(RED)
+ .setDrawLine(false);
+ voltagePlot.addMessageLine(localizer, ['state', 'right_voltage_error'])
+ .setColor(GREEN)
+ .setDrawLine(false);
+ voltagePlot.addMessageLine(output, ['left_voltage'])
+ .setColor(RED)
+ .setPointSize(0);
+ voltagePlot.addMessageLine(output, ['right_voltage'])
+ .setColor(GREEN)
+ .setPointSize(0);
+
+ // Heading
+ const yawPlot = aosPlotter.addPlot(element);
+ yawPlot.plot.getAxisLabels().setTitle('Robot Yaw');
+ yawPlot.plot.getAxisLabels().setXLabel(TIME);
+ yawPlot.plot.getAxisLabels().setYLabel('Yaw (rad)');
+
+ yawPlot.addMessageLine(status, ['localizer', 'theta']).setColor(GREEN);
+
+ yawPlot.addMessageLine(localizer, ['down_estimator', 'yaw']).setColor(BLUE);
+
+ yawPlot.addMessageLine(localizer, ['state', 'theta']).setColor(RED);
+
+ // Pitch/Roll
+ const orientationPlot = aosPlotter.addPlot(element);
+ orientationPlot.plot.getAxisLabels().setTitle('Orientation');
+ orientationPlot.plot.getAxisLabels().setXLabel(TIME);
+ orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
+
+ orientationPlot.addMessageLine(localizer, ['down_estimator', 'lateral_pitch'])
+ .setColor(RED)
+ .setLabel('roll');
+ orientationPlot
+ .addMessageLine(localizer, ['down_estimator', 'longitudinal_pitch'])
+ .setColor(GREEN)
+ .setLabel('pitch');
+
+ const stillPlot = aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 3]);
+ stillPlot.plot.getAxisLabels().setTitle('Still Plot');
+ stillPlot.plot.getAxisLabels().setXLabel(TIME);
+ stillPlot.plot.getAxisLabels().setYLabel('bool, g\'s');
+ stillPlot.plot.setDefaultYRange([-0.1, 1.1]);
+
+ stillPlot.addMessageLine(localizer, ['down_estimator', 'gravity_magnitude'])
+ .setColor(WHITE)
+ .setDrawLine(false);
+
+ // Absolute X Position
+ const xPositionPlot = aosPlotter.addPlot(element);
+ xPositionPlot.plot.getAxisLabels().setTitle('X Position');
+ xPositionPlot.plot.getAxisLabels().setXLabel(TIME);
+ xPositionPlot.plot.getAxisLabels().setYLabel('X Position (m)');
+
+ xPositionPlot.addMessageLine(status, ['x']).setColor(RED);
+ xPositionPlot.addMessageLine(localizer, ['down_estimator', 'position_x'])
+ .setColor(BLUE);
+ xPositionPlot.addMessageLine(localizer, ['state', 'x']).setColor(CYAN);
+
+ // Absolute Y Position
+ const yPositionPlot = aosPlotter.addPlot(element);
+ yPositionPlot.plot.getAxisLabels().setTitle('Y Position');
+ yPositionPlot.plot.getAxisLabels().setXLabel(TIME);
+ yPositionPlot.plot.getAxisLabels().setYLabel('Y Position (m)');
+
+ const localizerY = yPositionPlot.addMessageLine(status, ['y']);
+ localizerY.setColor(RED);
+ yPositionPlot.addMessageLine(localizer, ['down_estimator', 'position_y'])
+ .setColor(BLUE);
+ yPositionPlot.addMessageLine(localizer, ['state', 'y']).setColor(CYAN);
+
+ // Gyro
+ const gyroPlot = aosPlotter.addPlot(element);
+ gyroPlot.plot.getAxisLabels().setTitle('Gyro Readings');
+ gyroPlot.plot.getAxisLabels().setYLabel('Angular Velocity (rad / sec)');
+ gyroPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ const gyroX = gyroPlot.addMessageLine(imu, ['gyro_x']);
+ gyroX.setColor(RED);
+ const gyroY = gyroPlot.addMessageLine(imu, ['gyro_y']);
+ gyroY.setColor(GREEN);
+ const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
+ gyroZ.setColor(BLUE);
+
+
+ const timingPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ timingPlot.plot.getAxisLabels().setTitle('Fault Counting');
+ timingPlot.plot.getAxisLabels().setXLabel(TIME);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu', 'imu_failures', 'imu_to_pico_checksum_mismatch'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu', 'imu_failures', 'pico_to_pi_checksum_mismatch'])
+ .setColor(RED)
+ .setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu', 'imu_failures', 'other_zeroing_faults'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+
+ timingPlot
+ .addMessageLine(
+ localizer, ['imu', 'imu_failures', 'missed_messages'])
+ .setColor(PINK)
+ .setDrawLine(false);
+}